[HELP] Problem with point cloud messages!

edit

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 ...