Сохранение и добавление прошлых облаков точек из kinect с помощью библиотеки облаков точек и ROS

Я думаю, вы делаете это неправильно ... Я имею в виду, идея cloud_cb2 - быть обратным вызовом (по крайней мере, это обычная вещь в примерах, где они используют похожее имя и определение), поэтому идея состоит в том, что каждый раз он входит в эту функцию, он дает вам новое облако, которое вы должны интегрировать в свое предыдущее облако ...

Я полагаю, что, делая pcl::fromROSMsg (*next_input, *cloud2_in);, вы заставляете программу предоставлять вам новое облако, но это не должно быть так, как я говорил вам ранее.

Затем, чтобы ответить на ваш вопрос:

icp.align(Final);

Если вы читаете руководство из PCL здесь, оно сообщает вам, что эта функция получает в качестве входных данных переменная облака точек, которая будет содержать результат 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 ();
}

Я просто внес некоторые изменения, чтобы показать, как это должно быть, но я не тестировал это, поэтому, вероятно, вам нужно будет исправить это еще раз. Надеюсь, этот ответ вам поможет. Кроме того, я не знаю, как алгоритм ICP будет работать при первом обратном вызове, когда облако Final пусто. Кроме того, я рекомендую немного понизить выборку данных, иначе он будет использовать невероятно огромные объемы памяти и процессора после выполнения этого для некоторых кадров.