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