c++ - ICP产生具有较高适应性值的比对

我正在学习如何在PCL中使用ICP。这样，我编写了一个从给定文件夹中读取一系列.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
.
.
.

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

非常感谢您的帮助。

最佳答案

我也在学习如何在PCL中使用ICP。我发现您可以设置几个参数，例如

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

之后，我发现自己的健身得分更好(0.005至2e-5)。

关于c++ - ICP产生具有较高适应性值的比对，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/32360744/