ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

bounding box PTR input

asked 2018-08-05 08:09:28 -0600

th6262 gravatar image

updated 2022-04-10 15:05:50 -0600

jayess gravatar image

Hello everyone, I'm working on object tracking and I'm trying to apply euclidian cluster extraction and then fit bounding boxes around the clusters, before tracking them.

The problem I'm facing is, that my base-code is based on nodelets etc. and not as straightforward as in the tutorials provided by PCL.

Right now I'm struggling to get the bounding boxes around the clusters. For the cluster extraction I used the samples from the perception_pcl on github ( I didn't alter the code) Github link to Code I'm using

And it's working with my input PointCloud. For the bounding boxes I found this function:

visualization_msgs::Marker mark_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster, std::string ns ,int id, float r, float g, float b)
{
  Eigen::Vector4f centroid;
  Eigen::Vector4f min;
  Eigen::Vector4f max;

  pcl::compute3DCentroid (*cloud_cluster, centroid);
  pcl::getMinMax3D (*cloud_cluster, min, max);

  uint32_t shape = visualization_msgs::Marker::CUBE;
  visualization_msgs::Marker marker;
  marker.header.frame_id = cloud_cluster->header.frame_id;
  marker.header.stamp = ros::Time::now();

  marker.ns = ns;
  marker.id = id;
  marker.type = shape;
  marker.action = visualization_msgs::Marker::ADD;

  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;

  marker.scale.x = (max[0]-min[0]);
  marker.scale.y = (max[1]-min[1]);
  marker.scale.z = (max[2]-min[2]);

  if (marker.scale.x ==0)
      marker.scale.x=0.1;

  if (marker.scale.y ==0)
    marker.scale.y=0.1;

  if (marker.scale.z ==0)
    marker.scale.z=0.1;

  marker.color.r = r;
  marker.color.g = g;
  marker.color.b = b;
  marker.color.a = 0.5;

  marker.lifetime = ros::Duration();
//   marker.lifetime = ros::Duration(0.5);
  return marker;
}

Now my problem is, I don't know what Input I need as Ptr for this function. If I use "output", which is the output of the cluster in the cpp file, I gives me this error message:

catkin_ws/src/perception_pcl/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp:375:29: error: could not convert ‘output’ from ‘pcl_ros::PCLNodelet::PointCloud {aka pcl::PointCloud<pcl::PointXYZ>}’ to ‘pcl::PointCloud<pcl::PointXYZRGB>::Ptr {aka boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >}’
       mark_cluster(output, i);

I would appreciate any help and thank you in advance! I have very limited experience with cpp, so I'm guessing the solution should be rather obvious.

edit retag flag offensive close merge delete

Comments

hello is it possible to provide the full code you used and that worked at the end i am really in need of this code

Thank you in advance

hichriayoub gravatar image hichriayoub  ( 2022-04-08 09:44:51 -0600 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2018-08-06 05:49:48 -0600

The error message you're getting is because the output point cloud is the wrong type. It is a concrete XYZ point cloud, you would need to convert it firstly to a color point cloud i.e. an XYZRGB point cloud then create a shared pointer to this point cloud and pass this shared pointer.

However looking at the code of the function you've shown above, it doesn't use the color information in the point cloud at all (it must have been that type just to work with some other code) so you can simply change the type of the point cloud parameter to pcl::PointCloud<pcl::PointXYZ>::Ptr and the function will do the same job and work with your code.

Finally to get a shared pointer to your output cloud you can use the MakeShared function as below:

mark_cluster(output.makeShared(), "marker_name_space", 1, 255, 0, 0);

Hopefully this should get it working.

edit flag offensive delete link more

Comments

Thanks! Your suggestion helped me a lot! Now I changed the Type from XYZRGB to just XYZ and added

mark_cluster(output.makeShared(), i);

to call the function and

marker_pub.publish(marker);

to publish it. It does compile correctly now, but the "published" visualization_msg is empty.

th6262 gravatar image th6262  ( 2018-08-06 10:08:41 -0600 )edit

Actually it works now! I tried:

  marker_pub.publish(mark_cluster(output.makeShared(), i));

thanks a lot!

th6262 gravatar image th6262  ( 2018-08-06 10:19:04 -0600 )edit
1

Glad you got it working. If you accept this answer then other people will know it's a solution. Thanks.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-08-06 10:50:53 -0600 )edit

Sorry will do! I'm new to this page so didn't know about it ! Thank your for your help I really appreciate it ! Best regards

th6262 gravatar image th6262  ( 2018-08-06 13:14:34 -0600 )edit

No worries. Happy to help.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-08-06 15:17:22 -0600 )edit

Question Tools

Stats

Asked: 2018-08-05 08:09:28 -0600

Seen: 1,578 times

Last updated: Apr 10 '22