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 get the 8 vertices of the bounding box
        for(int k=0; k <= 1; k++)
            for(int j=0; j <= 1; j++)
                for(int i=0; i <= 1; i++)
                    boxes_cloud_msg->points.push_back (pcl::PointXYZ(box_min.x() + i*x_range,
                                                                     box_min.y() + j*y_range,
                                                                     box_min.z() + k*z_range));

        for(int i=0; i < map_cloud->points.size(); ++i){

            if(map_cloud->points[i].x > box_min.x() && map_cloud->points[i].x < box_max.x() &&
                    map_cloud->points[i].y > box_min.y() && map_cloud->points[i].y < box_max.y() &&
                    map_cloud->points[i].z > box_min.z() && map_cloud->points[i].z < box_max.z()){

                objects_cloud_msg->points.push_back(map_cloud->points[i]);
                num_points++;
            }

        }
    }
    objects_cloud_msg->height = num_points;

    _boxes_cloud_pub.publish(boxes_cloud_msg);

    _objects_cloud_pub.publish(objects_cloud_msg);

}

I'm 99% sure that the math is correct, that is, the two clouds are in the same reference frame (i.e. map). In RViz, I visualize correctly the two clouds and I can see that in the space defined by the bounding box it actually falls a certain number of points but the if statement never becomes true and objects_cloud is always empty.

I'd like to show you a screenshot of the situation but I can't because I'm posting this question from another pc, nevertheless, I kindly ask you to trust me since I made all the possible checks that came to my mind.

As in the title, still I can't understand where is the problem, so I'd be very glad if someone could give me some hint about this bug!!!

Thanks.

objects_cloud_msg->width  = 1;