CscSlamEngine.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 CscSlamEngine_h
#define CscSlamEngine_h
 
#include <CscCommon.h>
#include <mutex>
 
#include <queue>
#include <mrpt/math/TPose2D.h>
#include <Lidar/CscEntityPositionManager.h>
#include <Axiomes/Csc2dTypes.h>
namespace mrpt::maps {
class COccupancyGridMap2D;
}
namespace mrpt::obs {
class CObservation2DRangeScan;
}
namespace mrpt::slam {
class CMonteCarloLocalization2D;
class CMetricMapBuilderRBPF;
}
using namespace conscience_utils::logging;
using mrpt::math::TPoint2D, mrpt::math::TPose2D, mrpt::obs::CObservation2DRangeScan, mrpt::maps::COccupancyGridMap2D, mrpt::slam::CMonteCarloLocalization2D, mrpt::slam::CMetricMapBuilderRBPF;
using std::string, std::recursive_mutex, std::vector, std::map, std::pair, std::lock_guard, std::queue;
namespace fs = std::filesystem;
using namespace conscience_core::axiomes;
 
namespace conscience_core::lidar {
 
class CscLidarEngine;
struct LidarValues;
 
enum class CscSlamCellOccupancyStatus : uint8_t {
    SlamCellOccupancyStatus_Unknown,
    SlamCellOccupancyStatus_Free,
    SlamCellOccupancyStatus_Occupied
};
 
class CscSlamEngine {
public:
    static constexpr double defaultCellOccupiedThreshold = 0.14;
 
    struct PhantomCluster {
        vector<pair<int, int>> cells; // (i,j) indices
        double overlapRatio{0.0};     // supported/occCount
        bool duplicate;
    };
    struct PhantomClusterTracked {
        std::vector<std::pair<int, int>> cells; // occupied cells in cluster
        int hits = 0;                           // number of times observed
        std::chrono::steady_clock::time_point lastHit;
    };
 
    CscSlamEngine(const string &entitySerialNumber, bool isSimulatedEntity, CscEntityPositionManager *positionManager, CscLidarEngine *lidarEngine);
    ~CscSlamEngine();
 
    void stop();
    void start(CscPoint3dOriented *initialPosition = nullptr);
 
    const CscPoint3d &getRobotStartPosition() const;
    const CscQuaternion &getRobotStartOrientation() const;
    CscPoint3dOriented *getRobotPose3d() const;
 
    fs::path saveCartography();
    bool loadCartography(const fs::path &cartographyDir);
    bool loadLatestCartography();
    bool hasLoadedCartography();
 
    bool isStarted() const;
    bool isStartedAndReady() const;
 
    void consumeLidarValues();
 
    void forceLocalisationPosition(double xInMeters, double zInMeters, double headingInRad);
 
    struct MapToImageParams {
        optional<TPose2D> robotPose = nullopt;
 
        map<TPoint2D, Vec3> dots;
        map<TPoint2D, Vec3> crosses;
        vector<TPose2D> path;
        vector<CscSlamEngine::PhantomCluster> clusters;
        vector<CscSlamEngine::PhantomClusterTracked> clustersTracked;
        vector<uint8_t> cellsVisibility;
 
        const CObservation2DRangeScan *lidarScan = nullptr;
    };
    void saveMrptMapToPng(const string &filename,
                          const MapToImageParams &params);
 
    CscPoint3d *transformToWorld3dPosition(const TPoint2D &pointSlam2d) const;
    vector<CscPoint3d *> getMapAsWorldPoints(double threshold = defaultCellOccupiedThreshold) const;
 
    inline constexpr CscSlamCellOccupancyStatus getCellStatus(float cellValue, double occupiedThreshold = defaultCellOccupiedThreshold) {
        if (cellValue < 0.0f || std::abs(cellValue - 0.5f) < 1e-6f) { // Tolérance pour floating point
            return CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Unknown;
        }
        if (cellValue <= occupiedThreshold) {
            return CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Occupied;
        }
        return CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Free;
    }
 
    void clearCartography();
 
    mrpt::math::TPose2D getRobotPose() const;
    ptr<CObservation2DRangeScan> getLidarScan() const;
    ptr<COccupancyGridMap2D> getSLAMMap() const;
 
private:
    class PhantomClusterTracker {
    public:
        PhantomClusterTracker(double overlapThreshold = 0.3, int maxAgeSeconds = 300);
 
        // Update tracker with new clusters from this tick
        void update(const std::vector<PhantomCluster> &newClusters);
 
        const std::vector<PhantomClusterTracked> &tracked() const;
 
        string getSummary() const;
 
    private:
        std::vector<PhantomClusterTracked> trackedClusters;
        const double overlapThreshold;
        const int maxAgeSeconds;
 
        // Check if clusters overlap enough to be considered the same
        bool clustersSimilar(
            const std::vector<std::pair<int, int>> &a,
            const std::vector<std::pair<int, int>> &b) const;
    };
 
    const string entitySerialNumber;
    const bool isSimulatedEntity;
    void consumeLidarValues(const LidarValues &lidarValues);
 
    CscPoint3d robotStartPosition;
    CscQuaternion robotStartOrientation;
    CscEntityPositionManager *positionManager = nullptr;
    CscLidarEngine *lidarEngine = nullptr;
 
    void clearSlamData();
 
    CMonteCarloLocalization2D *slamLocalization = nullptr;
 
    mutable recursive_mutex slamInstanceMutex;
    CMetricMapBuilderRBPF *slamInstance = nullptr;
    ptr<COccupancyGridMap2D> slamMap = nullptr;
    bool slamInitialized = false;
    double robotStartYaw = 0;
    void initMrptSlam();
    queue<const LidarValues *> lidarValuesQueue;
 
    unique_ptr<CscLogger> logger;
 
    unsigned long long lastLidarValuesTimestamp = 0;
 
    mutable mutex lastRangeScanMutex;
    ptr<CObservation2DRangeScan> lastRangeScan = nullptr;
    std::chrono::steady_clock::time_point lastPngExportTime = std::chrono::steady_clock::now();
    double odoLastX = 0;
    double odoLastZ = 0;
    bool started = false;
    bool odoDiffInitialized = false;
    unsigned int lastMapSize = 0;
    unsigned int identicalCount = 0;
    std::chrono::steady_clock::time_point lastTimeSlamOdo = std::chrono::steady_clock::now();
 
    struct CscSlamMapDiscrepancyState {
        size_t discrepantScanCount = 0;
        size_t ticksSinceDiscrepancyStart = 0;
        double refMean = 0;
        double refStd = 0;
        size_t initializationTicksCount = 0;
    };
    CscSlamMapDiscrepancyState discrepancyState;
 
    struct PhantomClusterParams {
        double maxDistanceMeters = 8;
 
        double minOverlapRatio = 0.2;
 
        int supportRadius = 2;
 
        int clusterThresh = 10;
 
        int maxClusterShiftCells = 14;
 
        int gapTolerance = 2;
 
        int mergeDistanceCells = 3;
    };
    struct PhantomClusterResult {
        bool hasPhantoms{false};
        vector<PhantomCluster> clusters;
        vector<uint8_t> visible;
    };
    PhantomClusterResult detectPhantomClusters(
        const COccupancyGridMap2D &slamMap,
        const CObservation2DRangeScan &scan,
        const TPose2D &pose,
        const PhantomClusterParams &params,
        double occThreshold = defaultCellOccupiedThreshold);
    PhantomClusterResult lastClustersResult;
    PhantomClusterTracker phantomClusterTracker;
 
    CscPoint3dOriented *initialPosition = nullptr;
 
protected:
};
 
/* MRPT HELPERS - START */
 
inline double distancePoints(const TPoint2D &pointA, const TPoint2D &pointB) {
    return std::hypot(pointA.x - pointB.x, pointA.y - pointB.y);
}
inline double distancePoints(const TPose2D &poseA, const TPose2D &poseB) {
    return distancePoints(TPoint2D(poseA), TPoint2D(poseB));
}
inline CscPoint2d toMrptReferenceFrame(const CscPoint3d &point) {
    return CscPoint2d(point.x / 100.0 /*to meters*/, point.z / 100.0 /*to meters*/);
}
 
string mrptPoseToJson(const TPose2D &pose);
string mrptPointToJson(const TPoint2D &point);
CscPoint2dOriented mrptPoseToCscPointOriented(const TPose2D &pose);
CscPoint2d mrptPointToCscPoint(const TPoint2D &point);
 
/* MRPT HELPERS - END */
 
namespace directions {
 
struct CscAvailableDirection {
    double relYaw;
    string name;
};
 
static const CscAvailableDirection AVAILABLE_DIRECTION_FORWARD = {0.0, "forward"};
static const CscAvailableDirection AVAILABLE_DIRECTION_LEFT = {M_PI_2, "left"};
static const CscAvailableDirection AVAILABLE_DIRECTION_RIGHT = {-M_PI_2, "right"};
static const CscAvailableDirection AVAILABLE_DIRECTION_BACK = {M_PI, "back"};
static const vector<CscAvailableDirection> AVAILABLE_ALL_DIRECTIONS = {
    AVAILABLE_DIRECTION_FORWARD,
    AVAILABLE_DIRECTION_LEFT,
    AVAILABLE_DIRECTION_RIGHT,
    AVAILABLE_DIRECTION_BACK};
 
optional<pair<int, double>> getBestDirectionForScan(
    CObservation2DRangeScan *scan,
    const vector<CscAvailableDirection> &authorizedDirections,
    double robotWidth, double robotLength, map<int, double> *computedDirections = nullptr);
void saveScanWithBestDirectionAsPng(
    const mrpt::obs::CObservation2DRangeScan &scan,
    double bestYaw,
    const std::string &bestDirName,
    const fs::path &filename,
    double robotWidth, double robotLength);
 
double estimateFreeDistanceForLidarScan(
    double directionYawRads,
    const mrpt::obs::CObservation2DRangeScan &scan,
    double robotWidthMeters,
    double robotLengthMeters,
    double minClusterDistanceMeters = 0.10,
    int minClusterSize = 3,
    double maxDepthMeters = 2.0);
 
}
 
}
 
#endif