GeneralizedICP.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
// @author Ignacio Vizzo     [ivizzo@uni-bonn.de]
//
// Copyright (c) 2021 Ignacio Vizzo, Cyrill Stachniss, University of Bonn.
// ----------------------------------------------------------------------------
 
#include "pipelines/registration/GeneralizedICP.h"
 
#include <Eigen.h>
#include <Logging.h>
#include <ecvKDTreeSearchParam.h>
#include <ecvPointCloud.h>
 
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
 
namespace cloudViewer {
namespace pipelines {
namespace registration {
 
namespace {
 
inline Eigen::Matrix3d GetRotationFromE1ToX(const Eigen::Vector3d &x) {
    const Eigen::Vector3d e1{1, 0, 0};
    const Eigen::Vector3d v = e1.cross(x);
    const double c = e1.dot(x);
    if (c < -0.99) {
        // Then means that x and e1 are in the same direction
        return Eigen::Matrix3d::Identity();
    }
 
    const Eigen::Matrix3d sv = utility::SkewMatrix(v);
    const double factor = 1 / (1 + c);
    return Eigen::Matrix3d::Identity() + sv + (sv * sv) * factor;
}
 
std::shared_ptr<ccPointCloud> InitializePointCloudForGeneralizedICP(
        const ccPointCloud &pcd, double epsilon) {
    auto output = std::make_shared<ccPointCloud>(pcd);
    if (output->HasCovariances()) {
        utility::LogDebug("GeneralizedICP: Using pre-computed covariances.");
        return output;
    }
    if (output->hasNormals()) {
        utility::LogDebug("GeneralizedICP: Computing covariances from normals");
    } else {
        // Compute covariances the same way is done in the original GICP paper.
        utility::LogDebug("GeneralizedICP: Computing covariances from points.");
        output->EstimateNormals(
                cloudViewer::geometry::KDTreeSearchParamKNN(20));
    }
 
    output->covariances_.resize(output->size());
    const Eigen::Matrix3d C = Eigen::Vector3d(epsilon, 1, 1).asDiagonal();
#pragma omp parallel for schedule(static)
    for (int i = 0; i < (int)output->size(); i++) {
        const auto Rx = GetRotationFromE1ToX(output->getEigenNormal(i));
        output->covariances_[i] = Rx * C * Rx.transpose();
    }
    return output;
}
}  // namespace
 
double TransformationEstimationForGeneralizedICP::ComputeRMSE(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const CorrespondenceSet &corres) const {
    if (corres.empty()) {
        return 0.0;
    }
    double err = 0.0;
    for (const auto &c : corres) {
        const Eigen::Vector3d vs = source.getEigenPoint(c[0]);
        const Eigen::Matrix3d &Cs = source.covariances_[c[0]];
        const Eigen::Vector3d vt = target.getEigenPoint(c[1]);
        const Eigen::Matrix3d &Ct = target.covariances_[c[1]];
        const Eigen::Vector3d d = vs - vt;
        const Eigen::Matrix3d M = Ct + Cs;
        const Eigen::Matrix3d W = M.inverse().sqrt();
        err += d.transpose() * W * d;
    }
    return std::sqrt(err / (double)corres.size());
}
 
Eigen::Matrix4d
TransformationEstimationForGeneralizedICP::ComputeTransformation(
        const ccPointCloud &source,
        const ccPointCloud &target,
        const CorrespondenceSet &corres) const {
    if (corres.empty() || !target.HasCovariances() ||
        !source.HasCovariances()) {
        return Eigen::Matrix4d::Identity();
    }
 
    auto compute_jacobian_and_residual =
            [&](int i,
                std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
                std::vector<double> &r, std::vector<double> &w) {
                const Eigen::Vector3d vs = source.getEigenPoint(corres[i][0]);
                const Eigen::Matrix3d &Cs = source.covariances_[corres[i][0]];
                const Eigen::Vector3d vt = target.getEigenPoint(corres[i][1]);
                const Eigen::Matrix3d &Ct = target.covariances_[corres[i][1]];
                const Eigen::Vector3d d = vs - vt;
                const Eigen::Matrix3d M = Ct + Cs;
                const Eigen::Matrix3d W = M.inverse().sqrt();
 
                Eigen::Matrix<double, 3, 6> J;
                J.block<3, 3>(0, 0) = -utility::SkewMatrix(vs);
                J.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity();
                J = W * J;
 
                constexpr int n_rows = 3;
                J_r.resize(n_rows);
                r.resize(n_rows);
                w.resize(n_rows);
                for (size_t i = 0; i < n_rows; ++i) {
                    r[i] = W.row(i).dot(d);
                    w[i] = kernel_->Weight(r[i]);
                    J_r[i] = J.row(i);
                }
            };
 
    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    double r2 = -1.0;
    std::tie(JTJ, JTr, r2) =
            utility::ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
                    compute_jacobian_and_residual, (int)corres.size());
 
    bool is_success = false;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);
 
    return is_success ? extrinsic : Eigen::Matrix4d::Identity();
}
 
RegistrationResult RegistrationGeneralizedICP(
        const ccPointCloud &source,
        const ccPointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const TransformationEstimationForGeneralizedICP
                &estimation /* = TransformationEstimationForGeneralizedICP()*/,
        const ICPConvergenceCriteria
                &criteria /* = ICPConvergenceCriteria()*/) {
    return RegistrationICP(
            *InitializePointCloudForGeneralizedICP(source, estimation.epsilon_),
            *InitializePointCloudForGeneralizedICP(target, estimation.epsilon_),
            max_correspondence_distance, init, estimation, criteria);
}
 
}  // namespace registration
}  // namespace pipelines
}  // namespace cloudViewer