PointToModelICP.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstddef>
#include <iterator>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <pcl/PointIndices.h>
#include <pcl/common/transforms.h>
#include <pcl/correspondence.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_var_trimmed.h>
#include <pcl/registration/default_convergence_criteria.h>
#include <pcl/registration/transformation_estimation_2D.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "models.h"
#include "wykobi_utils.h"
 
namespace armarx
{
 
    struct RegistrationResult
    {
        Eigen::Isometry3f transform = Eigen::Isometry3f::Identity();
        int iterations{0};
        pcl::Correspondences correspondences;
 
        pcl::registration::DefaultConvergenceCriteria<float> convergenceCriteria;
 
        Eigen::Matrix2f covariance = Eigen::Matrix2f::Identity();
 
    public:
        RegistrationResult() : convergenceCriteria(iterations, transform.matrix(), correspondences)
        {
        }
    };
 
    inline Eigen::Vector3f
    toEigen(const pcl::PointXYZ& pt)
    {
        return Eigen::Vector3f{pt.x, pt.y, pt.z};
    }
 
    template <typename PointT = pcl::PointXYZ>
    class PointToModelICP
    {
    public:
        PointToModelICP<PointT>(const wykobi::Model& model) :
            model(model),
            tmpCloudWithPrior(new pcl::PointCloud<PointT>),
            tmpCloudProjected(new pcl::PointCloud<PointT>)
        {
        }
 
        void
        setInputCloud(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
        {
            inputCloud = cloud;
        }
 
        RegistrationResult
        align()
        {
            if (inputCloud->empty())
            {
                ARMARX_WARNING << "Input point cloud is empty!";
                return {};
            }
 
            RegistrationResult result;
 
            // visualizer.addPointCloud(cloudSource, "cloudSource");
 
            // visualizer.addPointCloud(cloudSource, "projection");
 
            do
            {
                ARMARX_INFO << result.transform.matrix();
                result.transform = alignStep(result.transform, result.correspondences);
                result.iterations++;
            } while (not result.convergenceCriteria.hasConverged());
 
            ARMARX_INFO << "Converged after " << result.iterations << " iterations.";
 
            result.covariance = computeCovariance(model.polygon, result);
 
            return result;
        }
 
    protected:
        Eigen::Matrix2f
        computeCovariance(const decltype(wykobi::Model::polygon)& polygon,
                          const RegistrationResult& result)
        {
            // use a tmp variable to avoid unnecessary memory allocation
            auto& tmpCloud = tmpCloudWithPrior;
 
            pcl::transformPointCloud(*inputCloud, *tmpCloud, result.transform);
            wykobi::project(*tmpCloud, polygon);
 
            pcl::PointIndices cloudIndices;
            cloudIndices.indices.reserve(result.correspondences.size());
            std::transform(result.correspondences.begin(),
                           result.correspondences.end(),
                           std::back_inserter(cloudIndices.indices),
                           [](const auto& c) { return c.index_query; });
 
            Eigen::Matrix3f covarianceMatrix;
            pcl::computeCovarianceMatrix(*tmpCloud, cloudIndices, covarianceMatrix);
 
            return covarianceMatrix.block<2, 2>(0, 0);
        }
 
        Eigen::Isometry3f
        alignStep(const Eigen::Isometry3f& prior, pcl::Correspondences& correspondences)
        {
            correspondences.clear();
            correspondences.reserve(inputCloud->size());
 
            // use the prior to transform point cloud into a promising configuration
            pcl::transformPointCloud(*inputCloud, *tmpCloudWithPrior, prior);
 
            *tmpCloudProjected = *tmpCloudWithPrior;
 
            // project the points onto the polygon
            wykobi::project(*tmpCloudProjected, model.polygon);
 
            ARMARX_INFO << tmpCloudProjected->size() << " correspondences";
 
            // To my future me: zip -> transform -> if
            for (size_t i = 0; i < tmpCloudProjected->size(); i++)
            {
                const auto& ptWithPrior = tmpCloudWithPrior->points.at(i);
                const auto& ptProjected = tmpCloudProjected->points.at(i);
 
                const float distance = (toEigen(ptWithPrior) - toEigen(ptProjected)).norm();
 
                const int qi = static_cast<int>(i);
                correspondences.emplace_back(qi, qi, distance);
            }
 
            ARMARX_CHECK(inputCloud->size() == tmpCloudProjected->size());
 
            // reject outliers
            // pcl::registration::CorrespondenceRejectorMedianDistance correspondenceCalculator;
            // pcl::registration::CorrespondenceRejectorVarTrimmed correspondenceCalculator;
            pcl::registration::CorrespondenceRejectorDistance correspondenceCalculator;
            correspondenceCalculator.setMaximumDistance(.1);
 
            pcl::Correspondences remainingCorrespondences;
 
            correspondenceCalculator.setInputSource<PointT>(tmpCloudWithPrior);
            correspondenceCalculator.setInputTarget<PointT>(tmpCloudProjected);
            correspondenceCalculator.getRemainingCorrespondences(correspondences,
                                                                 remainingCorrespondences);
 
            const std::size_t nCorrespondencesBefore = correspondences.size();
            const std::size_t nCorrespondencesAfter = remainingCorrespondences.size();
 
            ARMARX_INFO << "Rejected " << (nCorrespondencesBefore - nCorrespondencesAfter)
                        << " outliers.";
            // ARMARX_INFO << "Distance " << correspondenceCalculator.getTrimmedDistance();
 
            correspondences = remainingCorrespondences;
 
            // estimate transformation from non-projected points (without prior) to projected ones
            Eigen::Isometry3f localTrafo = Eigen::Isometry3f::Identity();
            estimator.estimateRigidTransformation(
                *inputCloud, *tmpCloudProjected, correspondences, localTrafo.matrix());
 
            return localTrafo;
        }
 
    private:
        int iterations{0};
        const wykobi::Model model;
 
        typename pcl::PointCloud<PointT>::ConstPtr inputCloud;
        typename pcl::PointCloud<PointT>::Ptr tmpCloudWithPrior;
        typename pcl::PointCloud<PointT>::Ptr tmpCloudProjected;
 
        typename pcl::registration::DefaultConvergenceCriteria<float>::Ptr convergenceCriteria;
 
        pcl::registration::TransformationEstimation2D<PointT, PointT> estimator;
    };
 
    // interactive
    // pcl::visualization::PCLVisualizer &visualizer
 
    // visualizer.updatePointCloud(cloudTarget, "projection");
    // // visualizer.updatePointCloudPose("cloudSource", transformation);
 
    // visualizer.spinOnce(1000);
 
} // namespace armarx