Registration.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "pipelines/registration/Registration.h"
 
#include <ecvFeature.h>
#include <ecvKDTreeFlann.h>
#include <ecvPointCloud.h>
 
// CV_CORE_LIB
#include <Helper.h>
#include <Logging.h>
#include <Parallel.h>
 
namespace cloudViewer {
namespace pipelines {
namespace registration {
 
static RegistrationResult GetRegistrationResultAndCorrespondences(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const geometry::KDTreeFlann &target_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return result;
    }
 
    double error2 = 0.0;
 
#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < (int)source.size(); i++) {
            std::vector<int> indices(1);
            std::vector<double> dists(1);
            const auto &point = source.getEigenPoint(i);
            if (target_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           1, indices, dists) > 0) {
                error2_private += dists[0];
                correspondence_set_private.push_back(
                        Eigen::Vector2i(i, indices[0]));
            }
        }
#ifdef _OPENMP
#pragma omp critical(GetRegistrationResultAndCorrespondences)
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif
 
    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        result.fitness_ = (double)corres_number / (double)source.size();
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return result;
}
 
static RegistrationResult EvaluateRANSACBasedOnCorrespondence(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    double error2 = 0.0;
    int good = 0;
    double max_dis2 = max_correspondence_distance * max_correspondence_distance;
    for (const auto &c : corres) {
        double dis2 = (source.getEigenPoint(c[0]) - target.getEigenPoint(c[1]))
                              .squaredNorm();
        if (dis2 < max_dis2) {
            good++;
            error2 += dis2;
            result.correspondence_set_.push_back(c);
        }
    }
    if (good == 0) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        result.fitness_ = (double)good / (double)corres.size();
        result.inlier_rmse_ = std::sqrt(error2 / (double)good);
    }
    return result;
}
 
RegistrationResult EvaluateRegistration(const ccPointCloud &source,
                                        const ccPointCloud &target,
                                        double max_correspondence_distance,
                                        const Eigen::Matrix4d &transformation
                                        /* = Eigen::Matrix4d::Identity()*/) {
    geometry::KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    ccPointCloud pcd = source;
    if (!transformation.isIdentity()) {
        pcd.Transform(transformation);
    }
 
    return GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
}
 
RegistrationResult RegistrationICP(
        const ccPointCloud &source,
        const ccPointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        const ICPConvergenceCriteria
                &criteria /* = ICPConvergenceCriteria()*/) {
    if (max_correspondence_distance <= 0.0) {
        utility::LogError("Invalid max_correspondence_distance.");
    }
    if ((estimation.GetTransformationEstimationType() ==
                 TransformationEstimationType::PointToPlane ||
         estimation.GetTransformationEstimationType() ==
                 TransformationEstimationType::ColoredICP) &&
        (!source.hasNormals() || !target.hasNormals())) {
        utility::LogError(
                "TransformationEstimationPointToPlane and "
                "TransformationEstimationColoredICP "
                "require pre-computed normal vectors.");
    }
 
    Eigen::Matrix4d transformation = init;
    geometry::KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    ccPointCloud pcd = source;
    if (!init.isIdentity()) {
        pcd.Transform(init);
    }
    RegistrationResult result;
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
    for (int i = 0; i < criteria.max_iteration_; i++) {
        utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i,
                          result.fitness_, result.inlier_rmse_);
        Eigen::Matrix4d update = estimation.ComputeTransformation(
                pcd, target, result.correspondence_set_);
        transformation = update * transformation;
        pcd.Transform(update);
        RegistrationResult backup = result;
        result = GetRegistrationResultAndCorrespondences(
                pcd, target, kdtree, max_correspondence_distance,
                transformation);
        if (std::abs(backup.fitness_ - result.fitness_) <
                    criteria.relative_fitness_ &&
            std::abs(backup.inlier_rmse_ - result.inlier_rmse_) <
                    criteria.relative_rmse_) {
            break;
        }
    }
    return result;
}
 
RegistrationResult RegistrationRANSACBasedOnCorrespondence(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 3*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria
        /* = RANSACConvergenceCriteria()*/,
        utility::optional<unsigned int> seed /* = utility::nullopt*/) {
    if (ransac_n < 3 || (int)corres.size() < ransac_n ||
        max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }
 
    RegistrationResult best_result;
    int exit_itr = -1;
#ifdef _OPENMP
#pragma omp parallel
#endif
    {
        CorrespondenceSet ransac_corres(ransac_n);
        RegistrationResult best_result_local;
        int exit_itr_local = criteria.max_iteration_;
        unsigned int seed_val =
                seed.has_value() ? seed.value() : std::random_device{}();
        utility::UniformRandIntGenerator rand_generator(
                0, static_cast<int>(corres.size()) - 1, seed_val);
 
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (itr < exit_itr_local) {
                for (int j = 0; j < ransac_n; j++) {
                    ransac_corres[j] = corres[rand_generator()];
                }
 
                Eigen::Matrix4d transformation =
                        estimation.ComputeTransformation(source, target,
                                                         ransac_corres);
 
                // Check transformation: inexpensive
                bool check = true;
                for (const auto &checker : checkers) {
                    if (!checker.get().Check(source, target, ransac_corres,
                                             transformation)) {
                        check = false;
                        break;
                    }
                }
                if (!check) continue;
 
                ccPointCloud pcd = source;
                pcd.Transform(transformation);
                auto result = EvaluateRANSACBasedOnCorrespondence(
                        pcd, target, corres, max_correspondence_distance,
                        transformation);
 
                if (result.IsBetterRANSACThan(best_result_local)) {
                    best_result_local = result;
 
                    // Update exit condition if necessary
                    double exit_itr_d =
                            std::log(1.0 - criteria.confidence_) /
                            std::log(1.0 - std::pow(result.fitness_, ransac_n));
                    exit_itr_local =
                            exit_itr_d < double(criteria.max_iteration_)
                                    ? static_cast<int>(std::ceil(exit_itr_d))
                                    : exit_itr_local;
                }
            }  // if < exit_itr_local
        }  // for loop
 
#ifdef _OPENMP
#pragma omp critical(RegistrationRANSACBasedOnCorrespondence)
#endif
        {
            if (best_result_local.IsBetterRANSACThan(best_result)) {
                best_result = best_result_local;
            }
            if (exit_itr_local > exit_itr) {
                exit_itr = exit_itr_local;
            }
        }
    }
    utility::LogDebug(
            "RANSAC exits at {:d}-th iteration: inlier ratio {:e}, "
            "RMSE {:e}",
            exit_itr, best_result.fitness_, best_result.inlier_rmse_);
    return best_result;
}
 
RegistrationResult RegistrationRANSACBasedOnFeatureMatching(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const utility::Feature &source_feature,
        const utility::Feature &target_feature,
        bool mutual_filter,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 3*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria
        /* = RANSACConvergenceCriteria()*/,
        utility::optional<unsigned int> seed /* = utility::nullopt*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }
 
    int num_src_pts = int(source.size());
    int num_tgt_pts = int(target.size());
 
    geometry::KDTreeFlann kdtree_target(target_feature);
    pipelines::registration::CorrespondenceSet corres_ij(num_src_pts);
 
#ifdef _OPENMP
#pragma omp parallel for num_threads(utility::EstimateMaxThreads())
#endif
    for (int i = 0; i < num_src_pts; i++) {
        std::vector<int> corres_tmp(1);
        std::vector<double> dist_tmp(1);
 
        kdtree_target.SearchKNN(Eigen::VectorXd(source_feature.data_.col(i)), 1,
                                corres_tmp, dist_tmp);
        int j = corres_tmp[0];
        corres_ij[i] = Eigen::Vector2i(i, j);
    }
 
    // Do reverse check if mutual_filter is enabled
    if (mutual_filter) {
        geometry::KDTreeFlann kdtree_source(source_feature);
        pipelines::registration::CorrespondenceSet corres_ji(num_tgt_pts);
 
#ifdef _OPENMP
#pragma omp parallel for num_threads(utility::EstimateMaxThreads())
#endif
        for (int j = 0; j < num_tgt_pts; ++j) {
            std::vector<int> corres_tmp(1);
            std::vector<double> dist_tmp(1);
            kdtree_source.SearchKNN(
                    Eigen::VectorXd(target_feature.data_.col(j)), 1, corres_tmp,
                    dist_tmp);
            int i = corres_tmp[0];
            corres_ji[j] = Eigen::Vector2i(i, j);
        }
 
        pipelines::registration::CorrespondenceSet corres_mutual;
        for (int i = 0; i < num_src_pts; ++i) {
            int j = corres_ij[i](1);
            if (corres_ji[j](0) == i) {
                corres_mutual.emplace_back(i, j);
            }
        }
 
        // Empirically mutual correspondence set should not be too small
        if (int(corres_mutual.size()) >= ransac_n * 3) {
            utility::LogDebug("{:d} correspondences remain after mutual filter",
                              corres_mutual.size());
            return RegistrationRANSACBasedOnCorrespondence(
                    source, target, corres_mutual, max_correspondence_distance,
                    estimation, ransac_n, checkers, criteria, seed);
        }
        utility::LogDebug(
                "Too few correspondences after mutual filter, fall back to "
                "original correspondences.");
    }
 
    return RegistrationRANSACBasedOnCorrespondence(
            source, target, corres_ij, max_correspondence_distance, estimation,
            ransac_n, checkers, criteria, seed);
}
 
Eigen::Matrix6d GetInformationMatrixFromPointClouds(
        const ccPointCloud &source,
        const ccPointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    ccPointCloud pcd = source;
    if (transformation.isIdentity() == false) {
        pcd.Transform(transformation);
    }
    RegistrationResult result;
    geometry::KDTreeFlann target_kdtree(target);
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, target_kdtree, max_correspondence_distance,
            transformation);
 
    // write q^*
    // see http://redwood-data.org/indoor/registration.html
    // note: I comes first in this implementation
    Eigen::Matrix6d GTG = Eigen::Matrix6d::Zero();
#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        Eigen::Matrix6d GTG_private = Eigen::Matrix6d::Zero();
        Eigen::Vector6d G_r_private = Eigen::Vector6d::Zero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int c = 0; c < int(result.correspondence_set_.size()); c++) {
            int t = result.correspondence_set_[c](1);
            double x = target.getEigenPoint(t)(0);
            double y = target.getEigenPoint(t)(1);
            double z = target.getEigenPoint(t)(2);
            G_r_private.setZero();
            G_r_private(1) = z;
            G_r_private(2) = -y;
            G_r_private(3) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = -z;
            G_r_private(2) = x;
            G_r_private(4) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = y;
            G_r_private(1) = -x;
            G_r_private(5) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
        }
#ifdef _OPENMP
#pragma omp critical(GetInformationMatrixFromPointClouds)
#endif
        { GTG += GTG_private; }
#ifdef _OPENMP
    }
#endif
    return GTG;
}
 
}  // namespace registration
}  // namespace pipelines
}  // namespace cloudViewer