Skip to content

Commit 24e8e06

Browse files
MoscowskyAntonMoscowskyAnton
committed
added detection of complex stuff
1 parent 60e6023 commit 24e8e06

File tree

4 files changed

+145
-46
lines changed

4 files changed

+145
-46
lines changed

‎launch/extended_object_detection_oakd_example.launch‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,8 @@
2626
<remap from="depth/image_raw" to="/oak/stereo/depth"/>
2727
<remap from="depth/info" to="/oak/stereo/camera_info"/>
2828

29-
<rosparam param="selected_on_start_simple_objects">[67]</rosparam>
30-
<rosparam param="selected_on_start_complex_objects">[-1]</rosparam>
29+
<rosparam param="selected_on_start_simple_objects">[-1]</rosparam>
30+
<rosparam param="selected_on_start_complex_objects">[10]</rosparam>
3131

3232
</node>
3333

‎src/extended_object_detection_node/eod_node.cpp‎

Lines changed: 131 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ geometry_msgs::Vector3 fromCvVector(cv::Vec3d cv_vector){
1616
return ros_vector;
1717
}
1818

19+
1920
geometry_msgs::Point fromVector(const geometry_msgs::Vector3& vector){
2021
geometry_msgs::Point point;
2122
point.x = vector.x;
@@ -24,6 +25,7 @@ geometry_msgs::Point fromVector(const geometry_msgs::Vector3& vector){
2425
return point;
2526
}
2627

28+
2729
EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
2830
nh_ = nh;
2931
nh_p_ = nh_p;
@@ -54,13 +56,11 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
5456

5557
// // object selection
5658
std::vector<int>selected_on_start_simple_objects;
57-
nh_p_.getParam("selected_on_start_simple_objects",selected_on_start_simple_objects);
58-
59+
nh_p_.getParam("selected_on_start_simple_objects",selected_on_start_simple_objects);
5960
if( selected_on_start_simple_objects.size() == 0 ){
60-
ROS_INFO("All objects selected.");
61-
for( size_t i = 0 ; i < object_base->simple_objects.size() ; i++ ){
62-
selected_simple_objects.push_back(object_base->simple_objects[i]);
63-
}
61+
//ROS_INFO("All objects selected.");
62+
for( size_t i = 0 ; i < object_base->simple_objects.size() ; i++ )
63+
selected_simple_objects.push_back(object_base->simple_objects[i]);
6464
}
6565
else{
6666
if(selected_on_start_simple_objects[0] != -1){
@@ -70,10 +70,30 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
7070
}
7171
}
7272
}
73-
ROS_INFO("Selected to detect on start %li objects", selected_simple_objects.size());
73+
ROS_INFO("Selected to detect on start %li simple objects", selected_simple_objects.size());
74+
#ifdef USE_IGRAPH
75+
std::vector<int>selected_on_start_complex_objects;
76+
nh_p_.getParam("selected_on_start_complex_objects",selected_on_start_complex_objects);
77+
if( selected_on_start_complex_objects.size() == 0 ){
78+
//ROS_INFO("All objects selected.");
79+
for( size_t i = 0 ; i < object_base->complex_objects_graph.size() ; i++ )
80+
selected_complex_objects.push_back(object_base->complex_objects_graph[i]);
81+
}
82+
else{
83+
if(selected_on_start_complex_objects[0] != -1){
84+
for( size_t i = 0 ; i < object_base->complex_objects_graph.size() ; i++ ){
85+
if( find(selected_on_start_complex_objects.begin(), selected_on_start_complex_objects.end(), object_base->complex_objects_graph[i]->ID) != selected_on_start_complex_objects.end() )
86+
selected_complex_objects.push_back(object_base->complex_objects_graph[i]);
87+
}
88+
}
89+
}
90+
ROS_INFO("Selected to detect on start %li complex objects", selected_complex_objects.size());
91+
#endif
7492

7593
set_simple_objects_srv_ = nh_p_.advertiseService("set_simple_objects", &EOD_ROS::set_simple_objects_cb, this);
76-
94+
#ifdef USE_IGRAPH
95+
set_complex_objects_srv_ = nh_p_.advertiseService("set_complex_objects", &EOD_ROS::set_complex_objects_cb, this);
96+
#endif
7797
// set up publishers
7898
simple_objects_pub_ = nh_p_.advertise<extended_object_detection::SimpleObjectArray>("simple_objects",1);
7999
if( publish_markers)
@@ -84,29 +104,29 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
84104

85105
// set up message filters
86106
if( !subscribe_depth){
87-
ROS_INFO("Configuring filter on rgb image and info...");
88-
107+
//ROS_INFO("Configuring filter on rgb image and info...");
89108
rgb_sync_.reset( new RGBSynchronizer(RGBInfoSyncPolicy(10), sub_rgb_, sub_info_) );
90109
rgb_sync_->registerCallback(boost::bind(&EOD_ROS::rgb_info_cb, this, boost::placeholders::_1, boost::placeholders::_2));
91110
}
92111
else{
93-
ROS_INFO("Configuring filter on rgbd images and infos...");
94-
112+
//ROS_INFO("Configuring filter on rgbd images and infos...");
95113
sub_depth_.subscribe(*rgb_it_, "depth/image_raw", 10);
96114
sub_depth_info_.subscribe(nh_, "depth/info", 10);
97115

98116
rgbd_sync_.reset( new RGBDSynchronizer(RGBDInfoSyncPolicy(10), sub_rgb_, sub_info_, sub_depth_, sub_depth_info_) );
99117
rgbd_sync_->registerCallback(boost::bind(&EOD_ROS::rgbd_info_cb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
100118
}
101-
ROS_INFO("Configured!");
119+
//ROS_INFO("Configured!");
102120
}
103121

122+
104123
bool EOD_ROS::check_time(ros::Time stamp){
105124
if( frame_sequence == 0)
106125
return true;
107126
return (stamp - prev_detected_time).toSec() > rate_limit_sec;
108127
}
109128

129+
110130
cv::Mat EOD_ROS::getK(const sensor_msgs::CameraInfoConstPtr& info_msg){
111131
cv::Mat K = cv::Mat::zeros(3, 3, CV_64F);
112132
for (size_t i=0; i<3; i++) {
@@ -117,6 +137,7 @@ cv::Mat EOD_ROS::getK(const sensor_msgs::CameraInfoConstPtr& info_msg){
117137
return K;
118138
}
119139

140+
120141
cv::Mat EOD_ROS::getD(const sensor_msgs::CameraInfoConstPtr& info_msg){
121142
cv::Mat D = cv::Mat::zeros(1, 5, CV_64F);
122143
for (size_t i=0; i<5; i++) {
@@ -125,28 +146,28 @@ cv::Mat EOD_ROS::getD(const sensor_msgs::CameraInfoConstPtr& info_msg){
125146
return D;
126147
}
127148

149+
128150
void EOD_ROS::rgb_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info){
129-
//ROS_INFO("Got Image!");
130-
// CHECK RATE
151+
//ROS_INFO("Got Image!");
152+
// CHECK RATE
131153
if( !check_time(ros::Time::now()) ) {
132154
ROS_WARN("Skipped frame");
133155
return;
134156
}
135157
// TODO add possibility to exclude old stamp images (if detection goes to slow)
136-
137158
cv::Mat rgb;
138159
try{
139-
rgb = cv_bridge::toCvCopy(rgb_image, "bgr8")->image;
160+
rgb = cv_bridge::toCvCopy(rgb_image, "bgr8")->image;
140161
}
141162
catch (cv_bridge::Exception& e){
142163
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", rgb_image->encoding.c_str());
143164
return;
144-
}
145-
146-
eod::InfoImage ii = eod::InfoImage(rgb, getK(rgb_info), getD(rgb_info) );
147-
detect(ii, eod::InfoImage(), rgb_image->header);
165+
}
166+
eod::InfoImage ii = eod::InfoImage(rgb, getK(rgb_info), getD(rgb_info) );
167+
detect(ii, eod::InfoImage(), rgb_image->header);
148168
}
149169

170+
150171
void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info, const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::CameraInfoConstPtr& depth_info){
151172
//ROS_INFO("Got RGBD!");
152173
// CHECK RATE
@@ -188,24 +209,34 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
188209
ROS_INFO("Detecting...");
189210
prev_detected_time = ros::Time::now();
190211

191-
cv::Mat image_to_draw;
192-
193-
extended_object_detection::SimpleObjectArray simples_msg;
194-
195-
212+
cv::Mat image_to_draw;
196213
if(publish_image_output)
197214
image_to_draw = rgb.clone();
198215

199216
// detect simple objects
200-
for (auto& s_it : selected_simple_objects){
201-
//ROS_INFO("Identifiyng...");
202-
s_it->Identify(rgb, depth, frame_sequence);
203-
//ROS_INFO("Adding...");
204-
add_data_to_simple_msg(&(*s_it), simples_msg, rgb.K());
217+
extended_object_detection::SimpleObjectArray simples_msg;
218+
for (auto& s_it : selected_simple_objects){
219+
s_it->Identify(rgb, depth, frame_sequence);
220+
for(auto& eoi : s_it->objects)
221+
simples_msg.objects.push_back(eoi_to_base_object(s_it->name, s_it->ID, &eoi, rgb.K()));
205222
if(publish_image_output)
206223
s_it->draw(image_to_draw, cv::Scalar(0, 255, 0));
207224
}
208-
225+
// detect complex objects
226+
#ifdef USE_IGRAPH
227+
extended_object_detection::ComplexObjectArray complex_msg;
228+
for(auto& c_it : selected_complex_objects){
229+
//ROS_INFO("Identifying complex...");
230+
c_it->Identify(rgb, depth, frame_sequence);
231+
for(auto& eoi : c_it->complex_objects){
232+
extended_object_detection::ComplexObject cmplx_msg;
233+
cmplx_msg.complex_object = eoi_to_base_object(c_it->name, c_it->ID, &eoi, rgb.K());
234+
complex_msg.objects.push_back(cmplx_msg);
235+
}
236+
if(publish_image_output)
237+
c_it->drawAll(image_to_draw, cv::Scalar(255, 255, 0), 2);
238+
}
239+
#endif
209240
simples_msg.header = header;
210241
if( use_actual_time )
211242
simples_msg.header.stamp = ros::Time::now();
@@ -231,18 +262,12 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
231262
}
232263

233264

234-
void EOD_ROS::add_data_to_simple_msg( eod::SimpleObject* so, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K){
235-
for(auto& eoi : so->objects){
236-
msg.objects.push_back(eoi_to_base_object(so, &eoi, K));
237-
}
238-
}
239-
240-
extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K){
265+
extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( std::string name, int id, eod::ExtendedObjectInfo* eoi, const cv::Mat& K){
241266
//ROS_INFO("Forming...");
242267
extended_object_detection::BaseObject bo;
243268
// common
244-
bo.type_id = so->ID;
245-
bo.type_name = so->name;
269+
bo.type_id = id;
270+
bo.type_name = name;
246271
bo.score = eoi->total_score;
247272
// extracted info
248273
for( auto& exi : eoi->extracted_info){
@@ -288,6 +313,7 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
288313
return bo;
289314
}
290315

316+
291317
visualization_msgs::Marker EOD_ROS::base_object_to_marker_arrow(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id){
292318
visualization_msgs::Marker mrk;
293319
mrk.header = header;
@@ -308,6 +334,7 @@ visualization_msgs::Marker EOD_ROS::base_object_to_marker_arrow(extended_object_
308334
return mrk;
309335
}
310336

337+
311338
visualization_msgs::Marker EOD_ROS::base_object_to_marker_frame(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id){
312339
visualization_msgs::Marker mrk;
313340
mrk.header = header;
@@ -328,6 +355,7 @@ visualization_msgs::Marker EOD_ROS::base_object_to_marker_frame(extended_object_
328355
return mrk;
329356
}
330357

358+
331359
int EOD_ROS::find_simple_obj_index_by_id(int id){
332360
for( int i = 0 ; i < selected_simple_objects.size() ; i++ ){
333361
if( selected_simple_objects[i]->ID == id )
@@ -336,6 +364,7 @@ int EOD_ROS::find_simple_obj_index_by_id(int id){
336364
return -1;
337365
}
338366

367+
339368
bool EOD_ROS::set_simple_objects_cb(extended_object_detection::SetObjects::Request &req, extended_object_detection::SetObjects::Response &res){
340369

341370
if( req.remove_all && req.add_all ){/*do literally nothing*/}
@@ -385,6 +414,67 @@ bool EOD_ROS::set_simple_objects_cb(extended_object_detection::SetObjects::Reque
385414
return true;
386415
}
387416

417+
418+
#ifdef USE_IGRAPH
419+
int EOD_ROS::find_complex_obj_index_by_id(int id){
420+
for( int i = 0 ; i < selected_complex_objects.size() ; i++ ){
421+
if( selected_complex_objects[i]->ID == id )
422+
return i;
423+
}
424+
return -1;
425+
}
426+
427+
428+
bool EOD_ROS::set_complex_objects_cb(extended_object_detection::SetObjects::Request &req, extended_object_detection::SetObjects::Response &res){
429+
430+
if( req.remove_all && req.add_all ){/*do literally nothing*/}
431+
else{
432+
if( req.remove_all ){
433+
selected_complex_objects.clear();
434+
}
435+
else if( req.add_all ){
436+
selected_complex_objects.clear();
437+
selected_complex_objects.assign(object_base->complex_objects_graph.begin(), object_base->complex_objects_graph.end());
438+
}
439+
else{
440+
for( auto& change : req.changes ){
441+
int object_id = abs(change);
442+
eod::ComplexObjectGraph* co = object_base->getComplexObjectGraphByID(object_id);
443+
if(co){
444+
// add
445+
if( change > 0 ){
446+
if( find_complex_obj_index_by_id(object_id) != -1 ){
447+
// already selected
448+
ROS_WARN("[set_simple_objects srv] object with id %i is already selected", object_id);
449+
}
450+
else{
451+
selected_complex_objects.push_back(co);
452+
}
453+
}
454+
// remove
455+
else{
456+
int no_in_list = find_complex_obj_index_by_id(object_id);
457+
if( no_in_list != -1 ){
458+
selected_complex_objects.erase(selected_complex_objects.begin()+no_in_list);
459+
}
460+
else{
461+
// already removed
462+
ROS_WARN("[set_simple_objects srv] object with id %i is already removed", object_id);
463+
}
464+
}
465+
}
466+
else{
467+
ROS_ERROR("[set_simple_objects srv] no object in base with id %i",object_id);
468+
}
469+
}
470+
}
471+
}
472+
for(auto& co : selected_complex_objects )
473+
res.result.push_back(co->ID);
474+
return true;
475+
}
476+
#endif
477+
388478
int main(int argc, char **argv)
389479
{
390480
ros::init(argc, argv, "extended_object_detection_node");

‎src/extended_object_detection_node/eod_node.h‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,9 @@ class EOD_ROS{
5656
image_transport::Publisher output_image_pub_;
5757

5858
ros::ServiceServer set_simple_objects_srv_;
59+
#ifdef USE_IGRAPH
60+
ros::ServiceServer set_complex_objects_srv_;
61+
#endif
5962

6063
// params
6164
bool subscribe_depth;
@@ -72,22 +75,28 @@ class EOD_ROS{
7275
void rgb_info_cb(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info);
7376

7477
bool set_simple_objects_cb(extended_object_detection::SetObjects::Request &req, extended_object_detection::SetObjects::Response &res);
78+
#ifdef USE_IGRAPH
79+
bool set_complex_objects_cb(extended_object_detection::SetObjects::Request &req, extended_object_detection::SetObjects::Response &res);
80+
#endif
7581

7682
void rgbd_info_cb(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info, const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::CameraInfoConstPtr& depth_info);
7783

7884
// functions
7985
//void detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header);
8086
void detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header);
8187
bool check_time(ros::Time stamp);
82-
void add_data_to_simple_msg( eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K);
83-
extended_object_detection::BaseObject eoi_to_base_object(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
88+
//void add_data_to_simple_msg( eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K);
89+
extended_object_detection::BaseObject eoi_to_base_object(std::string name, int id, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
8490
cv::Mat getK(const sensor_msgs::CameraInfoConstPtr& info_msg);
8591
cv::Mat getD(const sensor_msgs::CameraInfoConstPtr& info_msg);
8692

8793
visualization_msgs::Marker base_object_to_marker_arrow(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
8894
visualization_msgs::Marker base_object_to_marker_frame(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
8995

9096
int find_simple_obj_index_by_id(int id);
97+
#ifdef USE_IGRAPH
98+
int find_complex_obj_index_by_id(int id);
99+
#endif
91100

92101
// EOD
93102
eod::ObjectBase * object_base;

‎src/lib‎

0 commit comments

Comments
 (0)