Функция PCL ICP не добавляет второе облако точек

Question

Я написал небольшой фрагмент кода для слияния двух точечных облаков с ICP с использованием PCL. Однако, независимо от того, что я делаю, я получаю последнее облако точек, содержащее только первое облако точек. Картина:

#define _CRT_SECURE_NO_DEPRECATE 

#include <pcl/point_types.h> 
#include <pcl/visualization/cloud_viewer.h> 
#include <pcl/registration/icp.h> 
#include <pcl/io/pcd_io.h> 
#include <boost/make_shared.hpp> 
#include "Dot3DReader.h" 


int main(int argc, char** argv) 
{ 

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>); 

if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:/Imerso/PCL PointCloudExampleData/Frame1.pcd", *cloud1) == -1) //* load the file 
{ 
    PCL_ERROR("Couldn't read file Frame1.pcd \n"); 
    return (-1); 
} 

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>); 

if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:/Imerso/PCL PointCloudExampleData/Frame2.pcd", *cloud2) == -1) //* load the file 
{ 
    PCL_ERROR("Couldn't read file Frame2.pcd \n"); 
    return (-1); 
} 

std::cout << "Read both files." << std::endl; 

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 
icp.setInputSource(cloud1); 
icp.setInputTarget(cloud2); 

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> Final(new pcl::PointCloud<pcl::PointXYZ>()); 


std::cout << "Starting aligning." << std::endl; 

icp.align(*Final); 

std::cout << "Finished aligning." << std::endl; 
std::cout << "has converged:" << icp.hasConverged() << " score: " << 
    icp.getFitnessScore() << std::endl; 
// CloudViewer を生成 
// （表示は別スレッドで実行される) 
pcl::visualization::CloudViewer viewer("Final cloud Viewer"); 


// CloudViewer で点群を表示 
viewer.showCloud(Final); 

while (true) { 
    // ESC キーで終了 
    if (GetKeyState(VK_ESCAPE) < 0) { 
     return 0; 
    } 
} 
return 0; 
} 

В качестве бонуса, я также хочу, чтобы сохранить цвета точек выровнены при использовании точек RGB. Как я могу это сделать?

Я предполагаю, что он не находит соответствия и просто отбрасывает второе облако точек.

Благодарим за помощь.

Answer 1

Я не уверен, что я глуп, или код документа и кода вводит в заблуждение, но мне удалось выяснить, что делают функции и как вы его используете.

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

Пример кода: boost :: shared_ptr> cloud1, cloud2, GlobalCloud; // Заполните inn в первых двух с pointclouds, которые вы хотите объединить.

pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp; 
icp.setInputSource(cloud1); //This cloud will be transformed to match 
icp.setInputTarget(cloud2); //this cloud. The result is stored in the cloud provided as input below. 


icp.align(*cloud1); //Overwrite the source cloud with the transformed cloud. 

*GlobalCloud = *cloud1 + *cloud2; //Merge the two now aligned and matched clouds. 

//Do whatever you want with the global cloud. 

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

Просто спросите, есть ли что-то, что вы хотите знать, и я постараюсь помочь.