Matrix 迭代最近点返回错误的变换矩阵

我想通过移动和旋转我的深度传感器（结构传感器）来创建点云。 到目前为止，我掌握的情况如下：

• 创建两个相似的点云，但第二个点云有点偏移
• 使用ICP获得变换矩阵（我切换了源云和目标云，因此得到了逆矩阵）
• 使用变换矩阵变换源云（我创建的第二个点云）
• 添加不在整个点云中的所有点

虽然我正在缓慢地移动传感器，但是新的点被添加到了奇怪的位置，所以我不确定我是否做对了。我感觉转换矩阵是错误的，因为我在转换向量中得到的值从很低（000008）到很高（300000），这些值都是负值和正值

一些额外信息：

• 我使用NDK在Android上工作，所以我不能使用kinfu示例，因为不支持CUDA
• 我使用PCL版本1.6

有人能帮我吗

编辑，添加一些代码

过滤

pcl:：体素网格；
grid.setLeafSize（5.01,5.01,5.01）；
grid.setInputCloud（cloud_in）；
grid.filter（*in）；
grid.setInputCloud（cloud\u out）；
grid.filter（*out）；

加点

pcl:：迭代闭合点icp；
icp.setInputCloud（输出）；
icp.设定输入目标（单位：英寸）；
点云最终版；
pcl:：点云转换云；
icp.设定最大对应距离（0.1）；
icp.SetTransactions（5）；
icp.设定最大值（20）；
icp.setTransformationEpsilon（1e-2）；
icp.Seteuclideanfitnessipsilon（1e-5）；
icp.align（最终版）；
if（icp.hasConvergend（））{
Eigen:：Matrix4f transformationMatrix=icp.getFinalTransformation（）；
pcl:：transformPointCloud（*cloud\u out，*cloud\u out，transformationMatrix）；
vtkSmartPointer conn=vtkSmartPointer:：New（）；
vtkPoints*oldPoints=totalPolyData->GetPoints（）；
对于（pcl:：PointCloud:：iterator it=cloud\u out->begin（）；it！=cloud\u out->end（）；it++）{
如果（！find（（double）it->x，（double）it->y，（double）it->z））{
oldPoints->InsertNextPoint（（双）it->x，（双）it->y，（双）it->z）；
}
}
totalPolyData->设定点（旧点）；
对于（int i=0；iGetNumberOfPoints（）；i++）
conn->InsertNextCell（1，&i）；
totalPolyData->SetVerts（连接）；

您可以尝试将两个点云完全连接起来（而不是只添加不存在的点），并检查其是否良好。 好像有什么地方出了问题，我怀疑是PCL的问题。你能发布你正在使用的过滤和添加代码吗

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);