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 <filesystem>
#include <shared_mutex>
 
#include <mrpt/math/TPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/maps/CSimplePointsMap.h>
 
#include <Lidar/CscEntityPositionManager.h>
#include <Lidar/CscLidarEngine.h>
#include <Axiomes/Csc2dTypes.h>
 
#include <unordered_set>
 
namespace mrpt::maps {
class COccupancyGridMap2D;
}
 
namespace mrpt::obs {
class CObservation2DRangeScan;
}
 
namespace mrpt::slam {
class CMonteCarloLocalization2D;
class CMetricMapBuilderRBPF;
}
 
using namespace conscience_utils::logging;
 
using mrpt::maps::CSimplePointsMap;
using mrpt::math::TPoint2D, mrpt::math::TPose2D, mrpt::obs::CObservation2DRangeScan, mrpt::maps::COccupancyGridMap2D, mrpt::slam::CMonteCarloLocalization2D, mrpt::slam::CMetricMapBuilderRBPF;
using mrpt::poses::CPose3D;
using std::recursive_mutex, std::pair, std::lock_guard, std::queue;
using std::shared_lock, std::shared_mutex, std::unique_lock;
 
namespace fs = std::filesystem;
 
using namespace conscience_core::axiomes;
 
namespace conscience_core::lidar {
 
class CscLidarEngine;
class LidarValues;
class LidarValuesSnapshot;
 
enum class CscSlamCellOccupancyStatus : uint8_t {
    SlamCellOccupancyStatus_Unknown,
    SlamCellOccupancyStatus_Free,
    SlamCellOccupancyStatus_Occupied
};
 
struct CscSlam3dMapVoxelKey {
    int32_t x = 0;
    int32_t y = 0;
    int32_t z = 0;
 
    bool operator==(const CscSlam3dMapVoxelKey &other) const;
    bool operator!=(const CscSlam3dMapVoxelKey &other) const;
 
    void toMrptPoint(float voxelSizeMeters, float &outX, float &outY, float &outZ) const;
 
    void toCscPoint(float voxelSizeMeters, double &outX, double &outY, double &outZ) const;
 
    static CscSlam3dMapVoxelKey fromMrptPoint(
        double x,
        double y,
        double z,
        float voxelSizeMeters);
};
 
struct CscSlam3dMapVoxelKeyHash {
    size_t operator()(const CscSlam3dMapVoxelKey &key) const;
};
 
ENUM(CscSlam3dMapVoxelDiffType, SLAM3D_VOXEL_DIFF_TYPE_ADD, SLAM3D_VOXEL_DIFF_TYPE_REMOVE);
 
struct CscSlam3dMapVoxelDiff {
    CscSlam3dMapVoxelDiffType type = SLAM3D_VOXEL_DIFF_TYPE_ADD;
    CscSlam3dMapVoxelKey voxel;
};
 
ENUM(CscSlam3dMapVoxelChangesMode, SLAM3D_VOXEL_CHANGES_MODE_DIFFS, SLAM3D_VOXEL_CHANGES_MODE_FULL_REBUILD);
 
struct CscSlam3dMapVoxelChanges {
    CscSlam3dMapVoxelChangesMode mode;
    float voxelSizeMeters;
    vector<CscSlam3dMapVoxelDiff> diffs;
    vector<CscSlam3dMapVoxelKey> fullVoxels;
 
    static CscSlam3dMapVoxelChanges forDiffs(vector<CscSlam3dMapVoxelDiff> diffs, float voxelSizeMeters);
    static CscSlam3dMapVoxelChanges forFullRebuild(vector<CscSlam3dMapVoxelKey> diffs, float voxelSizeMeters);
 
    bool isFullRebuild() const;
    bool empty() const;
};
 
struct CscSlam3dMapVoxelChangesListener {
    function<void(const CscSlam3dMapVoxelChanges &changes)> callback;
    uint64_t id;
};
 
struct CscSlam3dMapVoxelGrid {
    using VoxelSet = std::unordered_set<CscSlam3dMapVoxelKey, CscSlam3dMapVoxelKeyHash>;
 
    const float voxelSizeMeters;
 
    const VoxelSet &getVoxelsUnsafe() const;
 
    explicit CscSlam3dMapVoxelGrid(float voxelSizeMeters = 0.035);
 
    CscSlam3dMapVoxelKey cscPointToVoxel(const CscPoint3d &point) const;
 
    bool contains(const CscSlam3dMapVoxelKey &voxel) const;
 
    std::optional<CscSlam3dMapVoxelDiff> addVoxel(const CscSlam3dMapVoxelKey &voxel);
    std::optional<CscSlam3dMapVoxelDiff> removeVoxel(const CscSlam3dMapVoxelKey &voxel);
 
    std::optional<CscSlam3dMapVoxelDiff> addPointConscience(const CscPoint3d &point);
    std::optional<CscSlam3dMapVoxelDiff> removePointConscience(const CscPoint3d &point);
 
    std::optional<CscSlam3dMapVoxelDiff> addPointMrpt(double x, double y, double z);
    std::optional<CscSlam3dMapVoxelDiff> removePointMrpt(double x, double y, double z);
 
    vector<CscSlam3dMapVoxelKey> getAllVoxelsCopy() const;
 
    void replaceWith(CscSlam3dMapVoxelGrid &&other);
 
    void clear();
 
    size_t size() const;
    bool empty() const;
 
private:
    VoxelSet voxels;
};
 
struct CscSlam3dProbabilisticVoxel {
    CscSlam3dMapVoxelKey key;
 
    double sumX = 0.0;
    double sumY = 0.0;
    double sumZ = 0.0;
 
    uint32_t hits = 0;
    uint32_t misses = 0;
 
    uint64_t firstSeenFrame = 0;
    uint64_t lastSeenFrame = 0;
 
    float confidence = 0.0f;
 
    void addHit(double x, double y, double z, uint64_t frameIndex);
    void addMiss(uint64_t frameIndex);
    void getCentroidMrpt(float &x, float &y, float &z) const;
 
    bool isVisible(float visibleThreshold) const;
    bool shouldErase(float eraseThreshold) const;
};
 
class CscSlam3dProbabilisticVoxelGrid {
public:
    using VoxelMap = std::unordered_map<
        CscSlam3dMapVoxelKey,
        CscSlam3dProbabilisticVoxel,
        CscSlam3dMapVoxelKeyHash>;
 
    explicit CscSlam3dProbabilisticVoxelGrid(float voxelSizeMeters = 0.03f);
 
    CscSlam3dMapVoxelKey mrptPointToVoxel(double x, double y, double z) const;
 
    CscSlam3dProbabilisticVoxel &touchVoxel(
        const CscSlam3dMapVoxelKey &key,
        uint64_t frameIndex);
 
    std::optional<CscSlam3dMapVoxelDiff> addMissVoxel(
        const CscSlam3dMapVoxelKey &key,
        uint64_t frameIndex);
 
    std::vector<CscSlam3dMapVoxelDiff> integrateRayMrpt(
        double originX,
        double originY,
        double originZ,
        double hitX,
        double hitY,
        double hitZ,
        uint64_t frameIndex,
        std::unordered_set<CscSlam3dMapVoxelKey, CscSlam3dMapVoxelKeyHash> &frameHitVoxels,
        std::unordered_set<CscSlam3dMapVoxelKey, CscSlam3dMapVoxelKeyHash> &frameMissVoxels);
 
    optional<CscSlam3dMapVoxelDiff> addPointMrpt(
        double x,
        double y,
        double z,
        uint64_t frameIndex);
 
    void clear();
 
    size_t size() const;
    bool empty() const;
 
    float getVoxelSizeMeters() const;
    const VoxelMap &getVoxelsUnsafe() const;
 
    float getVisibleThreshold() const;
 
    CscSlam3dMapVoxelGrid buildVisibleVoxelGrid() const;
    void eraseDeadVoxels();
 
    size_t visibleVoxelCount() const;
 
    std::vector<CscSlam3dMapVoxelKey> collectFarNeighborCandidates(
        int minNeighborCount,
        int minHits) const;
 
    std::vector<CscSlam3dMapVoxelKey> purgeFarNeighborCandidates(
        const std::vector<CscSlam3dMapVoxelKey> &candidates,
        int minNeighborCount,
        int minHits);
 
private:
    size_t visibleVoxelCountCache = 0;
    float voxelSizeMeters = 0.03f;
    const float visibleThreshold = 1.0f;
    double maxRayDistanceMeters = 6.5;
    double minRayDistanceMeters = 0.10;
    double missEndMarginMeters = 0.08;
 
    float eraseThreshold = 0.01f;
 
    VoxelMap voxels;
};
 
struct CartographyChangesQueue {
    mutex queueMutex;
    deque<CscSlam3dMapVoxelChanges> changesQueue;
 
    void push(CscSlam3dMapVoxelChanges changes);
    vector<CscSlam3dMapVoxelChanges> consumeAll();
    void clear();
};
 
class CscSlam3dMap {
public:
    CscSlam3dMap();
 
    void appendLocalFrame(
        const std::vector<CscPoint3d> &localPointsCm,
        const CPose3D &pose,
        size_t decimationStep,
        double minPointDistanceMeters,
        double maxPointDistanceMeters);
 
    vector<CscPoint3d *> pointsCopyPtrs(const CscPoint3d &entityStartPosition, const CscQuaternion &entityStartOrientation) const;
 
    vector<CscSlam3dMapVoxelKey> getAllVoxelsCopy() const {
        return voxelGrid.getAllVoxelsCopy();
    }
 
    size_t probabilisticVoxelGridSize() const;
    CscSlam3dMapVoxelGrid buildVisibleVoxelGridFromProbabilisticState() const;
 
    void clear();
 
    bool isEmpty() const;
 
    uint64_t add3dMapVoxelChangesListener(function<void(const CscSlam3dMapVoxelChanges &changes)> callback, CscSlam3dMapVoxelChanges *changes = nullptr);
    void remove3dMapVoxelChangesListener(uint64_t listenerId);
 
    void withProbabilisticVoxelGridReadLock(
        const std::function<void(const CscSlam3dProbabilisticVoxelGrid &grid)> &callback) const;
 
    size_t visibleProbabilisticVoxelGridSize() const;
 
    void purgeFarNeighbors(int minNeighborCount = 2, int minHits = 3);
 
private:
    mutable std::shared_mutex updateMutex;
 
    CscSlam3dMapVoxelGrid voxelGrid;
    mutex voxelChangesListenersMutex;
    vector<CscSlam3dMapVoxelChangesListener> voxelChangesListeners;
    std::atomic<uint64_t> nextVoxelChangesListenerId = 1;
    void fireVoxelChanges(const CscSlam3dMapVoxelChanges &changes) {
        vector<CscSlam3dMapVoxelChangesListener> voxelChangesListeners;
        {
            lock_guard lock(voxelChangesListenersMutex);
            voxelChangesListeners = this->voxelChangesListeners;
        }
        for (const auto &listener : voxelChangesListeners) {
            listener.callback(changes);
        }
    }
 
    CscSlam3dProbabilisticVoxelGrid probabilisticVoxelGrid;
    uint64_t probabilisticFrameIndex = 0;
};
 
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 {
        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 &getEntityStartPosition() const;
 
    const CscQuaternion &getEntityStartOrientation() 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> entityPose = 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 saveMrpt2dMapToPng(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();
 
    // TODO Fusion CS-3742 archi -> we should have only one API for the following
    mrpt::math::TPose2D getEntityPose2dMrpt() const;
 
    CscPoint3dOriented *getEntityPose3d() const;
 
    ptr<CObservation2DRangeScan> getLidarScan() const;
    ptr<COccupancyGridMap2D> getSLAMMap() const;
 
    bool is3dSlamInitialized() const;
 
    uint64_t add3dMapVoxelChangesListener(function<void(const CscSlam3dMapVoxelChanges &changes)> callback, CscSlam3dMapVoxelChanges *changes = nullptr);
    void remove3dMapVoxelChangesListener(uint64_t listenerId);
 
    vector<CscSlam3dMapVoxelKey> get3dMapVoxelsCopy() const;
 
    struct Slam3dParams {
        bool enabled = true;
 
        size_t decimationStep = 1;
        size_t decimationStepVoxelToPointCloud = 1;
        size_t minFramePoints = 120;
        size_t minMapPointsBeforeIcp = 1500;
 
        double minPointDistanceMeters = 0.20;
        double maxPointDistanceMeters = 15.0;
    };
 
private:
    void save3dMapTopViewPngUnlocked(const string &filename, double metersPerPixel, int imageSize, const CscPoint3dOriented *entityPosition = nullptr, const CscSize3d *entityDimensions = nullptr) const;
    void save3dMapToPcdAsciiUnlocked(const string &filename) const;
    class PhantomClusterTracker {
    public:
        PhantomClusterTracker(double overlapThreshold = 0.3, int maxAgeSeconds = 300);
 
        // Update tracker with new clusters from this tick
        void update(const vector<PhantomCluster> &newClusters);
 
        const vector<PhantomClusterTracked> &tracked() const;
 
        string getSummary() const;
 
    private:
        vector<PhantomClusterTracked> trackedClusters;
        const double overlapThreshold;
        const int maxAgeSeconds;
 
        // Check if clusters overlap enough to be considered the same
        bool clustersSimilar(const vector<std::pair<int, int>> &a,
                             const vector<std::pair<int, int>> &b) const;
    };
 
    const string entitySerialNumber;
    const bool isSimulatedEntity;
 
    void consumeLidarValues(const LidarValuesSnapshot &lidarValues);
 
    CscPoint3d entityStartPosition;
    CscQuaternion entityStartOrientation;
 
    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 entityStartYaw = 0;
 
    void initMrptSlam();
 
    queue<const LidarValuesSnapshot *> 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;
 
    Slam3dParams slam3dParams;
 
    mutable std::mutex slam3dMutex;
 
    CscSlam3dMap *slam3dMap = nullptr;
    CPose3D slam3dPose = CPose3D(0, 0, 0, 0, 0, 0);
 
    bool slam3dInitialized = false;
    size_t slam3dFramesIntegrated = 0;
 
    bool shouldUse3dSlam(const LidarValuesSnapshot &snap) const;
    void initMrptSlam3d();
 
    CPose3D odometryToMrptPose3d(const OdometryData &odometry) const;
 
    void consumeLidarValues3d(const LidarValuesSnapshot &snap,
                              const CPose3D &odoIncrement, optional<uint64_t> timeStamp = {});
};
 
/* 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*/);
}
 
inline CscPoint3d toMrptReferenceFrame3d(double x, double y, double z) {
    return CscPoint3d(x / 100.0 /*to meters*/, -z / 100.0 /*to meters*/, y);
}
 
inline CscPoint3d fromMrptReferenceFrame3d(double mx, double my, double mz) {
    return CscPoint3d(
        mx * 100.0,
        mz * 100.0,
        -my * 100.0);
}
 
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 entityWidth,
                                                    double entityLength,
                                                    map<int, double> *computedDirections = nullptr);
 
void saveScanWithBestDirectionAsPng(const mrpt::obs::CObservation2DRangeScan &scan,
                                    double bestYaw,
                                    const std::string &bestDirName,
                                    const fs::path &filename,
                                    double entityWidth,
                                    double entityLength);
 
double estimateFreeDistanceForLidarScan(double directionYawRads,
                                        const mrpt::obs::CObservation2DRangeScan &scan,
                                        double entityWidthMeters,
                                        double entityLengthMeters,
                                        double minClusterDistanceMeters = 0.10,
                                        int minClusterSize = 3,
                                        double maxDepthMeters = 2.0);
 
double estimateFreeDistanceFrontMetersForLidarScan3d(
    const LidarValuesSnapshot &scan,
    const CscSize3d &entitySizeCm,
    double voxelSizeMeters,
    int minVoxelPointCount,
    double maxDepthMeters,
    double minObstacleYMeters,
    double maxObstacleYMeters = std::numeric_limits<double>::infinity());
 
}
 
}
 
#endif