Question

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

更新了地图

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个新云对齐，然后添加已经在pointcloud中的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 ();
}

我只是做了一些改动来展示它应该如何，但我还没有测试过，所以你可能需要进一步纠正它。我希望这个答案可以帮到你。此外，我不知道当最终云为空时，ICP算法在第一次回调时如何工作。另外，我建议对数据进行一些下采样，否则在为某些帧执行后会使用相当大量的内存和cpu

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

1 个答案: