ICP生成具有高Fitness值的路线

Question

我正在学习如何在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
.
.
.

希望你能帮上忙。请不要犹豫，询问我能提供的任何信息。

非常感谢你的帮助。

Answer 1

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

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

在这之后，我发现我的健康评分更好了(0.005到2e-5)。