使用点云库和 ROS 从 kinect 存储和添加过去的点云

Question

我正在尝试通过在 Ubuntu 12.04 中使用来自点云库和 ROS Hydro 的迭代最近点添加来自 Kinect 的点云来构建本地地图。但是，我无法将连续的点云添加在一起来更新地图。问题是对齐的点云仅与那些当前帧的源点云一起添加。我在存储以前的点云时遇到了一些麻烦。从代码中可以看出，我用

更新了地图

Final+=*cloud_in;

但是每次都会计算一个新的Final，所以我丢失了旧的Final值。我需要保留它。我是 C++ 和 ROS 的新手，所以非常感谢对此事的帮助。

代码如下：

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

Answer 1

我认为你做错了......我的意思是，cloud_cb2 的想法是回调（至少在使用相似名称和定义的示例中这是常见的事情), 所以这个想法是每次它进入这个函数时，它都会给你一个新的云，你应该将它集成到你以前的云中......

我想通过pcl::fromROSMsg (*next_input, *cloud2_in);你是在强迫程序给你一个新的云，但它不应该像我之前告诉你的那样。

那么，回答你的问题：

icp.align(Final);

如果您阅读 PCL here 中的教程，它会告诉您此函数接收一个点云变量作为输入，该变量将包含 icp 结果。

此外，结果将是

的对齐（或尝试）

icp.setInputSource(cloud2_in);

匹配

icp.setInputTarget(cloud_in);

所以你正在覆盖 Final，2 个新的云对齐，然后添加点云中已经存在的 cloud_in 的点。

推荐你，再检查一下你的工作流程，应该是这样的

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

我只是做了一些更改以显示它应该如何，但我还没有测试它，所以您可能需要进一步更正它。我希望这个答案对你有帮助。另外，我不知道当 Final cloud 为空时，ICP 算法将如何在第一次回调中工作。此外，我建议对数据进行一些下采样，否则它会使用大量的内存并且 cpu 在对某些帧

执行此操作后

使用点云库和 ROS 从 kinect 存储和添加过去的点云

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

c++

kinect

openni

ros

point-cloud-library