Skip to content

Commit 929d1e4

Browse files
MoscowskyAntonMoscowskyAnton
committed
added more vis and out
1 parent 8d678b4 commit 929d1e4

File tree

2 files changed

+72
-38
lines changed

2 files changed

+72
-38
lines changed

‎src/extended_object_detection_node/eod_node.cpp‎

Lines changed: 69 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,10 @@
22
#include <cstdint>
33
#include <sensor_msgs/image_encodings.h>
44
#include <geometry_msgs/Vector3.h>
5-
//#include <geometry_msgs/Point.h>
5+
#include <geometry_msgs/Point.h>
66
#include <math.h>
77
#include <visualization_msgs/MarkerArray.h>
8+
#include "geometry_utils.h"
89

910
geometry_msgs::Vector3 getUnitTranslation(cv::Point point, const cv::Mat& K){
1011
geometry_msgs::Vector3 unit_translate;
@@ -14,6 +15,14 @@ geometry_msgs::Vector3 getUnitTranslation(cv::Point point, const cv::Mat& K){
1415
return unit_translate;
1516
}
1617

18+
geometry_msgs::Vector3 multiplyVectorScalar(geometry_msgs::Vector3 vector, double scalar){
19+
geometry_msgs::Vector3 new_vector;
20+
new_vector.x = vector.x * scalar;
21+
new_vector.y = vector.y * scalar;
22+
new_vector.z = vector.z * scalar;
23+
return new_vector;
24+
}
25+
1726
EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
1827
nh_ = nh;
1928
nh_p_ = nh_p;
@@ -29,7 +38,7 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
2938
// get params
3039
nh_p_.param("subscribe_depth", subscribe_depth, false);
3140
nh_p_.param("rate_limit_sec", rate_limit_sec, 0.1);
32-
nh_p_.param("publish_output", publish_output, false);
41+
nh_p_.param("publish_image_output", publish_image_output, false);
3342
nh_p_.param("use_actual_time", use_actual_time, false);
3443
nh_p_.param("publish_markers", publish_markers, false);
3544

@@ -177,7 +186,7 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se
177186
detect(ii_rgb, ii_depth, rgb_image->header);
178187
}
179188

180-
//void EOD_ROS::detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header){
189+
181190
void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header){
182191
ROS_INFO("Detecting...");
183192
prev_detected_time = ros::Time::now();
@@ -187,7 +196,7 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
187196
extended_object_detection::SimpleObjectArray simples_msg;
188197

189198

190-
if(publish_output)
199+
if(publish_image_output)
191200
image_to_draw = rgb.clone();
192201

193202
// detect simple objects
@@ -196,7 +205,7 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
196205
s_it->Identify(rgb, depth, frame_sequence);
197206
//ROS_INFO("Adding...");
198207
add_data_to_simple_msg(&(*s_it), simples_msg, rgb.K);
199-
if(publish_output)
208+
if(publish_image_output)
200209
s_it->draw(image_to_draw, cv::Scalar(0, 255, 0));
201210
}
202211

@@ -207,17 +216,22 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
207216
simple_objects_pub_.publish(simples_msg);
208217

209218
if(publish_markers){
210-
publish_simple_as_markers(rgb.K, header);
211-
}
212-
if(publish_output){
219+
visualization_msgs::MarkerArray mrk_array_msg;
220+
for(auto& bo : simples_msg.objects){
221+
int id_cnt = 0;
222+
mrk_array_msg.markers.push_back(base_object_to_marker_arrow(bo, rgb.K, header, cv::Scalar(0, 255, 0),id_cnt++));
223+
}
224+
simple_objects_markers_pub_.publish(mrk_array_msg);
225+
}
226+
227+
if(publish_image_output){
213228
sensor_msgs::ImagePtr detected_image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_to_draw).toImageMsg();
214229
output_image_pub_.publish(detected_image_msg);
215-
}
216-
217-
230+
}
218231
frame_sequence++;
219232
}
220233

234+
221235
void EOD_ROS::add_data_to_simple_msg( eod::SimpleObject* so, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K){
222236
for(auto& eoi : so->objects){
223237
msg.objects.push_back(eoi_to_base_object(so, &eoi, K));
@@ -235,15 +249,25 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
235249
for( auto& exi : eoi->extracted_info){
236250
bo.extracted_info.keys.push_back(exi.first);
237251
bo.extracted_info.values.push_back(exi.second);
238-
}
239-
252+
}
240253
// rect
241254
bo.rect.left_bottom.x = eoi->x;
242255
bo.rect.left_bottom.y = eoi->y;
243256
bo.rect.right_up.x = eoi->x + eoi->width;
244257
bo.rect.right_up.y = eoi->y + eoi->height;
245258

246-
// transform
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+
270+
// translation
247271
if( eoi->tvec.size() > 0 ){
248272
bo.transform.translation.x = eoi->tvec[0][0];
249273
bo.transform.translation.y = eoi->tvec[0][1];
@@ -252,33 +276,44 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
252276
else{
253277
bo.transform.translation = getUnitTranslation(eoi->getCenter(), K);
254278
}
279+
// rotation
280+
if (eoi->rvec.size() > 0 ){
281+
double quaternion[4];
282+
cv::Mat rotMat;
283+
cv::Rodrigues(eoi->rvec, rotMat);
284+
eod::getQuaternion( rotMat, quaternion);
285+
bo.transform.rotation.x = quaternion[0];
286+
bo.transform.rotation.y = quaternion[1];
287+
bo.transform.rotation.z = quaternion[2];
288+
bo.transform.rotation.w = quaternion[3];
289+
}
290+
else // use zero quaternion
291+
bo.transform.rotation.w = 1;
255292

256293
//TODO tracks
257294
return bo;
258295
}
259296

260-
void EOD_ROS::publish_simple_as_markers(const cv::Mat& K, std_msgs::Header header){
261-
visualization_msgs::MarkerArray mrk_array_msg();
262-
//mrk_array_msg.header = header;
263-
for(auto& so : selected_simple_objects){
264-
int id_cnt = 0;
265-
for(auto& eoi : so->objects){
266-
mrk_array_msg.markers.append(eoi_to_marker(so, &eoi, K, header, cv::Scalar(0, 255, 0)),id_cnt++);
267-
268-
269-
}
270-
}
271-
simple_objects_markers_pub_.publish(mrk_array_msg);
272-
}
273-
274-
visualization_msgs::Marker EOD_ROS::eoi_to_marker(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id){
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){
275298
visualization_msgs::Marker mrk;
276-
mrk.header = header;
277-
278-
mrk.ns = so->name;
299+
mrk.header = header;
300+
mrk.ns = base_object.type_name +"_arrow";
279301
mrk.id = id;
280-
281-
302+
mrk.type = visualization_msgs::Marker::ARROW;
303+
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;
308+
mrk.points.push_back(end);
309+
mrk.scale.x = 0.01;
310+
mrk.scale.y = 0.02;
311+
mrk.scale.z = 0.1;
312+
mrk.pose.orientation.w = 1;
313+
mrk.color.r = color[0]/255;
314+
mrk.color.g = color[1]/255;
315+
mrk.color.b = color[2]/255;
316+
mrk.color.a = 1;
282317
return mrk;
283318
}
284319

‎src/extended_object_detection_node/eod_node.h‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ class EOD_ROS{
6060
// params
6161
bool subscribe_depth;
6262
double rate_limit_sec;
63-
bool publish_output;
63+
bool publish_image_output;
6464
bool use_actual_time;
6565
bool publish_markers;
6666

@@ -82,10 +82,9 @@ class EOD_ROS{
8282
void add_data_to_simple_msg( eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K);
8383
extended_object_detection::BaseObject eoi_to_base_object(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
8484
cv::Mat getK(const sensor_msgs::CameraInfoConstPtr& info_msg);
85-
cv::Mat getD(const sensor_msgs::CameraInfoConstPtr& info_msg);
86-
void publish_simple_as_markers(const cv::Mat& K, std_msgs::Header header);
85+
cv::Mat getD(const sensor_msgs::CameraInfoConstPtr& info_msg);
8786

88-
visualization_msgs::Marker eoi_to_marker(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
87+
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);
8988

9089

9190
int find_simple_obj_index_by_id(int id);

0 commit comments

Comments
 (0)