CscPointCloudMatcher.h

Go to the documentation of this file.

//
//  CscSlamEngine.hpp
//  ConscienceRobotManager
//
//  Created by Iliès Zaoui on 23/10/2022.
//  Copyright © 2022 Conscience Robotics. All rights reserved.
//
 
#ifndef CscPointCloudMatcher_h
#define CscPointCloudMatcher_h
 
#include <array>
#include <map>
#include <string>
#include <vector>
 
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
 
#include "Axiomes/Csc3dTypes.h"
 
#include "Util/conscience_log.h"
 
using std::cout, std::endl, std::string, std::pair, std::vector, std::to_string;
 
using namespace conscience_utils::logging;
using namespace conscience_core::axiomes;
 
namespace conscience_core::lidar {
 
class CscPointCloudMatcher {
public:
    struct Result {
        bool success;
        float score = 0;
 
        CscPoint3dOriented *transformation = nullptr;
    };
 
    CscPointCloudMatcher();
 
    const Result compare(const vector<const CscPoint3d *> &pointsReference, const vector<const CscPoint3d *> &points, bool XZTo2DOnly) const;
    float calculateLikelihood(const vector<const CscPoint3d *> &pointsReference, const vector<const CscPoint3d *> &points, bool XZTo2DOnly);
 
private:
    unique_ptr<CscLogger> logger;
 
    // ICP configuration method
    void configureICP(pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> &icp);
 
    // Conversion methods
    pcl::PointCloud<pcl::PointXYZ>::Ptr convertToPCLPointCloud(const vector<const CscPoint3d *> &points);
    vector<const CscPoint3d *> convertToCscPoint3dVector(const pcl::PointCloud<pcl::PointXYZ> &pclCloud);
 
    // Filtering method
    void filterPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                          const Eigen::Vector4f &minPoint,
                          const Eigen::Vector4f &maxPoint);
    pair<const Eigen::Vector4f, const Eigen::Vector4f> computeBoxAroundReference(pcl::PointXYZ &reference, double maxDistanceForFilter);
};
 
}
 
#endif