CODEWITHSUNDEEP
c++kinectopennipoint-cloud-libraryros
asidd
asidd

Reputation: 3

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 ();
}

Upvotes: 0

Views: 3860

Answers (1)

api55
api55

Reputation: 11420

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

Upvotes: 0

Related Questions