open3d registration_ransac_based_on_feature_matching 算法理解

0.写在前面

1.最近在做open3d的点云配准,这里附上RegistrationRANSACBasedOnFeatureMatching的源码和一些注释
2.还没看参数checkers的作用

1.主要思想

1. 通过kdtree在target_feature中查找距离source_feature中每个点最近的点,结果储存在similar_features
2. 这些相距最近的点对保存在ransac_corres中
3. 根据这些点对计算源点云到目标点云的变换矩阵transformation
4. 应用transformation,将source向target变换
5. 返回结果RegistrationResult(用fitness和inlier_rmse来评价好坏)

2.点云的输入是否可以省略

1.用来定义similar_features的大小
2.计算近邻中心点编号(source_sample_id)
3.需要根据两个点云来计算变换矩阵(最关键)

3.点云特征的作用

直接在点云中查找哪些点是对应的点十分困难,
因此通过这些点对应的特征,计算最近邻的点,以此来查找对应点云中对应的点

3.附上代码及注释

RegistrationResult RegistrationRANSACBasedOnFeatureMatching(
        const geometry::PointCloud &source,
        const geometry::PointCloud &target,
        const Feature &source_feature,
        const Feature &target_feature,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 4*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria
        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }

    RegistrationResult result;
    int total_validation = 0;
    bool finished_validation = false;
    int num_similar_features = 1;
    //存储各个点的最近邻点
    std::vector<std::vector<int>> similar_features(source.points_.size());

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
		//存储点对的集合
        CorrespondenceSet ransac_corres(ransac_n);
        geometry::KDTreeFlann kdtree(target);
        geometry::KDTreeFlann kdtree_feature(target_feature);//用target的feature初始化
        RegistrationResult result_private;

#ifdef _OPENMP
#pragma omp for nowait //取消栅障
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (!finished_validation) {
                std::vector<double> dists(num_similar_features);//存放近邻的中心距离
                Eigen::Matrix4d transformation;
                for (int j = 0; j < ransac_n; j++) {
                	//返回一个伪随机整数 均匀分布
                    int source_sample_id = utility::UniformRandInt(
                            0, static_cast<int>(source.points_.size()) - 1);
                    if (similar_features[source_sample_id].empty()) {
                        std::vector<int> indices(num_similar_features);//存放近邻的索引
                        kdtree_feature.SearchKNN(
                        		//搜索中心点设置为source_feature
                                Eigen::VectorXd(source_feature.data_.col(
                                        source_sample_id)),
                                num_similar_features, //K nearest neighbor search
                                indices, 
                                dists);
#ifdef _OPENMP
#pragma omp critical //并行程序块，同时只能有一个线程能访问该并行程序块
#endif
                        { similar_features[source_sample_id] = indices; }
                    }
                    ransac_corres[j](0) = source_sample_id;//source的点
                    //在target中对应的点
                    if (num_similar_features == 1)
                        ransac_corres[j](1) =
                                similar_features[source_sample_id][0];
                    else {
                        ransac_corres[j](1) = similar_features
                                [source_sample_id][utility::UniformRandInt(
                                        0, num_similar_features - 1)];
                    }
                }
                bool check = true;
                for (const auto &checker : checkers) {
                    if (!checker.get().require_pointcloud_alignment_ &&
                        !checker.get().Check(source, target, ransac_corres,
                                             transformation)) {
                        check = false;
                        break;
                    }
                }
                if (!check) continue;
                //默认点对点的icp
                transformation = estimation.ComputeTransformation(
                        source, target, ransac_corres);
                check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ &&
                        !checker.get().Check(source, target, ransac_corres,
                                             transformation)) {
                        check = false;
                        break;
                    }
                }
                if (!check) continue;
                geometry::PointCloud pcd = source;
                pcd.Transform(transformation);//将source向target变换
                //计算配准结果(RegistrationResult)
                auto this_result = GetRegistrationResultAndCorrespondences(
                        pcd, target, kdtree, max_correspondence_distance,
                        transformation);
                 //比较返回结果的好坏
                if (this_result.fitness_ > result_private.fitness_ ||
                    (this_result.fitness_ == result_private.fitness_ &&
                     this_result.inlier_rmse_ < result_private.inlier_rmse_)) {
                    result_private = this_result;
                }
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    total_validation = total_validation + 1;
                    if (total_validation >= criteria.max_validation_)
                        finished_validation = true;
                }
            }  // end of if statement
        }      // end of for-loop
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            if (result_private.fitness_ > result.fitness_ ||
                (result_private.fitness_ == result.fitness_ &&
                 result_private.inlier_rmse_ < result.inlier_rmse_)) {
                result = result_private;
            }
        }
#ifdef _OPENMP
    }
#endif
    utility::LogDebug("total_validation : {:d}", total_validation);
    utility::LogDebug("RANSAC: Fitness {:e}, RMSE {:e}", result.fitness_,
                      result.inlier_rmse_);
    return result;
}

参考
https://github.com/intel-isl/Open3D/blob/2a55f8ed0ee6f71c7b76382dffe27f1ef3b29ab7/cpp/open3d/pipelines/registration/Registration.cpp#L241:1