c++kinectopennipoint-cloud-libraryros

Storing and adding past point clouds from kinect using point cloud library and ROS


I am trying to build a local map by adding point clouds from Kinect using iterative closest point from Point Cloud Library and ROS Hydro in Ubuntu 12.04. However, I am not able to add consecutive point clouds together to update the map. The problem is that the aligned pointcloud is only added with the source pointcloud for those current frames. I am having a bit of trouble storing the previous point clouds. As seen from the code I update the map with

Final+=*cloud_in;

However a new Final is computed every time, so I lose the old Final value. I need to retain it. I am a novice in C++ and ROS, so I would greatly appreciate help on this matter.

Listed below is the Code:

ros::Publisher _pub;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>);

void
cloud_cb2 (const sensor_msgs::PointCloud2ConstPtr& next_input)
{
  pcl::fromROSMsg (*next_input, *cloud_in);
  //remove NAN points from the cloud
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cloud_in,*cloud_in, indices);
// Convert the sensor_msgs/PointCloud2 data to pcl::PointCloud
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_in (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromROSMsg (*next_input, *cloud2_in);
  //remove NAN points
  std::vector<int> indices2;
  pcl::removeNaNFromPointCloud(*cloud2_in,*cloud2_in, indices2);

  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  icp.setInputSource(cloud2_in);
  icp.setInputTarget(cloud_in);
  pcl::PointCloud<pcl::PointXYZRGB> Final;
  icp.align(Final);
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;
  Final+=*cloud_in;

 // Convert the pcl::PointCloud to sensor_msgs/PointCloud2
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg( Final, output );
  // Publish the map
  _pub.publish(output);
}

int main (int argc, char** argv)
{
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // ROS subscriber for /camera/depth_registered/points
  ros::Subscriber sub = nh.subscribe(
                    "/camera/depth_registered/points",
                    2,
                    cloud_cb2
                    );

  // Create ROS publisher for transformed pointcloud
  _pub = nh.advertise<sensor_msgs::PointCloud2>(
                           "output",
                           1
                           );
  // Spin
  ros::spin ();
}

Solution

  • I think you are doing it wrong... I mean, the idea of cloud_cb2 is to be a callback (at least that is the common thing in the examples where they use a similar name and definition), so the idea is that every time it enters this function, it gives you a new cloud that you should integrate to your previous cloud...

    I suppose that by doing pcl::fromROSMsg (*next_input, *cloud2_in); you are forcing the program to give you a new cloud, but it should not be like that as I told you before.

    Then, to answer your question:

    icp.align(Final);
    

    If you read the tutorial from PCL here, it tells you that this function receives as input a point cloud variable that will contain the icp result.

    Also, the result will be the alignment (or try) of

    icp.setInputSource(cloud2_in);
    

    to match

    icp.setInputTarget(cloud_in);
    

    So you are overwriting Final, with the 2 new clouds align, and then adding the points of cloud_in that is already in the pointcloud.

    I recommend you, to check your workflow again, it should be something like this

    ros::Publisher _pub;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr Final (new pcl::PointCloud<pcl::PointXYZRGB>);
    
    void
    cloud_cb2 (const sensor_msgs::PointCloud2ConstPtr& next_input)
    {
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
      pcl::fromROSMsg (*next_input, *cloud_in);
      //remove NAN points from the cloud
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*cloud_in,*cloud_in, indices);
    
      pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
      icp.setInputSource(cloud_in);
      icp.setInputTarget(Final);
      pcl::PointCloud<pcl::PointXYZRGB> Final;
      icp.align(tmp_cloud);
      std::cout << "has converged:" << icp.hasConverged() << " score: " <<
      icp.getFitnessScore() << std::endl;
      std::cout << icp.getFinalTransformation() << std::endl;
      Final = tmp_cloud;
    
     // Convert the pcl::PointCloud to sensor_msgs/PointCloud2
      sensor_msgs::PointCloud2 output;
      pcl::toROSMsg( Final, output );
      // Publish the map
      _pub.publish(output);
    }
    
    int main (int argc, char** argv)
    {
      ros::init (argc, argv, "my_pcl_tutorial");
      ros::NodeHandle nh;
    
      // ROS subscriber for /camera/depth_registered/points
      ros::Subscriber sub = nh.subscribe(
                        "/camera/depth_registered/points",
                        2,
                        cloud_cb2
                        );
    
      // Create ROS publisher for transformed pointcloud
      _pub = nh.advertise<sensor_msgs::PointCloud2>(
                               "output",
                               1
                               );
      // Spin
      ros::spin ();
    }
    

    I just did some changes to show how it should be, but I haven't tested it so probably you'll need to correct it even further. I hope this answer helps you. Also, I do not know how the ICP algorithm will work in the first call back when the Final cloud is empty. Also, I recommend some downsampling of the data, or else it will use incredibly huge amounts of memory and cpu after doing it for some frames