ICP 产生具有高适应度值的对齐答案

【问题标题】：ICP produces alignments with high Fitness valuesICP 产生具有高适应度值的对齐
【发布时间】：2017-02-07 10:07:36
【问题描述】：

我正在学习如何将 ICP 与 PCL 结合使用。因此，我编写了一个从给定文件夹中读取一系列 .pcd 文件的函数。

//Reads all PCD files in a folder. Specify the path without the last '/' and the number on the last and first file: xxx.pcb.
//Returns null if something goes wrong. Otherwise returns a single global pointcloud with each subcloud colour coded.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr LoadPCDFiles(std::string FolderPath, int FinalFile, int FirstFile)
{
    //Initialize some variables we need.
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr Global(new pcl::PointCloud<pcl::PointXYZRGB>());
    srand(time(NULL));
    int FileIter = FirstFile;
    std::string BasePathString = FolderPath + "/%.3d.pcd";
    Eigen::Matrix4f AccumulatedTransformationMatrix = Eigen::Matrix4f::Identity();

    while (FileIter <= FinalFile) {
        char buffer[260];
        std::sprintf(buffer, BasePathString.c_str(), FileIter);
        std::string path(buffer);
        cloud = ReadPCDFile(path, std::rand() % 256, std::rand() % 256, std::rand() % 256);
        if (cloud == NULL) return NULL;//Error
        //pcl::transformPointCloud<pcl::PointXYZRGB>(*cloud, *cloud, Eigen::Vector3f(cloud->sensor_origin_.x(), cloud->sensor_origin_.y(), cloud->sensor_origin_.z()), cloud->sensor_orientation_);
        //pcl::transformPointCloud<pcl::PointXYZRGB>(*cloud, *cloud, AccumulatedTransformationMatrix);

        int AlignmentIter = 0; //The number of alignment attemps we have made.
        int MaxAlignment = 25; //Maax number of attempts allowed.
        float LastFitness = VTK_FLOAT_MAX;
        bool hasConverged = false;
        std::cout << "Loaded PCD: " << path << std::endl;
        if (FileIter != FirstFile) {

            while (AlignmentIter < MaxAlignment && LastFitness > 0.001) {
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr Temp(new pcl::PointCloud<pcl::PointXYZRGB>());
                pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
                icp.setInputSource(cloud);
                icp.setInputTarget(Global);

                icp.align(*Temp);

                float CurrentFitness = icp.getFitnessScore();
                hasConverged = icp.hasConverged();
                std::cout << "Alignment value: " << CurrentFitness << std::endl;

                if (abs(LastFitness - CurrentFitness) < 0.0001) { break; }

                cloud = Temp;
                LastFitness = CurrentFitness;
                AlignmentIter++;
            }


            //AccumulatedTransformationMatrix = AccumulatedTransformationMatrix * icp.getFinalTransformation();
            if (hasConverged) {
                *Global = *Global + *cloud;

                std::cout << "Updated global cloud: " << (*Global).size() << std::endl;
            } else {
                std::cout << "Unable to align the clouds." << std::endl;
            }
        } else {
            *Global = *cloud;
            std::cout << "First subcloud added." << std::endl;
        }



        FileIter++;
    }

    std::cout << "Merged clouds." << std::endl;
    return Global;
}

但是，这会产生一些非常混乱的云。

它的适应度值刚刚超过 0.052。

它的适应度值在 0.0030 - 0.0856 范围内。 All values.

我给每一朵云都赋予了一种随机颜色来突出显示哪朵云最终在哪里。

ReadPCDFile() 函数只是读取指定的文件并添加随机颜色。注释掉的代码行只是我拼命地弄乱了一些想法，但最终都失败了。

这是其中一个 PCD 文件的示例。

# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 13509
HEIGHT 1
VIEWPOINT 7.9950223 0.60561943 0.4050533 - 0.3022367 0.019049812 0.95157933 0.052790195
POINTS 13509
DATA ascii
- 1.0610341 - 0.55464476 1.933659
- 1.0921015 - 0.5514014 1.9721884
- 1.0254035 - 0.55889213 1.922963
- 1.0736096 - 0.54884547 1.9840248
- 1.1002594 - 0.5510374 1.9640691
- 1.05391 - 0.556308 1.9436798
- 1.0801263 - 0.5577761 1.9309663
- 1.1306747 - 0.54748887 1.9968926
- 0.98958766 - 0.55052584 1.926852
.
.
.

希望你能帮上忙。请随时询问我可以提供的任何信息。

非常感谢您的帮助。

【问题讨论】：

从两个点云开始怎么样，最好是清晰可对齐的点云？当试图让这两者很好地对齐时，应该更容易掌握出了什么问题。然后从那里出发。
我只添加了两个点云。第一张照片显示了这一点。我只有两朵云在同一个角落，角度略有不同。
|| 的输出是什么std::cout

标签： c++ point-cloud-library point-clouds

【解决方案1】：

我还在学习如何将 ICP 与 PCL 结合使用。我发现有几个参数可以设置，比如

icp.setTransformationEpsilon(1e-6);
icp.setMaximumIterations(1e6);

在这之后，我发现我的体能得分更好（0.005 到 2e-5）。

【讨论】：