Csc3dCartographyProcessor.h

Go to the documentation of this file.

#ifndef Csc3dCartographyProcessor_h
#define Csc3dCartographyProcessor_h
 
#include "CscCommon.h"
 
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/Dense>
 
using namespace conscience_utils::logging;
 
namespace conscience_core::lidar {
struct LidarExtrinsicsSidecar {
    float tx = 0.0f;
    float ty = 0.0f;
    float tz = 0.0f;
 
    float yawRad = 0.0f;
    float pitchRad = 0.0f;
    float rollRad = 0.0f;
};
 
struct OdometryDeltaSidecar {
    double deltaX = 0.0;
    double deltaY = 0.0;
    double deltaZ = 0.0;
 
    double deltaPitch = 0.0;
    double deltaYaw = 0.0;
    double deltaRoll = 0.0;
};
 
struct FrameMetadata {
    uint64_t frameId = 0;
 
    double timestamp = 0.0;
    optional<OdometryDeltaSidecar> odometryDelta = nullopt;
    LidarExtrinsicsSidecar lidarExtrinsics;
 
    string toJsonString() const;
};
struct CartographyConfig {
    float voxelLeafSize = 5.0f;
 
    float finalVoxelLeafSize = 2.0f;
 
    double maxEdgeRmse = 30.0;
 
    float maxAngleDeg = 45.0f;
 
    bool odometryOnlyMode = false;
 
    double maxTranslationCorrection = 20.0f; // cm
 
    bool useOdometryInitialGuess = true;
    bool fallbackToOdometryWhenGicpRejected = false;
 
    bool enableYawMultistart = true;
 
    float odometryYawClampDeg = 8.0f;
    float odometryTranslationScale = 0.7f;
 
    bool accumulateStaticBootstrap = true;
    float staticBootstrapMaxTranslationCm = 2.0f; // cm
    float staticBootstrapMaxYawDeg = 1.0f;
 
    std::vector<float> yawMultistartOffsetsDeg = {
        -20.0f,
        -12.0f,
        -6.0f,
        0.0f,
        6.0f,
        12.0f,
        20.0f
    };
 
    int localMapWindowSize = 5;
 
    int lumMaxIterations = 50;
 
    int lumOverlapWindow = 10;
 
    double gicpResolution = 10.0;
 
    double spatialLoopDist = 500.0;
 
    int temporalLoopGap = 50;
 
    double loopClosureThreshold = 30.0;
 
    float dynVoxelSize = 20.0f;
 
    int minFrameObservations = 1;
 
    int ransacMaxIterations = 200;
 
    double ransacDistanceThreshold = 5.0;
 
    int ransacMinInliers = 1000;
 
    bool enableSpatialFilter = true;
 
    float maxAbsCoord = 1500.0f;
 
    float maxRange = 1500.0f;
 
    bool enableGroundLevelling = false;
 
    float groundMaxTiltDeg = 15.0f;
 
    float groundMinUpDot = 0.95f;
 
 
};
typedef pcl::PointXYZI PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
 
class Csc3dCartographyProcessor {
public:
    explicit Csc3dCartographyProcessor(
        const fs::path &inputFolderPath,
        const fs::path &outputFilePath,
        const CartographyConfig &config = CartographyConfig());
 
    ~Csc3dCartographyProcessor() = default;
 
    bool runPipeline();
 
private:
    Eigen::Matrix4f estimateRelativeFrameTransform(
    int i,
    double& rmseOut,
    bool& acceptedOut);
    fs::path inputFolderPath;
    fs::path outputFilePath;
    CartographyConfig config;
    std::string pcdPrefix = "lidar_";
    std::unique_ptr<CscLogger> logger;
 
    vector<FrameMetadata> frameMetadata;
    vector<PointCloudT::Ptr> rawFrames;
    vector<PointCloudT::Ptr> downsampledFrames;
    vector<uint32_t> validFrameIds;
 
    Eigen::Matrix3f groundLevellingR0 = Eigen::Matrix3f::Identity();
 
    bool validateInputs() const;
 
    bool loadFrames();
 
    void computeInitialOdometry(vector<Eigen::Matrix4f> &poses);
 
    void buildAndOptimizeElchGraph(
        const vector<Eigen::Matrix4f> &initialPoses,
        vector<Eigen::Matrix4f> &optimizedPoses);
 
    PointCloudT::Ptr mergeAndFilter(const vector<Eigen::Matrix4f> &poses);
 
    void saveToPLY(const PointCloudT::Ptr &cloud, const fs::path &path);
 
    bool readJsonSidecar(const fs::path &jsonPath, FrameMetadata &out) const;
 
    PointCloudT::Ptr applyVoxelGridFilter(PointCloudT::Ptr input, float leafSize);
 
    void computeGicpAlignment(
        PointCloudT::Ptr source,
        PointCloudT::Ptr target,
        Eigen::Matrix4f &transformOut,
        double &rmseOut,
        const Eigen::Matrix4f &initialGuess,
        int numThreads = 0);
 
    string getProgressBarText(int current, int total, const std::string &prefix);
 
    std::vector<bool> acceptedPoseMask;
};
 
}
 
#endif