MapRegistration.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 <array>
#include <cmath>
#include <cstdint>
#include <filesystem>
#include <memory>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include "RobotAPI/libraries/armem/core/Time.h"
 
#include <armarx/localization_and_mapping/cartographer_adapter/types.h>
 
#include "../InterpolatingTimeQueue.h"
#include "models.h"
 
namespace cartographer::mapping
{
    class MapBuilderInterface;
}
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    struct TimeRange
    {
        IceUtil::Time min;
        IceUtil::Time max;
 
        IceUtil::Time
        duration() const noexcept
        {
            return max - min;
        }
    };
 
    struct RotatedRect
    {
        Eigen::Vector2f center;
        float angle;
        Eigen::Vector2f extends;
 
        std::array<Eigen::Vector2f, 4>
        boundary() const
        {
            const Eigen::Affine2f centerPose =
                Eigen::Translation2f(center) * Eigen::Rotation2Df(angle);
 
            const Eigen::Vector2f e2 = extends / 2.F;
 
            return {centerPose * Eigen::Vector2f{e2.x(), e2.y()},
                    centerPose * Eigen::Vector2f{-e2.x(), e2.y()},
                    centerPose * Eigen::Vector2f{-e2.x(), -e2.y()},
                    centerPose * Eigen::Vector2f{e2.x(), -e2.y()}};
        }
 
        Eigen::Isometry3f
        cornerPose(int i) const
        {
            const float cornerAngle = angle + (i - 2) * M_PI_2f32;
 
            Eigen::Isometry3f ref_T_corner = Eigen::Isometry3f::Identity();
            ref_T_corner.translation().template head<2>() = boundary().at(i);
            ref_T_corner.linear() =
                Eigen::AngleAxisf(cornerAngle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
            return ref_T_corner;
        }
    };
 
    class MapRegistration
    {
    public:
        MapRegistration(std::unique_ptr<::cartographer::mapping::MapBuilderInterface> mapBuilder);
 
        ~MapRegistration();
 
        using Point = pcl::PointXYZ;
        using PointCloud = pcl::PointCloud<Point>;
 
        struct Cluster
        {
            PointCloud::Ptr points;
 
            pcl::PointXYZ centroid;
 
            Cluster(const PointCloud::Ptr& points);
        };
 
        using Clusters = std::vector<Cluster>;
 
        Clusters detectClusters(const PointCloud::ConstPtr& cloud) const;
 
        struct LocalPointCloud
        {
            Eigen::Affine2f pose;
            PointCloud pointCloud;
        };
 
        // PointCloud::ConstPtr readPointClouds() const;
 
        struct Association
        {
            const Cluster cluster;
            const wykobi::Model model;
        };
 
        using Associations = std::vector<Association>;
 
        Associations matchModelsToClusters(const std::vector<wykobi::Model>& models,
                                           const Clusters& clusters,
                                           const Eigen::Isometry3f& world_T_map) const;
 
        struct ModelCorrection
        {
            Eigen::Isometry3f correction;
 
            // fraction of points that support the hypothesis
            float weight;
 
            // covariance of the point-projection distance / vectors
            Eigen::Matrix2f covariance;
        };
 
        MapRegistration::ModelCorrection
        computeCorrection(const MapRegistration::Associations& associations);
 
        ModelCorrection alignModelToCluster(const wykobi::Model& model,
                                            const Cluster& cluster,
                                            const Eigen::Isometry3f& world_T_map) const;
 
        ModelCorrection
        computeCombinedCorrection(const std::vector<wykobi::Model>& models,
                                  const std::vector<ModelCorrection>& modelCorrections) const;
 
        void visualizeResult(const PointCloud::ConstPtr& cloud,
                             const std::vector<wykobi::Model>& models,
                             const ModelCorrection& combinedCorrection) const;
 
        TimeRange mapTimeRange() const;
 
        simox::math::InterpolatingTimeQueue<simox::math::PoseStamped> robotPoseQueue() const;
 
        std::vector<Eigen::Isometry3f> robotPoses() const;
 
        void computeBoundingBox(const PointCloud::ConstPtr& cloud);
 
        const RotatedRect&
        getBoundingBox() const
        {
            return boundingBox;
        }
 
    private:
        std::unique_ptr<::cartographer::mapping::MapBuilderInterface> mapBuilder;
 
        const OptimizedGraphData optimizedGraphData;
 
        RotatedRect boundingBox;
    };
 
} // namespace armarx::localization_and_mapping::cartographer_adapter