포인트 클라우드 라이브러리 및 ROS를 사용하여 kinect에서 과거 포인트 클라우드 저장 및 추가

나는 당신이 잘못하고 있다고 생각합니다 ... 내 말은, cloud_cb2의 아이디어는 콜백이라는 것입니다 (적어도 비슷한 이름과 정의를 사용하는 예제에서 일반적인 것입니다). 그래서 아이디어는 매번 이 기능에 들어가면 이전 클라우드에 통합해야하는 새 클라우드를 제공합니다.

그렇게 pcl::fromROSMsg (*next_input, *cloud2_in);함으로써 프로그램이 새로운 클라우드를 제공하도록 강요하는 것 같지만 이전에 말한 것처럼 그렇게해서는 안됩니다.

그런 다음 질문에 답하려면 :

icp.align(Final);

여기서 PCL 에서 튜토리얼을 읽으면 이 함수가 icp 결과를 포함 할 점 구름 변수를 입력으로 받는다는 것을 알려줍니다.

또한 결과는 정렬 (또는 시도)이됩니다.

icp.setInputSource(cloud2_in);

일치시키다

icp.setInputTarget(cloud_in);

따라서 Final두 개의 새 구름이 정렬되어을 덮어 쓴 다음 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 클라우드가 비어있을 때 첫 번째 콜백에서 ICP 알고리즘이 어떻게 작동하는지 알 수 없습니다. 또한 데이터의 일부 다운 샘플링을 권장합니다. 그렇지 않으면 일부 프레임에 대해 수행 한 후 엄청나게 많은 양의 메모리와 CPU를 사용하게됩니다.