QUYVN/BQP

Files

2026-07-13 09:25:40 +07:00

30 KiB

Raw Blame History

CartographerSharp - Hướng Dẫn Sử Dụng Chi Tiết

CartographerSharp là phiên bản C# của Google Cartographer, một hệ thống SLAM (Simultaneous Localization and Mapping) thời gian thực cho cả 2D và 3D. Dự án này cho phép tích hợp khả năng SLAM mạnh mẽ trực tiếp vào hệ sinh thái .NET.

🏗 Kiến Trúc Tổng Quan

CartographerSharp được xây dựng theo kiến trúc module với điểm trung tâm là IMapBuilder.

graph TD
    App[Ứng Dụng Robot] -->|Tạo| MapBuilder[MapBuilder]
    
    subgraph "SLAM Pipeline"
        MapBuilder -->|Quản lý| Trajectories[Trajectory Builders]
        MapBuilder -->|Quản lý| PoseGraph[Pose Graph]
        
        Trajectories -->|Đưa vào| LocalSLAM[Local SLAM]
        LocalSLAM -->|Scan Matching| ScanMatcher[Scan Matcher 2D/3D]
        LocalSLAM -->|Cập nhật| Submaps[Submaps]
        
        PoseGraph -->|Tối ưu| Optimizer[Optimization Problem]
        PoseGraph -->|Phát hiện| LoopClosure[Loop Closure]
        
        LoopClosure -->|Tạo| Constraints[Constraints]
        Constraints -->|Đưa vào| Optimizer
        
        Optimizer -->|Sử dụng| Ceres[CeresSharp Solver]
    end
    
    subgraph "Sensor Input"
        Lidar[Lidar/Laser]
        IMU[IMU]
        Odom[Odometry]
        Camera[Camera/Vision]
    end
    
    Lidar -->|Point Cloud| Trajectories
    IMU -->|Angular Velocity| Trajectories
    Odom -->|Wheel Encoder| Trajectories
    Camera -->|Landmarks| Trajectories

Luồng Dữ Liệu

sequenceDiagram
    participant App as Ứng Dụng
    participant MB as MapBuilder
    participant TB as TrajectoryBuilder
    participant LS as LocalSLAM
    participant PG as PoseGraph
    
    App->>MB: Tạo MapBuilder
    App->>MB: AddTrajectoryBuilder(options)
    MB-->>App: trajectoryId
    
    loop Mỗi khung sensor
        App->>TB: AddSensorData(lidar)
        TB->>LS: Xử lý scan
        LS->>LS: Scan matching
        LS->>LS: Cập nhật submap
        LS-->>PG: Thêm node mới
        
        opt IMU có sẵn
            App->>TB: AddSensorData(imu)
        end
        
        opt Odometry có sẵn
            App->>TB: AddSensorData(odom)
        end
    end
    
    App->>MB: FinishTrajectory()
    App->>PG: RunFinalOptimization()
    PG->>PG: Tìm loop closures
    PG->>PG: Tối ưu toàn cục
    PG-->>App: Bản đồ tối ưu

🔑 Các Interface Chính

Interface	Mô Tả	File
`IMapBuilder`	Điểm khởi đầu chính, quản lý toàn bộ SLAM stack	`Mapping/IMapBuilder.cs`
`ITrajectoryBuilder`	Xử lý dữ liệu sensor cho một quỹ đạo cụ thể	`Mapping/ITrajectoryBuilder.cs`
`IPoseGraph`	Quản lý đồ thị pose toàn cục, tối ưu hóa, phát hiện loop closure	`Mapping/IPoseGraph.cs`
`IPoseExtrapolator`	Ước tính pose hiện tại từ lịch sử và sensor (IMU/Odom)	`Mapping/IPoseExtrapolator.cs`
`IRangeDataInserter`	Logic chèn dữ liệu range vào grid xác suất hoặc TSDF	`Mapping/IRangeDataInserter.cs`
`IGrid`	Biểu diễn cấu trúc bản đồ (2D Grid, 3D Hybrid Grid)	`Mapping/IRangeDataInserter.cs`

📦 Cài Đặt

Yêu Cầu

.NET 6.0 trở lên
CeresSharp: Wrapper C# cho Ceres Solver (đã bao gồm trong solution)
Hệ điều hành: Linux (hỗ trợ chính), Windows, macOS

Thêm Reference

<!-- Trong file .csproj của bạn -->
<ItemGroup>
  <ProjectReference Include="path/to/CartographerSharp/CartographerSharp.csproj" />
  <ProjectReference Include="path/to/CeresSharp/CeresSharp.csproj" />
</ItemGroup>

Hoặc thêm DLL trực tiếp:

<ItemGroup>
  <Reference Include="CartographerSharp">
    <HintPath>path/to/CartographerSharp.dll</HintPath>
  </Reference>
</ItemGroup>

🚀 Hướng Dẫn Sử Dụng Cơ Bản

1. Khởi Tạo MapBuilder

using CartographerSharp.Mapping;
using CartographerSharp.Mapping.D2D;
using CartographerSharp.Sensor;

// Cấu hình options
var mapBuilderOptions = new MapBuilderOptions
{
    UseTrajectoryBuilder2D = true,  // Sử dụng SLAM 2D
    NumBackgroundThreads = 4,
    PoseGraphOptions = new PoseGraphOptions
    {
        OptimizationProblemOptions = new OptimizationProblemOptions
        {
            HuberScale = 1e1,
            AccelerationWeight = 1e3,
            RotationWeight = 1e5
        },
        ConstraintBuilderOptions = new ConstraintBuilderOptions
        {
            SamplingRatio = 0.3,
            MinScore = 0.55,
            GlobalLocalizationMinScore = 0.6
        }
    }
};

// Tạo MapBuilder
IMapBuilder mapBuilder = new MapBuilder(mapBuilderOptions);

2. Tạo Trajectory

// Định nghĩa các sensor sẽ sử dụng
var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>
{
    new ITrajectoryBuilder.SensorId 
    { 
        Type = ITrajectoryBuilder.SensorId.SensorType.Range, 
        Id = "horizontal_laser_2d" 
    },
    new ITrajectoryBuilder.SensorId 
    { 
        Type = ITrajectoryBuilder.SensorId.SensorType.Imu, 
        Id = "imu" 
    },
    new ITrajectoryBuilder.SensorId 
    { 
        Type = ITrajectoryBuilder.SensorId.SensorType.Odometry, 
        Id = "odom" 
    }
};

// Cấu hình trajectory
var trajectoryOptions = new TrajectoryBuilderOptions
{
    TrajectoryBuilder2DOptions = new TrajectoryBuilder2DOptions
    {
        MinRange = 0.2,
        MaxRange = 30.0,
        MinZValue = -0.8,
        MaxZValue = 2.0,
        VoxelFilterSize = 0.025,
        AdaptiveVoxelFilterOptions = new AdaptiveVoxelFilterOptions
        {
            MaxLength = 0.5,
            MinNumPoints = 200,
            MaxRange = 50.0
        }
    }
};

// Thêm trajectory
int trajectoryId = mapBuilder.AddTrajectoryBuilder(
    sensorIds, 
    trajectoryOptions, 
    OnLocalSlamResult  // Callback (tùy chọn)
);

ITrajectoryBuilder? trajectoryBuilder = mapBuilder.GetTrajectoryBuilder(trajectoryId);

3. Đưa Dữ Liệu Sensor

Dữ liệu Lidar/Laser

// Tạo point cloud
var points = new List<Vector3>
{
    new Vector3(1.0f, 0.0f, 0.0f),
    new Vector3(0.0f, 1.0f, 0.0f),
    // ... thêm các điểm
};

var timedPointCloud = new TimedPointCloudData
{
    Time = DateTimeOffset.UtcNow.Ticks,  // Universal Time Scale
    Origin = Vector3.Zero,
    Points = points
};

trajectoryBuilder?.AddSensorData("horizontal_laser_2d", timedPointCloud);

Dữ liệu IMU

var imuData = new ImuData
{
    Time = DateTimeOffset.UtcNow.Ticks,
    LinearAcceleration = new Vector3(0.0f, 0.0f, 9.81f),  // m/s²
    AngularVelocity = new Vector3(0.01f, 0.02f, 0.03f)     // rad/s
};

trajectoryBuilder?.AddSensorData("imu", imuData);

Dữ liệu Odometry

var odometryData = new OdometryData
{
    Time = DateTimeOffset.UtcNow.Ticks,
    Pose = new Rigid3d(
        new Vector3(1.0f, 2.0f, 0.0f),     // Translation
        Quaternion.Identity                  // Rotation
    )
};

trajectoryBuilder?.AddSensorData("odom", odometryData);

4. Kết Thúc và Tối Ưu

// Kết thúc trajectory
mapBuilder.FinishTrajectory(trajectoryId);

// Chạy tối ưu toàn cục cuối cùng
mapBuilder.PoseGraph.RunFinalOptimization();

// Lấy kết quả
var optimizedNodes = mapBuilder.PoseGraph.GetTrajectoryNodes();
var submapData = mapBuilder.PoseGraph.GetAllSubmapData();

⚙️ Cấu Hình Chi Tiết

MapBuilderOptions

public class MapBuilderOptions
{
    // Chế độ SLAM
    public bool UseTrajectoryBuilder2D { get; set; } = true;
    public bool UseTrajectoryBuilder3D { get; set; } = false;
    
    // Threading
    public int NumBackgroundThreads { get; set; } = 4;
    
    // Cấu hình PoseGraph
    public PoseGraphOptions PoseGraphOptions { get; set; }
}

TrajectoryBuilder2DOptions

public class TrajectoryBuilder2DOptions
{
    // Giới hạn range
    public double MinRange { get; set; } = 0.0;
    public double MaxRange { get; set; } = 30.0;
    
    // Giới hạn Z
    public double MinZValue { get; set; } = -0.8;
    public double MaxZValue { get; set; } = 2.0;
    
    // Voxel filtering
    public double VoxelFilterSize { get; set; } = 0.025;
    
    // Submaps
    public SubmapsOptions2D SubmapsOptions { get; set; }
    
    // Scan matching
    public RealTimeCorrelativeScanMatcherOptions2D 
        RealTimeCorrelativeScanMatcherOptions { get; set; }
    public CeresScanMatcherOptions2D 
        CeresScanMatcherOptions { get; set; }
    
    // Motion filter
    public MotionFilterOptions MotionFilterOptions { get; set; }
}

PoseGraphOptions

public class PoseGraphOptions
{
    // Tần suất tối ưu
    public int OptimizeEveryNNodes { get; set; } = 90;
    
    // Constraint builder
    public ConstraintBuilderOptions ConstraintBuilderOptions { get; set; }
    
    // Optimization problem
    public OptimizationProblemOptions OptimizationProblemOptions { get; set; }
    
    // Overlap computation
    public double MaxNumFinalIterations { get; set; } = 200;
}

💡 Các Tình Huống Sử Dụng

Ví Dụ 1: SLAM 2D Với Lidar

public class Lidar2DSlam
{
    private IMapBuilder _mapBuilder;
    private int _trajectoryId;
    
    public void Initialize()
    {
        var options = new MapBuilderOptions
        {
            UseTrajectoryBuilder2D = true,
            PoseGraphOptions = new PoseGraphOptions
            {
                OptimizeEveryNNodes = 90,
                ConstraintBuilderOptions = new ConstraintBuilderOptions
                {
                    SamplingRatio = 0.3,
                    MinScore = 0.55
                }
            }
        };
        
        _mapBuilder = new MapBuilder(options);
        
        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>
        {
            new(ITrajectoryBuilder.SensorId.SensorType.Range, "laser")
        };
        
        var trajOptions = new TrajectoryBuilderOptions
        {
            TrajectoryBuilder2DOptions = new TrajectoryBuilder2DOptions
            {
                MinRange = 0.2,
                MaxRange = 30.0,
                VoxelFilterSize = 0.025
            }
        };
        
        _trajectoryId = _mapBuilder.AddTrajectoryBuilder(
            sensorIds, trajOptions, null);
    }
    
    public void ProcessLidarScan(List<Vector3> laserPoints, long timestamp)
    {
        var trajectoryBuilder = _mapBuilder.GetTrajectoryBuilder(_trajectoryId);
        if (trajectoryBuilder == null) return;
        
        var timedPointCloud = new TimedPointCloudData
        {
            Time = timestamp,
            Origin = Vector3.Zero,
            Points = laserPoints
        };
        
        trajectoryBuilder.AddSensorData("laser", timedPointCloud);
    }
    
    public void Finish()
    {
        _mapBuilder.FinishTrajectory(_trajectoryId);
        _mapBuilder.PoseGraph.RunFinalOptimization();
        
        // Lưu bản đồ
        _mapBuilder.SerializeStateToFile(true, "map.pbstream");
    }
}

Ví Dụ 2: SLAM 2D Với Lidar + IMU + Odometry

public class MultiSensorSlam
{
    private IMapBuilder _mapBuilder;
    private ITrajectoryBuilder _trajectoryBuilder;
    
    public void Initialize()
    {
        var mapBuilderOptions = new MapBuilderOptions
        {
            UseTrajectoryBuilder2D = true,
            NumBackgroundThreads = 4
        };
        
        _mapBuilder = new MapBuilder(mapBuilderOptions);
        
        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>
        {
            new(ITrajectoryBuilder.SensorId.SensorType.Range, "lidar"),
            new(ITrajectoryBuilder.SensorId.SensorType.Imu, "imu"),
            new(ITrajectoryBuilder.SensorId.SensorType.Odometry, "odom")
        };
        
        var trajectoryOptions = CreateTrajectoryOptions();
        
        int trajectoryId = _mapBuilder.AddTrajectoryBuilder(
            sensorIds, trajectoryOptions, OnLocalSlamResult);
        
        _trajectoryBuilder = _mapBuilder.GetTrajectoryBuilder(trajectoryId)!;
    }
    
    private TrajectoryBuilderOptions CreateTrajectoryOptions()
    {
        return new TrajectoryBuilderOptions
        {
            TrajectoryBuilder2DOptions = new TrajectoryBuilder2DOptions
            {
                MinRange = 0.2,
                MaxRange = 30.0,
                MinZValue = -0.8,
                MaxZValue = 2.0,
                VoxelFilterSize = 0.025,
                UseImu = true,  // Sử dụng IMU
                SubmapsOptions = new SubmapsOptions2D
                {
                    NumRangeData = 90,
                    GridOptions2D = new GridOptions2D
                    {
                        GridType = GridType.ProbabilityGrid,
                        Resolution = 0.05
                    }
                },
                CeresScanMatcherOptions = new CeresScanMatcherOptions2D
                {
                    OccupiedSpaceWeight = 1.0,
                    TranslationWeight = 10.0,
                    RotationWeight = 40.0
                }
            }
        };
    }
    
    public void ProcessSensorData(
        List<Vector3>? lidarPoints,
        Vector3? linearAccel,
        Vector3? angularVel,
        Rigid3d? odomPose,
        long timestamp)
    {
        // Đưa dữ liệu theo thứ tự: IMU -> Odom -> Lidar
        
        if (linearAccel.HasValue && angularVel.HasValue)
        {
            var imuData = new ImuData
            {
                Time = timestamp,
                LinearAcceleration = linearAccel.Value,
                AngularVelocity = angularVel.Value
            };
            _trajectoryBuilder.AddSensorData("imu", imuData);
        }
        
        if (odomPose.HasValue)
        {
            var odomData = new OdometryData
            {
                Time = timestamp,
                Pose = odomPose.Value
            };
            _trajectoryBuilder.AddSensorData("odom", odomData);
        }
        
        if (lidarPoints != null)
        {
            var timedPointCloud = new TimedPointCloudData
            {
                Time = timestamp,
                Origin = Vector3.Zero,
                Points = lidarPoints
            };
            _trajectoryBuilder.AddSensorData("lidar", timedPointCloud);
        }
    }
    
    private void OnLocalSlamResult(int trajectoryId, 
        long time, 
        Rigid3d localPose, 
        RangeData rangeData, 
        ITrajectoryBuilder.InsertionResult? insertionResult)
    {
        Console.WriteLine($"Local SLAM @ {time}: {localPose}");
        
        if (insertionResult.HasValue)
        {
            Console.WriteLine($"  Node ID: {insertionResult.Value.NodeId}");
            Console.WriteLine($"  Submaps: {insertionResult.Value.InsertionSubmaps.Count}");
        }
    }
}

Ví Dụ 3: Load và Tiếp Tục SLAM

public class ResumableSlam
{
    public void LoadAndContinue(string pbstreamPath)
    {
        var mapBuilder = new MapBuilder(new MapBuilderOptions
        {
            UseTrajectoryBuilder2D = true
        });
        
        // Load state từ file
        var trajectoryRemapping = mapBuilder.LoadStateFromFile(
            pbstreamPath, 
            loadFrozenState: false  // false = có thể tiếp tục
        );
        
        Console.WriteLine($"Loaded {trajectoryRemapping.Count} trajectories");
        
        // Tiếp tục thêm dữ liệu mới
        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>
        {
            new(ITrajectoryBuilder.SensorId.SensorType.Range, "laser")
        };
        
        int newTrajectoryId = mapBuilder.AddTrajectoryBuilder(
            sensorIds, 
            new TrajectoryBuilderOptions { /* ... */ },
            null
        );
        
        // Xử lý dữ liệu mới...
    }
}

🔌 Tích Hợp Sensor

Định Nghĩa Sensor ID

Mỗi sensor cần có một ID duy nhất:

var sensorId = new ITrajectoryBuilder.SensorId
{
    Type = ITrajectoryBuilder.SensorId.SensorType.Range,  // hoặc Imu, Odometry, ...
    Id = "unique_sensor_name"
};

Các loại sensor:

SensorType	Mô Tả	Dữ Liệu
`Range`	Lidar, Laser Scanner, Depth Camera	`TimedPointCloudData`
`Imu`	Inertial Measurement Unit	`ImuData`
`Odometry`	Wheel encoders, Visual odometry	`OdometryData`
`FixedFramePose`	GPS, Motion capture	`FixedFramePoseData`
`Landmark`	Visual landmarks, Fiducial markers	`LandmarkData`

Chuẩn Bị Dữ Liệu

Point Cloud

// Từ Lidar scan 2D
public TimedPointCloudData ConvertLidarScan(
    double[] ranges, 
    double angleMin, 
    double angleIncrement,
    long timestamp)
{
    var points = new List<Vector3>();
    
    for (int i = 0; i < ranges.Length; i++)
    {
        if (ranges[i] > 0.0f && ranges[i] < 30.0f)
        {
            double angle = angleMin + i * angleIncrement;
            double x = ranges[i] * MathF.Cos(angle);
            double y = ranges[i] * MathF.Sin(angle);
            
            points.Add(new Vector3(x, y, 0.0f));
        }
    }
    
    return new TimedPointCloudData
    {
        Time = timestamp,
        Origin = Vector3.Zero,
        Points = points
    };
}

IMU Data

// Từ IMU hardware
public ImuData ConvertImuReading(
    double[] linearAccel,  // [ax, ay, az] trong m/s²
    double[] angularVel,   // [wx, wy, wz] trong rad/s
    long timestamp)
{
    return new ImuData
    {
        Time = timestamp,
        LinearAcceleration = new Vector3(
            (double)linearAccel[0],
            (double)linearAccel[1],
            (double)linearAccel[2]
        ),
        AngularVelocity = new Vector3(
            (double)angularVel[0],
            (double)angularVel[1],
            (double)angularVel[2]
        )
    };
}

📚 API Reference

IMapBuilder

public interface IMapBuilder
{
    // Thêm trajectory mới
    int AddTrajectoryBuilder(
        HashSet<ITrajectoryBuilder.SensorId> expectedSensorIds,
        TrajectoryBuilderOptions trajectoryBuilderOptions,
        LocalSlamResultCallback? localSlamResultCallback);
    
    // Lấy trajectory builder
    ITrajectoryBuilder? GetTrajectoryBuilder(int trajectoryId);
    
    // Kết thúc trajectory
    void FinishTrajectory(int trajectoryId);
    
    // Serialize/deserialize
    void SerializeState(bool includeUnfinishedSubmaps, IProtoStreamWriter writer);
    bool SerializeStateToFile(bool includeUnfinishedSubmaps, string filename);
    Dictionary<int, int> LoadState(IProtoStreamReader reader, bool loadFrozenState);
    Dictionary<int, int> LoadStateFromFile(string filename, bool loadFrozenState);
    
    // Properties
    int NumTrajectoryBuilders { get; }
    IPoseGraph PoseGraph { get; }
    List<TrajectoryBuilderOptionsWithSensorIds> GetAllTrajectoryBuilderOptions();
}

ITrajectoryBuilder

public interface ITrajectoryBuilder
{
    // Thêm dữ liệu từ các sensor
    void AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData);
    void AddSensorData(string sensorId, ImuData imuData);
    void AddSensorData(string sensorId, OdometryData odometryData);
    void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData);
    void AddSensorData(string sensorId, LandmarkData landmarkData);
    
    // Thêm kết quả local SLAM trực tiếp (advanced)
    void AddLocalSlamResultData(LocalSlamResultData localSlamResultData);
}

IPoseGraph

public interface IPoseGraph
{
    // Tối ưu hóa
    void RunFinalOptimization();
    
    // Lấy dữ liệu
    MapById<SubmapId, SubmapData> GetAllSubmapData();
    SubmapData GetSubmapData(SubmapId submapId);
    MapById<SubmapId, SubmapPose> GetAllSubmapPoses();
    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
    MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
    List<Constraint> Constraints();
    
    // Quản lý trajectory
    void FinishTrajectory(int trajectoryId);
    void FreezeTrajectory(int trajectoryId);
    void DeleteTrajectory(int trajectoryId);
    bool IsTrajectoryFinished(int trajectoryId);
    bool IsTrajectoryFrozen(int trajectoryId);
    
    // Transform
    Rigid3d GetLocalToGlobalTransform(int trajectoryId);
    
    // Callback
    void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
}

🐛 Troubleshooting

Vấn Đề 1: Scan Matching Thất Bại

Triệu chứng: Console log hiện "Scan matching failed" hoặc pose nhảy lung tung.

Nguyên nhân:

Point cloud quá thưa hoặc quá nhiễu
Chuyển động robot quá nhanh
Cấu hình scan matcher không phù hợp

Giải pháp:

var scanMatcherOptions = new CeresScanMatcherOptions2D
{
    OccupiedSpaceWeight = 1.0,
    TranslationWeight = 10.0,    // Tăng lên nếu pose translation không ổn định
    RotationWeight = 40.0,       // Tăng lên nếu pose rotation không ổn định
    
    CeresSolverOptions = new CeresSolverOptions
    {
        MaxNumIterations = 20,   // Tăng nếu cần độ chính xác cao hơn
        NumThreads = 1
    }
};

Vấn Đề 2: Bộ Nhớ Tăng Cao

Triệu chứng: Memory usage tăng liên tục.

Nguyên nhân:

Submaps không được giải phóng
Quá nhiều trajectory nodes

Giải pháp:

// 1. Sử dụng PoseGraphTrimmer
var trimmerOptions = new PoseGraphTrimmerOptions
{
    MaxNumFinishedNodesPerTrajectory = 1000,
    MaxNumSubmapsPerTrajectory = 100
};

// 2. Giảm tần suất lưu nodes
var submapsOptions = new SubmapsOptions2D
{
    NumRangeData = 90  // Tăng giá trị này để giảm số nodes
};

Vấn Đề 3: Loop Closure Không Hoạt Động

Triệu chứng: Robot quay về vị trí cũ nhưng không có loop closure.

Giải pháp:

var constraintBuilderOptions = new ConstraintBuilderOptions
{
    SamplingRatio = 0.3,              // Tăng để kiểm tra nhiều hơn
    MinScore = 0.55,                   // Giảm để dễ tìm matches
    GlobalLocalizationMinScore = 0.6,
    LoopClosureTranslationWeight = 1.1e4,
    LoopClosureRotationWeight = 1e5
};

Vấn Đề 4: Timestamp Issues

Lỗi: "Sensor data must be in increasing time order"

Giải pháp: Đảm bảo timestamp tăng dần và đồng bộ:

private long _lastTimestamp = 0;

public void AddData(SensorData data)
{
    long currentTime = data.Timestamp;
    
    if (currentTime <= _lastTimestamp)
    {
        // Skip hoặc điều chỉnh timestamp
        currentTime = _lastTimestamp + 1;
    }
    
    _lastTimestamp = currentTime;
    
    // Thêm vào trajectory builder
    _trajectoryBuilder.AddSensorData(sensorId, data);
}

🤖 Dành Cho AI Agents

Xin chào AI Agent! Nếu bạn đang phân tích hoặc refactor codebase này, đây là điều bạn cần biết.

Cấu Trúc Thư Mục

CartographerSharp/
├── Mapping/                    # Core SLAM logic
│   ├── IMapBuilder.cs         # Main interface
│   ├── MapBuilder.cs          # Implementation
│   ├── IPoseGraph.cs          # Optimization interface
│   ├── PoseGraph.cs           # Base implementation
│   ├── ITrajectoryBuilder.cs  # Sensor data interface
│   ├── Internal/
│   │   ├── 2D/                # 2D SLAM implementations
│   │   │   ├── PoseGraph2D.cs
│   │   │   ├── LocalTrajectoryBuilder2D.cs
│   │   │   └── Submap2D.cs
│   │   ├── 3D/                # 3D SLAM implementations
│   │   ├── Optimization/      # Ceres optimization problems
│   │   │   ├── OptimizationProblem2D.cs
│   │   │   ├── SpaCostFunction2D.cs
│   │   │   └── LandmarkCostFunction2D.cs
│   │   └── Constraints/       # Loop closure detection
│   │       ├── ConstraintBuilder2D.cs
│   │       └── FastCorrelativeScanMatcher2D.cs
│   └── 2D/                    # 2D specific components
│       ├── Grid2D.cs
│       └── Submap2D.cs
├── IO/                        # Serialization
│   ├── IProtoStreamReader.cs
│   └── IProtoStreamWriter.cs
├── Sensor/                    # Sensor data types
│   ├── PointCloud.cs
│   ├── ImuData.cs
│   └── OdometryData.cs
├── Transform/                 # Geometry utilities
│   ├── Rigid3d.cs
│   └── Rigid2d.cs
└── Proto/                     # Protobuf definitions (auto-generated)

Khái Niệm Quan Trọng

Coordinate Frames
- Map Frame: Global, loop-closed frame
- Odom Frame: Local, non-loop-closed frame
- Tracking Frame: Sensor frame
- Gravity-aligned Frame: Used for IMU integration
Key Data Structures
- NodeId: Unique identifier cho trajectory node (trajectory_id, node_index)
- SubmapId: Unique identifier cho submap (trajectory_id, submap_index)
- Constraint: Kết nối giữa submap và node (intra/inter-submap)
- TrajectoryNode: Chứa pose và sensor data tại một thời điểm

Optimization Flow

Local SLAM → Nodes → Add to PoseGraph
                        ↓
                 Constraint Builder
                        ↓
                 Detect Loop Closures
                        ↓
                 Optimization Problem
                        ↓
                 Ceres Solver
                        ↓
                 Update Node Poses

Refactoring Notes

Gần đây đã refactor: Abstract classes *Interface → Interfaces I*
Thread Safety: MapBuilder và components là concurrent-safe. Mutex được dùng trong PoseGraph.
Ceres Integration: Cost functions trong SpaCostFunction*.cs. Đảm bảo derivatives đúng.

Common Tasks

1. Thêm Sensor Mới

// 1. Thêm vào enum SensorType
public enum SensorType
{
    Range, Imu, Odometry, FixedFramePose, Landmark,
    MyNewSensor  // <- Thêm vào đây
}

// 2. Thêm data type
public class MyNewSensorData
{
    public long Time { get; set; }
    // ... fields
}

// 3. Thêm method vào ITrajectoryBuilder
void AddSensorData(string sensorId, MyNewSensorData data);

// 4. Implement trong TrajectoryBuilder2DAdapter/3DAdapter
public void AddSensorData(string sensorId, MyNewSensorData data)
{
    // Xử lý data
}

2. Điều Chỉnh Cost Function

// Trong SpaCostFunction2D.cs hoặc tương tự
public class MyCustomCostFunction : CostFunction
{
    public override bool Evaluate(
        double[][] parameters,
        double[] residuals,
        double[][]? jacobians)
    {
        // 1. Unpack parameters
        var rotation1 = parameters[0];
        var translation1 = parameters[1];
        
        // 2. Compute residuals
        residuals[0] = /* ... */;
        
        // 3. Compute Jacobians (nếu cần)
        if (jacobians != null)
        {
            jacobians[0][0] = /* ∂r/∂rotation1 */;
            // ...
        }
        
        return true;
    }
}

Debugging Tips

// 1. Enable verbose logging
var mapBuilder = new MapBuilder(options);
mapBuilder.PoseGraph.SetGlobalSlamOptimizationCallback(
    (submapIds, nodeIds) =>
    {
        Console.WriteLine($"Optimization: {nodeIds.Count} nodes, {submapIds.Count} submaps");
    }
);

// 2. Monitor local SLAM results
int trajectoryId = mapBuilder.AddTrajectoryBuilder(
    sensorIds, 
    options,
    (trajId, time, localPose, rangeData, insertionResult) =>
    {
        Console.WriteLine($"Local SLAM @ {time}");
        Console.WriteLine($"  Pose: {localPose}");
        if (insertionResult.HasValue)
            Console.WriteLine($"  Node: {insertionResult.Value.NodeId}");
    }
);

// 3. Kiểm tra constraints
var constraints = mapBuilder.PoseGraph.Constraints();
foreach (var c in constraints)
{
    Console.WriteLine($"{c.SubmapId} <-> {c.NodeId}: {c.ConstraintTag}");
}

📖 Tài Liệu Bổ Sung

CeresSharp: Xem ../CeresSharp/README.md để hiểu về solver
Original Cartographer: https://google-cartographer.readthedocs.io/
Paper: Hess, W., et al. "Real-time loop closure in 2D LIDAR SLAM." ICRA 2016.

📝 License

Licensed under the Apache License, Version 2.0

Được duy trì bởi RobotNet10 Team

30 KiB Raw Blame History

CartographerSharp - Hướng Dẫn Sử Dụng Chi Tiết

📋 Mục Lục

🏗 Kiến Trúc Tổng Quan

Luồng Dữ Liệu

🔑 Các Interface Chính

📦 Cài Đặt

Yêu Cầu

Thêm Reference

🚀 Hướng Dẫn Sử Dụng Cơ Bản

1. Khởi Tạo MapBuilder

2. Tạo Trajectory

3. Đưa Dữ Liệu Sensor

Dữ liệu Lidar/Laser

Dữ liệu IMU

Dữ liệu Odometry

4. Kết Thúc và Tối Ưu

⚙️ Cấu Hình Chi Tiết

MapBuilderOptions

TrajectoryBuilder2DOptions

PoseGraphOptions

💡 Các Tình Huống Sử Dụng

Ví Dụ 1: SLAM 2D Với Lidar

Ví Dụ 2: SLAM 2D Với Lidar + IMU + Odometry

Ví Dụ 3: Load và Tiếp Tục SLAM

🔌 Tích Hợp Sensor

Định Nghĩa Sensor ID

Chuẩn Bị Dữ Liệu

Point Cloud

IMU Data

📚 API Reference

IMapBuilder

ITrajectoryBuilder

IPoseGraph

🐛 Troubleshooting

Vấn Đề 1: Scan Matching Thất Bại

Vấn Đề 2: Bộ Nhớ Tăng Cao

Vấn Đề 3: Loop Closure Không Hoạt Động

Vấn Đề 4: Timestamp Issues

🤖 Dành Cho AI Agents

Cấu Trúc Thư Mục

Khái Niệm Quan Trọng

Refactoring Notes

Common Tasks

1. Thêm Sensor Mới

2. Điều Chỉnh Cost Function

Debugging Tips

📖 Tài Liệu Bổ Sung

📝 License

30 KiB

Raw Blame History