[HELP] Problem with point cloud messages!

asked 2017-11-11 04:19:09 -0500

schizzz8 gravatar image

updated 2017-11-11 07:07:25 -0500

Hi all,

I'm struggling with this bug since two days and I'm really going crazy, so I hope that you can give me good hints on what can cause this strange behaviour.

I'm working with gazebo, where I created an apartment-like environment and a mobile robot. The robot is equipped with an RGB-D camera and a logical_camera.

To get the "images" from the logical camera I wrote a gazebo plugin, as explained in the tutorial, and I slightly modified the message to also contain the models bounding boxes:

void LogicalCameraPlugin::OnUpdate(){
     msgs::LogicalCameraImage logical_image;
     semantic_map_benchmarking::LogicalCameraImage msg;
     logical_image = this->parentSensor->Image();
     gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
     if (!scene || !scene->Initialized())
         return;

     msg.header.stamp = ros::Time::now();
     msg.header.frame_id = "logical_camera_link";

     msg.pose.position.x = logical_image.pose().position().x();
     ...

     int number_of_models = logical_image.model_size();
     for(int i=0; i < number_of_models; i++){
         semantic_map_benchmarking::Model model_msg;

        rendering::VisualPtr visual = scene->GetVisual(logical_image.model(i).name());
        if (!visual)
             continue;

        math::Box bounding_box = visual->GetBoundingBox();

        model_msg.pose.position.x = logical_image.model(i).pose().position().x();

       model_msg.size.x = bounding_box.GetXLength();
       model_msg.size.y = bounding_box.GetYLength();
       model_msg.size.z = bounding_box.GetZLength();

       model_msg.min.x = bounding_box.min.x;
       model_msg.min.y = bounding_box.min.y;
       model_msg.min.z = bounding_box.min.z;

       model_msg.max.x = bounding_box.max.x;
       model_msg.max.y = bounding_box.max.y;
       model_msg.max.z = bounding_box.max.z;

       msg.models.push_back(model_msg);
}

  this->image_pub.publish(msg);
}

What I want to do is the following: given the logical camera image containing the models bounding boxes and the depth camera point cloud, for each bounding box find the points of the cloud that fall inside it.

So, I implemented a node that listens to logical_camera and rgbd camera messages with message filters and approximate time synchronization, and this is what I do in the callback:

void filterCallback(const semantic_map_benchmarking::LogicalCameraImage::ConstPtr& logical_image_msg,
                    const PointCloud::ConstPtr& scene_cloud_msg){

    tf::StampedTransform depth_camera_pose;
    try {
        _listener.waitForTransform("map",
                                   "camera_depth_optical_frame",
                                   ros::Time(0),
                                   ros::Duration(3));
        _listener.lookupTransform("map",
                                  "camera_depth_optical_frame",
                                  ros::Time(0),
                                  depth_camera_pose);
    }
    catch(tf::TransformException ex) {
        ROS_ERROR("%s", ex.what());
    }

    Eigen::Isometry3f depth_camera_transform = tfTransform2eigen(depth_camera_pose);
    PointCloud::Ptr map_cloud (new PointCloud ());
    pcl::transformPointCloud (*scene_cloud_msg, *map_cloud, depth_camera_transform);

    PointCloud::Ptr objects_cloud_msg (new PointCloud ());
    objects_cloud_msg->header.frame_id = "/map";
    objects_cloud_msg->width  = 1;
    objects_cloud_msg->is_dense = false;
    int num_points=0;

    PointCloud::Ptr boxes_cloud_msg (new PointCloud ());
    boxes_cloud_msg->header.frame_id = "/map";
    boxes_cloud_msg->height = logical_image_msg->models.size()*8;
    boxes_cloud_msg->width  = 1;
    boxes_cloud_msg->is_dense = false;

    tf::StampedTransform logical_camera_pose;
    tf::poseMsgToTF(logical_image_msg->pose,logical_camera_pose);

    for(int i=0; i < logical_image_msg->models.size(); i++){

        tf::Transform model_pose;
        tf::poseMsgToTF(logical_image_msg->models.at(i).pose,model_pose);

        Eigen::Vector3f box_min = tfTransform2eigen(logical_camera_pose)*tfTransform2eigen(model_pose)*
                Eigen::Vector3f(logical_image_msg->models.at(i).min.x,
                                logical_image_msg->models.at(i).min.y,
                                logical_image_msg->models.at(i).min.z);

        Eigen::Vector3f box_max = tfTransform2eigen(logical_camera_pose)*tfTransform2eigen(model_pose)*
                Eigen::Vector3f(logical_image_msg->models.at(i).max.x,
                                logical_image_msg->models.at(i).max.y,
                                logical_image_msg->models.at(i).max.z);

        float x_range = box_max.x()-box_min.x();
        float y_range = box_max.y()-box_min.y();
        float z_range = box_max.z()-box_min.z();

        //in this way I ...
(more)
edit retag flag offensive close merge delete

Comments

1

Not directly an answer, but three suggestions:

1 . change the title of your question: if you weren't looking for ppl to help you, you wouldn't be posting here. We already know that you are in need of help. Also: the title doesn't tell anyone anything at the moment. Make it descriptive.

..

gvdhoorn gravatar imagegvdhoorn ( 2017-11-11 05:20:18 -0500 )edit

2 . use as many convenience methods from pcl_conversions as possible (don't work with sensor_msgs/PointCloud2 instances directly fi)

3 . use PCL for this: I'm not an expert, but it would seem that PCL itself (so the library), as a lot of functionality ..

gvdhoorn gravatar imagegvdhoorn ( 2017-11-11 05:26:09 -0500 )edit

you should be able to reuse. One candidate: pcl::ExtractIndices (tutorial: Extracting indices from a PointCloud).

Reusing this should allow you to avoid ..

gvdhoorn gravatar imagegvdhoorn ( 2017-11-11 05:29:21 -0500 )edit

.. the work and any implementation mistakes that others have already made for you (and fixed) in PCL.

(I believe you don't need to use RANSAC in your case, as you already have the points).

gvdhoorn gravatar imagegvdhoorn ( 2017-11-11 05:30:49 -0500 )edit

As to your current code, I find this line:

objects_cloud_msg->width  = 1;

suspicious. Is your cloud really only a single line of points?

gvdhoorn gravatar imagegvdhoorn ( 2017-11-11 05:32:02 -0500 )edit