1055 lines
30 KiB
Markdown
1055 lines
30 KiB
Markdown
# CartographerSharp - Hướng Dẫn Sử Dụng Chi Tiết
|
|
|
|
CartographerSharp là phiên bản C# của [Google Cartographer](https://github.com/cartographer-project/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.
|
|
|
|
## 📋 Mục Lục
|
|
|
|
1. [Kiến Trúc Tổng Quan](#-kiến-trúc-tổng-quan)
|
|
2. [Các Interface Chính](#-các-interface-chính)
|
|
3. [Cài Đặt](#-cài-đặt)
|
|
4. [Hướng Dẫn Sử Dụng Cơ Bản](#-hướng-dẫn-sử-dụng-cơ-bản)
|
|
5. [Cấu Hình Chi Tiết](#-cấu-hình-chi-tiết)
|
|
6. [Các Tình Huống Sử Dụng](#-các-tình-huống-sử-dụng)
|
|
7. [Tích Hợp Sensor](#-tích-hợp-sensor)
|
|
8. [API Reference](#-api-reference)
|
|
9. [Troubleshooting](#-troubleshooting)
|
|
10. [Dành Cho AI Agents](#-dành-cho-ai-agents)
|
|
|
|
---
|
|
|
|
## 🏗 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`.
|
|
|
|
```mermaid
|
|
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
|
|
|
|
```mermaid
|
|
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
|
|
|
|
```xml
|
|
<!-- 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:
|
|
|
|
```xml
|
|
<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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
// Đị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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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:
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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
|
|
|
|
```csharp
|
|
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**:
|
|
|
|
```csharp
|
|
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**:
|
|
|
|
```csharp
|
|
// 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**:
|
|
|
|
```csharp
|
|
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ộ:
|
|
|
|
```csharp
|
|
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
|
|
|
|
1. **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
|
|
|
|
2. **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
|
|
|
|
3. **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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
```csharp
|
|
// 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
|
|
|
|
Copyright 2016 The Cartographer Authors
|
|
|
|
Licensed under the Apache License, Version 2.0
|
|
|
|
---
|
|
|
|
**Được duy trì bởi RobotNet10 Team**
|