@@ -23,6 +23,14 @@ geometry_msgs::Vector3 multiplyVectorScalar(geometry_msgs::Vector3 vector, doubl
2323 return new_vector;
2424}
2525
26+ geometry_msgs::Point fromVector (const geometry_msgs::Vector3& vector){
27+ geometry_msgs::Point point;
28+ point.x = vector.x ;
29+ point.y = vector.y ;
30+ point.z = vector.z ;
31+ return point;
32+ }
33+
2634EOD_ROS::EOD_ROS (ros::NodeHandle nh, ros::NodeHandle nh_p){
2735 nh_ = nh;
2836 nh_p_ = nh_p;
@@ -181,7 +189,7 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se
181189 }
182190
183191 eod::InfoImage ii_rgb = eod::InfoImage (rgb, getK (rgb_info), getD (rgb_info) );
184- eod::InfoImage ii_depth = eod::InfoImage (depth, getK (depth_info), cv::Mat ( ) );
192+ eod::InfoImage ii_depth = eod::InfoImage (depth, getK (depth_info), getD (depth_info ) );
185193
186194 detect (ii_rgb, ii_depth, rgb_image->header );
187195}
@@ -219,7 +227,9 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
219227 visualization_msgs::MarkerArray mrk_array_msg;
220228 int id_cnt = 0 ;
221229 for (auto & bo : simples_msg.objects ){
222- mrk_array_msg.markers .push_back (base_object_to_marker_arrow (bo, rgb.K (), header, cv::Scalar (0 , 255 , 0 ),id_cnt++));
230+ mrk_array_msg.markers .push_back (base_object_to_marker_arrow (bo, rgb.K (), header, cv::Scalar (0 , 255 , 0 ),id_cnt));
231+ mrk_array_msg.markers .push_back (base_object_to_marker_frame (bo, rgb.K (), header, cv::Scalar (0 , 255 , 0 ),id_cnt));
232+ id_cnt++;
223233 }
224234 simple_objects_markers_pub_.publish (mrk_array_msg);
225235 }
@@ -245,28 +255,11 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
245255 bo.type_id = so->ID ;
246256 bo.type_name = so->name ;
247257 bo.score = eoi->total_score ;
248-
258+ // extracted info
249259 for ( auto & exi : eoi->extracted_info ){
250260 bo.extracted_info .keys .push_back (exi.first );
251261 bo.extracted_info .values .push_back (exi.second );
252- }
253- // rect
254- bo.rect .left_bottom .x = eoi->x ;
255- bo.rect .left_bottom .y = eoi->y ;
256- bo.rect .right_up .x = eoi->x + eoi->width ;
257- bo.rect .right_up .y = eoi->y + eoi->height ;
258-
259- // translation to rect's corners
260- geometry_msgs::Vector3 temp_translation;
261- temp_translation = getUnitTranslation (cv::Point (eoi->x , eoi->y ), K);
262- bo.rect .cornerTranslates .push_back (temp_translation);
263- temp_translation = getUnitTranslation (cv::Point (eoi->x , eoi->y + eoi->width ), K);
264- bo.rect .cornerTranslates .push_back (temp_translation);
265- temp_translation = getUnitTranslation (cv::Point (eoi->x + eoi->height , eoi->y + eoi->width ), K);
266- bo.rect .cornerTranslates .push_back (temp_translation);
267- temp_translation = getUnitTranslation (cv::Point (eoi->x + eoi->height , eoi->y ), K);
268- bo.rect .cornerTranslates .push_back (temp_translation);
269-
262+ }
270263 // translation
271264 if ( eoi->tvec .size () > 0 ){
272265 bo.transform .translation .x = eoi->tvec [0 ][0 ];
@@ -286,25 +279,40 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
286279 bo.transform .rotation .y = quaternion[1 ];
287280 bo.transform .rotation .z = quaternion[2 ];
288281 bo.transform .rotation .w = quaternion[3 ];
289- }
282+ }
290283 else // use zero quaternion
291284 bo.transform .rotation .w = 1 ;
285+
286+ // rect
287+ bo.rect .left_bottom .x = eoi->x ;
288+ bo.rect .left_bottom .y = eoi->y ;
289+ bo.rect .right_up .x = eoi->x + eoi->width ;
290+ bo.rect .right_up .y = eoi->y + eoi->height ;
291+
292+ // translation to rect's corners
293+ geometry_msgs::Vector3 temp_translation;
294+ temp_translation = multiplyVectorScalar (getUnitTranslation (cv::Point (eoi->x , eoi->y ), K),bo.transform .translation .z );
295+ bo.rect .cornerTranslates .push_back (temp_translation);
296+ temp_translation = multiplyVectorScalar (getUnitTranslation (cv::Point (eoi->x , eoi->y + eoi->height ), K),bo.transform .translation .z );
297+ bo.rect .cornerTranslates .push_back (temp_translation);
298+ temp_translation = multiplyVectorScalar (getUnitTranslation (cv::Point (eoi->x + eoi->width , eoi->y + eoi->height ), K),bo.transform .translation .z );
299+ bo.rect .cornerTranslates .push_back (temp_translation);
300+ temp_translation = multiplyVectorScalar (getUnitTranslation (cv::Point (eoi->x + eoi->width , eoi->y ), K),bo.transform .translation .z );
301+ bo.rect .cornerTranslates .push_back (temp_translation);
302+
292303
293304 // TODO tracks
294305 return bo;
295306}
296307
297- 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){
308+ 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){
298309 visualization_msgs::Marker mrk;
299310 mrk.header = header;
300311 mrk.ns = base_object.type_name +" _arrow"
301312 mrk.id = id;
302313 mrk.type = visualization_msgs::Marker::ARROW;
303314 mrk.points .push_back (geometry_msgs::Point ());
304- geometry_msgs::Point end;
305- end.x = base_object.transform .translation .x ;
306- end.y = base_object.transform .translation .y ;
307- end.z = base_object.transform .translation .z ;
315+ geometry_msgs::Point end = fromVector (base_object.transform .translation );
308316 mrk.points .push_back (end);
309317 mrk.scale .x = 0.01 ;
310318 mrk.scale .y = 0.02 ;
@@ -317,6 +325,26 @@ visualization_msgs::Marker EOD_ROS::base_object_to_marker_arrow(extended_object_
317325 return mrk;
318326}
319327
328+ 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){
329+ visualization_msgs::Marker mrk;
330+ mrk.header = header;
331+ mrk.ns = base_object.type_name +" _frame"
332+ mrk.id = id;
333+ mrk.type = visualization_msgs::Marker::LINE_STRIP;
334+ for ( auto & corner : base_object.rect .cornerTranslates )
335+ mrk.points .push_back (fromVector (corner));
336+ mrk.points .push_back (mrk.points [0 ]);
337+ mrk.scale .x = 0.02 ;
338+ mrk.scale .y = 0.02 ;
339+ mrk.scale .z = 0.1 ;
340+ mrk.pose .orientation .w = 1 ;
341+ mrk.color .r = color[0 ]/255 ;
342+ mrk.color .g = color[1 ]/255 ;
343+ mrk.color .b = color[2 ]/255 ;
344+ mrk.color .a = 1 ;
345+ return mrk;
346+ }
347+
320348int EOD_ROS::find_simple_obj_index_by_id (int id){
321349 for ( int i = 0 ; i < selected_simple_objects.size () ; i++ ){
322350 if ( selected_simple_objects[i]->ID == id )
0 commit comments