首页 文章

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

提问于
浏览 次
0

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

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 ();
}
c++ kinect openni point-cloud-library ros

1 回答

  • 0

    我认为你做错了......我的意思是，cloud_cb2的想法是回调（至少这是示例中他们使用类似名称和定义的常见事物），所以这个想法是每次都是它进入这个功能，它为你提供了一个新的 Cloud ，你应该集成到你以前的 Cloud ...

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

    然后，回答你的问题：

    icp.align(Final);

    如果您从PCL here阅读教程，它会告诉您此函数接收包含icp结果的点 Cloud 变量作为输入 .

    此外，结果将是对齐（或尝试）

    icp.setInputSource(cloud2_in);

    匹配

    icp.setInputTarget(cloud_in);

    因此，您将覆盖 Final ，将2个新 Cloud 对齐，然后添加已经在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 ();
}

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

    回复于

相关问题