GeneralizedICP.h

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.
// ----------------------------------------------------------------------------
 
#pragma once
 
#include <Eigen/Core>
#include <memory>
 
#include "pipelines/registration/Registration.h"
#include "pipelines/registration/RobustKernel.h"
#include "pipelines/registration/TransformationEstimation.h"
 
namespace cloudViewer {
namespace pipelines {
namespace registration {
 
class RegistrationResult;
 
class TransformationEstimationForGeneralizedICP
    : public TransformationEstimation {
public:
    ~TransformationEstimationForGeneralizedICP() override = default;
 
    TransformationEstimationType GetTransformationEstimationType()
            const override {
        return type_;
    };
    explicit TransformationEstimationForGeneralizedICP(
            double epsilon = 1e-3,
            std::shared_ptr<RobustKernel> kernel = std::make_shared<L2Loss>())
        : epsilon_(epsilon), kernel_(std::move(kernel)) {}
 
public:
    double ComputeRMSE(const ccPointCloud &source,
                       const ccPointCloud &target,
                       const CorrespondenceSet &corres) const override;
 
    Eigen::Matrix4d ComputeTransformation(
            const ccPointCloud &source,
            const ccPointCloud &target,
            const CorrespondenceSet &corres) const override;
 
public:
    // Small constant representing covariance along the normal.
    double epsilon_ = 1e-3;
 
    std::shared_ptr<RobustKernel> kernel_ = std::make_shared<L2Loss>();
 
private:
    const TransformationEstimationType type_ =
            TransformationEstimationType::GeneralizedICP;
};
 
//  A. Segal, D .Haehnel, S. Thrun
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());
 
}  // namespace registration
}  // namespace pipelines
}  // namespace cloudViewer