CscRealEntity3dLidarValues.h

Go to the documentation of this file.

//
// Created by Louis Grignon on 17/03/2026.
//
// based on https://chatgpt.com/c/69912668-fbb4-8385-91de-5664f0f0dfb7
// reception of optimized 3D pcl from entity with odo and queuing
#ifndef CscRealEntity3dLidarValues_H
#define CscRealEntity3dLidarValues_H
 
#include "CscCommon.h"
#include "Axiomes/Csc3dTypes.h"
 
#include <shared_mutex>
using std::shared_mutex;
 
namespace conscience_core::lidar {
 
struct LidarImuData {
    double timestamp = 0.0;
    double imu_accel_x = 0.0;
    double imu_accel_y = 0.0;
    double imu_accel_z = 0.0;
    double imu_ang_vel_x = 0.0;
    double imu_ang_vel_y = 0.0;
    double imu_ang_vel_z = 0.0;
    bool flag = false;
};
 
struct LidarExtrinsics {
    float tx = 0.0f; // centimeters, forward
    float ty = 0.0f; // centimeters, up
    float tz = 0.0f; // centimeters, right
 
    float yawRad = 0.0f;
    float pitchRad = 0.0f;
    float rollRad = 0.0f;
 
    bool enabled = true;
 
    inline void applyLidarToRobotTransform(float &x, float &y, float &z) const {
        if (!enabled) {
            return;
        }
 
        float rx = x;
        float ry = y;
        float rz = z;
 
        if (rollRad != 0.0f) {
            const float c = std::cos(rollRad);
            const float s = std::sin(rollRad);
            const float ny = c * ry - s * rz;
            const float nz = s * ry + c * rz;
            ry = ny;
            rz = nz;
        }
 
        if (pitchRad != 0.0f) {
            const float c = std::cos(pitchRad);
            const float s = std::sin(pitchRad);
            const float nx = c * rx + s * ry;
            const float ny = -s * rx + c * ry;
            rx = nx;
            ry = ny;
        }
 
        if (yawRad != 0.0f) {
            const float c = std::cos(yawRad);
            const float s = std::sin(yawRad);
            const float nx = c * rx - s * rz;
            const float nz = s * rx + c * rz;
            rx = nx;
            rz = nz;
        }
 
        x = rx + tx;
        y = ry + ty;
        z = rz + tz;
    }
};
 
class CscRealEntity3dLidarValues {
public:
    static vector<float> loadVerticalAnglesRadFromCsvFile(const string &filePath, const string &elevationHeaderName = "elevation");
    struct QueuedRangeImage {
        uint32_t frameId = 0u;
        uint16_t rows = 0u;
        uint16_t cols = 0u;
        double timestamp = 0.0;
        LidarImuData imu;
        ptr<vector<uint16_t>> rangesQ4mm = nullptr;
        LidarExtrinsics lidarExtrinsics;
 
        void getAsPointCloud(vector<axiomes::CscPoint3d> &pointCloud,
                             double &minY,
                             double &maxY,
                             double &avgY, const vector<float> &verticalAnglesRad);
    };
 
    explicit CscRealEntity3dLidarValues(const vector<float> &verticalAnglesRad);
 
    const vector<float> &getVerticalAnglesRad() const;
 
    void clear();
 
    void integrateFrame(uint16_t rows,
                        uint16_t cols,
                        uint32_t frameId,
                        bool isKeyFrame,
                        const uint8_t *data,
                        size_t count,
                        const LidarImuData &imu,
                        LidarExtrinsics lidarExtrinsics);
 
    map<double, double> get2DSliceAtHeight(double targetY = 15, double tolerance = 20) const;
 
    bool getLatest(QueuedRangeImage &out);
 
private:
    mutable shared_mutex mutex_;
 
    vector<float> verticalAnglesRad;
 
    ptr<QueuedRangeImage> currentFrame_ = nullptr;
 
    uint32_t currentFrameId_ = 0u;
    uint16_t currentRows_ = 0u;
    uint16_t currentCols_ = 0u;
 
    static size_t index_(uint16_t row, uint16_t col, uint16_t cols);
 
    void initCurrent_(uint16_t rows, uint16_t cols);
 
    unique_ptr<CscLogger> logger = CscLogger::getForCategory("CscRealEntity3dLidarValues");
};
}
#endif