Итеративная ближайшая точка возвращает неверную матрицу преобразования

Question

Я хочу создать облако точек, перемещая и поворачивая датчик глубины (датчик структуры). То, что я до сих пор является следующее:

• Создайте два облака точек, которые похожи, но вторая точка помутнения сместилась немного.
• Используйте ICP для получения матрицы преобразования (я переключил исходное и целевое облако, чтобы получить обратный).
• Преобразование исходного облака (второе облако точек, которое я создал) с матрицей преобразования.
• Добавьте все точки, которые еще не находятся в облаке общей точки.

Хотя я перемещаю датчик медленно, новые точки добавляются в странных местах, поэтому я не уверен, что я делаю это правильно. Я чувствую, что матрица преобразования ошибочна, потому что я получаю значения в моем векторе перевода от очень низкого (0,00008) до довольно высокого (3,00000), эти значения являются отрицательными и положительными.

Некоторые дополнительная информация:

• Я работаю на Android с NDK, так что я не могу использовать kinfu пример, потому что CUDA не поддерживается.
• Я использую версию PCL 1.6.

Любой шанс, что кто-то может мне помочь?

Редактировать, добавил код

Фильтрация

pcl::VoxelGrid<pcl::PointXYZ> grid; 

grid.setLeafSize (5.01, 5.01, 5.01); 
grid.setInputCloud (cloud_in); 
grid.filter (*in); 

grid.setInputCloud (cloud_out); 
grid.filter (*out); 

Точка добавления

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 
icp.setInputCloud(out); 
icp.setInputTarget(in); 
pcl::PointCloud<pcl::PointXYZ> Final; 
pcl::PointCloud<pcl::PointXYZ> transformedCloud; 
icp.setMaxCorrespondenceDistance(0.1); 
icp.setRANSACIterations(5); 
icp.setMaximumIterations(20); 
icp.setTransformationEpsilon(1e-2); 
icp.setEuclideanFitnessEpsilon(1e-5); 

icp.align(Final); 

if(icp.hasConverged()){ 
    Eigen::Matrix4f transformationMatrix = icp.getFinalTransformation(); 

    pcl::transformPointCloud(*cloud_out, *cloud_out, transformationMatrix); 
    vtkSmartPointer<vtkCellArray> conn = vtkSmartPointer<vtkCellArray>::New(); 

    vtkPoints* oldPoints = totalPolyData->GetPoints(); 

    for(pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_out->begin(); it != cloud_out->end(); it++){ 
     if(!find((double) it->x, (double) it->y, (double) it->z)){ 
      oldPoints->InsertNextPoint((double) it->x, (double) it->y, (double) it->z); 
     } 
    } 

    totalPolyData->SetPoints(oldPoints); 
    for (int i = 0; i < oldPoints->GetNumberOfPoints(); i++) 
     conn->InsertNextCell(1, &i); 
    totalPolyData->SetVerts(conn); 

Answer 1

Вы можете попытаться соединить два pointclouds полностью (не добавляя только несуществующие точки) и проверить если это хорошо или нет. Кажется, что-то не так происходит, и я подозреваю, что это ошибка PCL. Можете ли вы разместить фильтрацию и указать код, который вы используете?