MapRegistration.cpp

Go to the documentation of this file.

#include "MapRegistration.h"
 
#include <algorithm>
#include <deque>
#include <iostream>
#include <iterator>
#include <string>
#include <utility>
 
#include <Eigen/Core>
 
#include <pcl/PointIndices.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/correspondence.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/pcl_macros.h>
#include <pcl/registration/impl/transformation_estimation_2D.hpp>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
 
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>
 
#include "ArmarXCore/core/util/StringHelpers.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/localization_and_mapping/cartographer_adapter/map_registration/wykobi_types.h>
#include <armarx/localization_and_mapping/cartographer_adapter/types.h>
#include <armarx/localization_and_mapping/cartographer_adapter/utils.h>
 
#include "../InterpolatingTimeQueue.h"
#include "PointToModelICP.h"
#include "models.h"
#include "pcl_filter.h"
#include "wykobi_utils.h"
#include <cartographer/mapping/map_builder_interface.h>
 
namespace armarx::cartographer
{
 
    template <typename PointT>
    MapRegistration::ModelCorrection
    align(const pcl::PointCloud<PointT>& cloudSource,
          const wykobi::Model& model2,
          const Eigen::Isometry3f& world_T_map)
    {
        typename pcl::PointCloud<PointT>::Ptr sharedCloud(new pcl::PointCloud<PointT>);
 
        // the world_T_map trafo can be seen as a prior for
        pcl::transformPointCloud(cloudSource, *sharedCloud, world_T_map);
 
        for (auto& pt : sharedCloud->points)
        {
            pt.x /= 1000.F; // to m
            pt.y /= 1000.F; // to m
            pt.z /= 1000.F; // to m
        }
 
        wykobi::Model model = model2;
        model.center /= 1000.F;
 
        std::for_each(model.polygon.begin(),
                      model.polygon.end(),
                      [](auto& p)
                      {
                          p.x /= 1000.F;
                          p.y /= 1000.F;
                      });
 
 
        typename pcl::PointCloud<PointT>::Ptr polygonCloud(new pcl::PointCloud<PointT>);
        std::transform(model.polygon.begin(),
                       model.polygon.end(),
                       std::back_inserter(polygonCloud->points),
                       wykobi::fromWykobi);
 
        for (auto& pt : polygonCloud->points)
        {
            pt.x /= 1000.F; // to m
            pt.y /= 1000.F; // to m
            pt.z /= 1000.F; // to m
        }
 
        armarx::PointMaxRangeFilter<pcl::PointXYZ> rangeFilter(0.5F, model.center);
        rangeFilter.applyFilter(*sharedCloud);
 
 
        PointToModelICP<pcl::PointXYZ> icp(model);
        icp.setInputCloud(sharedCloud);
        const auto result = icp.align();
        /*const*/ auto trafo = result.transform;
 
        typename pcl::PointCloud<PointT>::Ptr registeredPolygonCloud(new pcl::PointCloud<PointT>);
        pcl::transformPointCloud(*polygonCloud, *registeredPolygonCloud, trafo.inverse());
 
        trafo.translation() *= 1000.F;
 
        return {.correction = trafo,
                .weight = static_cast<float>(result.correspondences.size()) / sharedCloud->size(),
                .covariance = result.covariance};
    }
 
    MapRegistration::MapRegistration(
        std::unique_ptr<::cartographer::mapping::MapBuilderInterface> mapBuilderPtr) :
        mapBuilder(std::move(mapBuilderPtr)),
        optimizedGraphData(utils::optimizedGraphData(*mapBuilder))
    {
    }
 
    MapRegistration::~MapRegistration() = default;
 
    simox::math::InterpolatingTimeQueue<simox::math::PoseStamped>
    MapRegistration::robotPoseQueue() const
    {
        simox::math::InterpolatingTimeQueue<simox::math::PoseStamped> queue;
 
        for (const OptimizedNodeData& data : optimizedGraphData.nodes_data)
        {
            Eigen::Isometry3f pose = data.global_pose;
            pose.translation() *= 1000.F; // toMM
 
            simox::math::PoseStamped ps{.pose = pose, .timestamp = data.timestamp};
            queue.insert(std::make_shared<const simox::math::PoseStamped>(std::move(ps)));
        }
 
        return queue;
    }
 
    MapRegistration::Clusters
    MapRegistration::detectClusters(const PointCloud::ConstPtr& cloud) const
    {
        constexpr float toMM = 1000.F;
 
        // Create the filtering object: downsample the dataset using a leaf size of 1cm
        pcl::VoxelGrid<pcl::PointXYZ> vg;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        vg.setInputCloud(cloud);
        constexpr float leafSize = 0.005f * toMM;
        vg.setLeafSize(leafSize, leafSize, leafSize);
        vg.filter(*cloud_filtered);
        std::cout << "PointCloud after filtering has: " << cloud_filtered->size() << " data points."
                  << std::endl; //*
 
        // Creating the KdTree object for the search method of the extraction
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
        tree->setInputCloud(cloud_filtered);
 
        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        ec.setClusterTolerance(0.01 * toMM); // 2cm
        ec.setMinClusterSize(100);
        // ec.setMaxClusterSize(25000);
        ec.setSearchMethod(tree);
        ec.setInputCloud(cloud_filtered);
        ec.extract(cluster_indices);
 
        pcl::PCDWriter writer;
 
        MapRegistration::Clusters clusters;
 
        int j = 0;
        for (auto it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCluster(new pcl::PointCloud<pcl::PointXYZ>);
            for (const auto& idx : it->indices)
            {
                cloudCluster->push_back((*cloud_filtered)[idx]);
            }
 
            cloudCluster->width = cloudCluster->size();
            cloudCluster->height = 1;
            cloudCluster->is_dense = true;
 
            std::cout << "PointCloud representing the Cluster: " << cloudCluster->size()
                      << " data points." << std::endl;
            std::stringstream ss;
            ss << "/tmp/cloud_cluster_" << j << ".pcd";
            writer.write<pcl::PointXYZ>(ss.str(), *cloudCluster, false); //*
            j++;
 
            clusters.emplace_back(cloudCluster);
        }
 
        return clusters;
    }
 
    const MapRegistration::Cluster&
    findNearestCluster(const Eigen::Vector3f& position,
                       const MapRegistration::Clusters& clusters,
                       const Eigen::Isometry3f& world_T_map)
    {
        std::vector<float> distances;
        distances.reserve(clusters.size());
 
        std::transform(clusters.begin(),
                       clusters.end(),
                       std::back_inserter(distances),
                       [&](const MapRegistration::Cluster& cluster) -> float
                       { return (world_T_map * toEigen(cluster.centroid) - position).norm(); });
 
        const std::size_t idx =
            std::distance(distances.begin(), std::min_element(distances.begin(), distances.end()));
 
        return clusters.at(idx);
    }
 
    MapRegistration::Associations
    MapRegistration::matchModelsToClusters(const std::vector<wykobi::Model>& models,
                                           const Clusters& clusters,
                                           const Eigen::Isometry3f& world_T_map) const
    {
        Associations assocs;
 
        for (const wykobi::Model& model : models)
        {
            const Cluster& cluster = findNearestCluster(model.center, clusters, world_T_map);
 
            assocs.push_back(Association{.cluster = cluster, .model = model});
        }
 
        return assocs;
    }
 
    MapRegistration::ModelCorrection
    MapRegistration::alignModelToCluster(const wykobi::Model& model,
                                         const Cluster& cluster,
                                         const Eigen::Isometry3f& world_T_map) const
    {
        return align<>(*cluster.points, model, world_T_map);
    }
 
    inline pcl::PointXYZ
    toPCL(const Eigen::Vector3f& v)
    {
        pcl::PointXYZ p;
 
        p.x = v.x();
        p.y = v.y();
        p.z = v.z();
 
        return p;
    }
 
    MapRegistration::ModelCorrection
    MapRegistration::computeCorrection(const MapRegistration::Associations& associations)
    {
        PointCloud worldPoints;
        PointCloud mapPoints;
 
        for (const Association& association : associations)
        {
            worldPoints.push_back(toPCL(association.model.center));
            mapPoints.push_back(association.cluster.centroid);
        }
 
        Eigen::Isometry3f world_T_map = Eigen::Isometry3f::Identity();
 
        ::pcl::registration::TransformationEstimation2D<Point, Point> transformEstimator;
        transformEstimator.estimateRigidTransformation(
            worldPoints, mapPoints, world_T_map.matrix());
 
        ModelCorrection combinedCorrection;
        combinedCorrection.correction = world_T_map;
 
        return combinedCorrection;
    }
 
    MapRegistration::ModelCorrection
    MapRegistration::computeCombinedCorrection(
        const std::vector<wykobi::Model>& models,
        const std::vector<ModelCorrection>& modelCorrections) const
    {
        // desire: obtain world -> map tranformation in Least Squares sense
 
        // first shot: weighted mean
 
        ARMARX_CHECK(models.size() == modelCorrections.size());
 
        PointCloud worldPoints;
        PointCloud mapPoints;
 
        for (size_t i = 0; i < models.size(); i++)
        {
            const auto& modelCorrection = modelCorrections.at(i);
            const auto& model = models.at(i);
 
            const auto& world_P_model = model.center;
            const Eigen::Vector3f map_P_model = modelCorrection.correction * world_P_model;
 
            worldPoints.push_back(toPCL(world_P_model));
            mapPoints.push_back(toPCL(map_P_model));
 
            // modelCorrection.correction.translation().head<2>();
            // ori += modelCorrection.weight * simox::math::mat3f_to_rpy(modelCorrection.correction.linear()).z();
        }
 
        Eigen::Isometry3f world_T_map = Eigen::Isometry3f::Identity();
 
        pcl::registration::TransformationEstimation2D<Point, Point> transformEstimator;
        transformEstimator.estimateRigidTransformation(
            worldPoints, mapPoints, world_T_map.matrix());
 
        // pos /= overallWeight;
        // ori /= overallWeight;
 
        ModelCorrection combinedCorrection;
        // combinedCorrection.correction = Eigen::Isometry3f::Identity();
        // combinedCorrection.correction.translation().head<2>() = pos;
        // combinedCorrection.correction.linear() = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
        combinedCorrection.correction = world_T_map;
 
        return combinedCorrection;
    }
 
    // void drawPolygon(pcl::visualization::PCLVisualizer& visualizer,
    //                  const wykobi::Polygon& polygon,
    //                  const std::string& name)
    // {
    //     typename pcl::PointCloud<pcl::PointXYZ>::Ptr polygonCloud(
    //         new pcl::PointCloud<pcl::PointXYZ>);
 
    //     std::transform(polygon.begin(),
    //                    polygon.end(),
    //                    std::back_inserter(polygonCloud->points),
    //                    wykobi::fromWykobi);
 
    //     const Eigen::Isometry3f trafo(Eigen::Translation3f(0, 0, 0.1));
 
    //     pcl::transformPointCloud(*polygonCloud, *polygonCloud, trafo);
 
    //     // visualizer.addPolygon<pcl::PointXYZ>(polygonCloud, 1.0, 0., 0., "polygon_" + name);
    // }
 
    void
    MapRegistration::visualizeResult(
        const MapRegistration::PointCloud::ConstPtr& cloud,
        const std::vector<wykobi::Model>& models,
        const MapRegistration::ModelCorrection& combinedCorrection) const
    {
 
        // pcl::visualization::PCLVisualizer visualizer("Viewer2");
        // visualizer.addCoordinateSystem(1.0);
        // // visualizer.addPointCloud(sharedCloud, "full_cloud");
 
        // int i = 0;
        // for (const auto& model : models)
        // {
        //     std::string name = std::to_string(i++);
        //     drawPolygon(visualizer, model.polygon, name);
        // }
 
        // pcl::visualization::PointCloudColorHandlerCustom<Point> colorHandler(0., 1., 0.);
 
        // // visualizer.addPointCloud(cloud, colorHandler, "full_cloud");
 
        // PointCloud::Ptr alignedCloud(new PointCloud);
        // pcl::transformPointCloud(*cloud, *alignedCloud, combinedCorrection.correction);
        // // visualizer.addPointCloud(alignedCloud, "polygon_aligned");
 
        // visualizer.spin();
    }
 
    TimeRange
    MapRegistration::mapTimeRange() const
    {
        ARMARX_CHECK(optimizedGraphData.nodes_data.size() > 1);
 
        return {.min = IceUtil::Time::microSeconds(optimizedGraphData.nodes_data.front().timestamp),
                .max = IceUtil::Time::microSeconds(optimizedGraphData.nodes_data.back().timestamp)};
    }
 
    std::vector<Eigen::Isometry3f>
    MapRegistration::robotPoses() const
    {
        const auto robotPoseInterpolatingQueue = robotPoseQueue();
        const auto robotPoseQ = robotPoseInterpolatingQueue.rawQueue();
 
        std::vector<Eigen::Isometry3f> robotPoses;
        robotPoses.reserve(robotPoseQ.size());
        std::transform(robotPoseQ.begin(),
                       robotPoseQ.end(),
                       std::back_inserter(robotPoses),
                       [](const auto& poseStampedPtr) { return poseStampedPtr->pose; });
 
        return robotPoses;
    }
 
    inline cv::Point2f
    toCV(const pcl::PointXYZ& pt)
    {
        return cv::Point2f(pt.x, pt.y);
    }
 
    inline Eigen::Vector2f
    toEigen(const cv::Point2f& pt)
    {
        return Eigen::Vector2f{pt.x, pt.y};
    }
 
    void
    MapRegistration::computeBoundingBox(const PointCloud::ConstPtr& cloud)
    {
        std::vector<cv::Point2f> points;
        points.reserve(cloud->size());
        std::transform(
            cloud->points.begin(), cloud->points.end(), std::back_inserter(points), toCV);
 
        cv::RotatedRect rect = cv::minAreaRect(points);
 
        const auto center = toEigen(rect.center);
        const auto extends = toEigen(rect.size);
 
        boundingBox = {
            .center = center.head<2>(), .angle = DEG2RAD(rect.angle), .extends = extends};
    }
 
    MapRegistration::Cluster::Cluster(const PointCloud::Ptr& points) : points(points)
    {
        pcl::computeCentroid(*points, centroid);
    }
} // namespace armarx::cartographer