Initial commit
This commit is contained in:
413
docs/CartographerSharp/CERES_USAGE.md
Normal file
413
docs/CartographerSharp/CERES_USAGE.md
Normal file
@@ -0,0 +1,413 @@
|
||||
# Ceres Solver Usage trong Cartographer - Tổng hợp Chi tiết
|
||||
|
||||
## 📊 Tổng quan
|
||||
|
||||
Ceres Solver là **thư viện optimization chính** của Cartographer, được sử dụng trong **128 dòng code** và là thành phần **không thể thiếu** cho SLAM algorithm.
|
||||
|
||||
---
|
||||
|
||||
## 🎯 Các Module sử dụng Ceres Solver
|
||||
|
||||
### 1. **Scan Matching (2D và 3D)**
|
||||
**Mục đích**: Khớp laser scans với map hiện tại để tìm vị trí tốt nhất của robot
|
||||
|
||||
#### Files liên quan:
|
||||
- `mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h/cc`
|
||||
- `mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h/cc`
|
||||
- `mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h/cc`
|
||||
- `mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h`
|
||||
- `mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h`
|
||||
- `mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h/cc`
|
||||
|
||||
#### Ceres APIs được sử dụng:
|
||||
```cpp
|
||||
// Core classes
|
||||
ceres::Problem // Tạo optimization problem
|
||||
ceres::Solver // Nonlinear solver
|
||||
ceres::Solver::Options // Solver configuration
|
||||
ceres::Solver::Summary // Solver results
|
||||
|
||||
// Cost Functions
|
||||
ceres::AutoDiffCostFunction // Automatic differentiation
|
||||
ceres::CostFunction // Base class for cost functions
|
||||
|
||||
// Interpolation
|
||||
ceres::BiCubicInterpolator // Bicubic interpolation cho grid
|
||||
ceres::CubicInterpolator // Cubic interpolation
|
||||
|
||||
// Linear Solver
|
||||
ceres::DENSE_QR // Dense QR linear solver
|
||||
```
|
||||
|
||||
#### Chi tiết sử dụng:
|
||||
1. **Occupied Space Cost Function** - Tính cost dựa trên độ khớp giữa point cloud và grid
|
||||
- Sử dụng `ceres::BiCubicInterpolator` để interpolate grid values
|
||||
- Tạo `ceres::AutoDiffCostFunction` với dynamic residuals
|
||||
|
||||
2. **TSDF Match Cost Function** - Tương tự cho TSDF grid
|
||||
- Sử dụng TSDF values thay vì probability values
|
||||
|
||||
3. **Translation/Rotation Delta Cost Functions** - Ràng buộc để giữ pose gần với initial estimate
|
||||
- Translation delta: Giữ translation gần với target
|
||||
- Rotation delta: Giữ rotation gần với initial angle
|
||||
|
||||
### 2. **Pose Graph Optimization (2D và 3D)**
|
||||
**Mục đích**: Optimize toàn bộ map, giải quyết loop closures và constraints
|
||||
|
||||
#### Files liên quan:
|
||||
- `mapping/internal/optimization/optimization_problem_2d.cc`
|
||||
- `mapping/internal/optimization/optimization_problem_3d.cc`
|
||||
- `mapping/internal/optimization/cost_functions/spa_cost_function_2d.h/cc`
|
||||
- `mapping/internal/optimization/cost_functions/spa_cost_function_3d.h`
|
||||
- `mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h`
|
||||
- `mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h`
|
||||
- `mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h`
|
||||
- `mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h`
|
||||
|
||||
#### Ceres APIs được sử dụng:
|
||||
```cpp
|
||||
// Core
|
||||
ceres::Problem::Options // Problem configuration
|
||||
ceres::Problem // Optimization problem container
|
||||
ceres::Solver::Options // Solver options
|
||||
ceres::Solver::Summary // Optimization summary
|
||||
|
||||
// Parameter Blocks
|
||||
problem.AddParameterBlock() // Thêm parameter blocks
|
||||
problem.SetParameterBlockConstant() // Fix parameters (frozen trajectories)
|
||||
|
||||
// Cost Functions
|
||||
ceres::AutoDiffCostFunction // Auto differentiation
|
||||
ceres::CostFunction // Base cost function
|
||||
|
||||
// Loss Functions
|
||||
ceres::HuberLoss // Robust loss function cho loop closures
|
||||
|
||||
// Parameterizations
|
||||
ceres::QuaternionParameterization // Quaternion parameterization (3D)
|
||||
ceres::LocalParameterization // Custom local parameterization
|
||||
ceres::AutoDiffLocalParameterization // Auto-diff local parameterization
|
||||
```
|
||||
|
||||
#### Chi tiết sử dụng:
|
||||
1. **SPA Cost Function** (Sparse Pose Adjustment)
|
||||
- 2D: `CreateAutoDiffSpaCostFunction`, `CreateAnalyticalSpaCostFunction`
|
||||
- 3D: Pose constraints giữa submaps và nodes
|
||||
- Sử dụng `ceres::HuberLoss` cho loop closure constraints (robust với outliers)
|
||||
|
||||
2. **Landmark Cost Functions**
|
||||
- 2D: `landmark_cost_function_2d.h`
|
||||
- 3D: `landmark_cost_function_3d.h`
|
||||
- Constrain landmarks với trajectory nodes
|
||||
|
||||
3. **Rotation Cost Function (3D)**
|
||||
- Constrain rotations trong 3D optimization
|
||||
|
||||
4. **Acceleration Cost Function (3D)**
|
||||
- Constrain acceleration cho smooth trajectories
|
||||
|
||||
5. **Parameter Management**
|
||||
- Submaps: 3 parameters (x, y, angle) cho 2D
|
||||
- Nodes: 3 parameters (x, y, angle) cho 2D
|
||||
- 3D: 7 parameters (3 translation + 4 quaternion) per pose
|
||||
- Frozen trajectories: Set parameter blocks constant
|
||||
|
||||
### 3. **IMU-based Pose Extrapolation**
|
||||
**Mục đích**: Dự đoán vị trí robot giữa các scans sử dụng IMU data
|
||||
|
||||
#### Files liên quan:
|
||||
- `mapping/internal/imu_based_pose_extrapolator.h/cc`
|
||||
- `mapping/internal/optimization/ceres_pose.h/cc`
|
||||
|
||||
#### Ceres APIs được sử dụng:
|
||||
```cpp
|
||||
// Core
|
||||
ceres::Problem
|
||||
ceres::Solver::Options
|
||||
ceres::Solver::Summary
|
||||
|
||||
// Pose Representation
|
||||
ceres::LocalParameterization
|
||||
ceres::QuaternionParameterization
|
||||
ceres::AutoDiffLocalParameterization
|
||||
|
||||
// Cost Functions
|
||||
ceres::AutoDiffCostFunction
|
||||
```
|
||||
|
||||
#### Chi tiết sử dụng:
|
||||
1. **CeresPose Class**
|
||||
- Wrapper cho pose trong Ceres problem
|
||||
- Translation: `std::array<double, 3>`
|
||||
- Rotation: `std::array<double, 4>` (quaternion w, x, y, z)
|
||||
- Sử dụng `ceres::QuaternionParameterization` để maintain quaternion constraints
|
||||
|
||||
2. **IMU Constraints**
|
||||
- Optimize gravity vector
|
||||
- Constrain IMU nodes với quaternion parameterization
|
||||
- Sử dụng `ceres::AutoDiffLocalParameterization` cho custom constraints
|
||||
|
||||
---
|
||||
|
||||
## 🔧 Chi tiết Ceres APIs được sử dụng
|
||||
|
||||
### 1. **Core Classes**
|
||||
|
||||
#### `ceres::Problem`
|
||||
```cpp
|
||||
ceres::Problem problem;
|
||||
problem.AddResidualBlock(cost_function, loss_function, parameter_blocks...);
|
||||
problem.AddParameterBlock(parameters, size);
|
||||
problem.SetParameterBlockConstant(parameters);
|
||||
```
|
||||
**Sử dụng**: Container chính cho optimization problem, chứa tất cả cost functions và parameters.
|
||||
|
||||
#### `ceres::Solver`
|
||||
```cpp
|
||||
ceres::Solver::Options options;
|
||||
options.linear_solver_type = ceres::DENSE_QR; // 2D scan matching
|
||||
options.max_num_iterations = 50;
|
||||
options.num_threads = 4;
|
||||
// ... nhiều options khác
|
||||
|
||||
ceres::Solver::Summary summary;
|
||||
ceres::Solve(options, &problem, &summary);
|
||||
```
|
||||
**Sử dụng**:
|
||||
- **Scan Matching**: `DENSE_QR` solver (nhỏ, nhanh)
|
||||
- **Pose Graph Optimization**: Sparse solver (lớn, hiệu quả)
|
||||
- Configuration từ `CeresSolverOptions` proto
|
||||
|
||||
### 2. **Cost Functions**
|
||||
|
||||
#### `ceres::AutoDiffCostFunction`
|
||||
```cpp
|
||||
ceres::AutoDiffCostFunction<Functor, residuals, params...>
|
||||
```
|
||||
**Sử dụng**: Automatic differentiation - không cần tính derivatives manually
|
||||
- `OccupiedSpaceCostFunction2D`
|
||||
- `TSDFMatchCostFunction2D`
|
||||
- `TranslationDeltaCostFunctor2D`
|
||||
- `RotationDeltaCostFunctor2D`
|
||||
- `SpaCostFunction` (2D và 3D)
|
||||
|
||||
#### Custom Cost Functions
|
||||
- Dynamic residuals (số lượng points trong point cloud)
|
||||
- Multi-parameter blocks (submap + node poses)
|
||||
|
||||
### 3. **Loss Functions**
|
||||
|
||||
#### `ceres::HuberLoss`
|
||||
```cpp
|
||||
new ceres::HuberLoss(huber_scale)
|
||||
```
|
||||
**Sử dụng**: Robust loss function cho loop closure constraints
|
||||
- Giảm ảnh hưởng của outliers
|
||||
- Dùng trong `OptimizationProblem2D` và `OptimizationProblem3D`
|
||||
- Chỉ áp dụng cho `INTER_SUBMAP` constraints
|
||||
|
||||
### 4. **Parameterizations**
|
||||
|
||||
#### `ceres::QuaternionParameterization`
|
||||
```cpp
|
||||
absl::make_unique<ceres::QuaternionParameterization>()
|
||||
```
|
||||
**Sử dụng**:
|
||||
- Maintain quaternion constraints (unit quaternion) trong 3D
|
||||
- Sử dụng trong `CeresPose` cho 3D optimization
|
||||
- Đảm bảo quaternion luôn normalized
|
||||
|
||||
#### `ceres::AutoDiffLocalParameterization`
|
||||
```cpp
|
||||
ceres::AutoDiffLocalParameterization<Functor, params, tangent_size>
|
||||
```
|
||||
**Sử dụng**: Custom local parameterizations với auto-differentiation
|
||||
|
||||
### 5. **Interpolation**
|
||||
|
||||
#### `ceres::BiCubicInterpolator`
|
||||
```cpp
|
||||
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
|
||||
interpolator.Evaluate(x, y, &value, &gradient_x, &gradient_y);
|
||||
```
|
||||
**Sử dụng**:
|
||||
- Interpolate grid values trong `OccupiedSpaceCostFunction2D`
|
||||
- Tính gradients cho optimization
|
||||
- Smooth interpolation cho probability/TSDF grids
|
||||
|
||||
#### `ceres::CubicInterpolator`
|
||||
**Sử dụng**: 1D cubic interpolation (nếu cần)
|
||||
|
||||
### 6. **Solver Options**
|
||||
|
||||
Configuration từ `common/proto/ceres_solver_options.proto`:
|
||||
```protobuf
|
||||
message CeresSolverOptions {
|
||||
int32 use_nonmonotonic_steps = 1;
|
||||
int32 max_num_iterations = 2;
|
||||
int32 num_threads = 3;
|
||||
double initial_trust_region_radius = 4;
|
||||
double max_trust_region_radius = 5;
|
||||
double min_trust_region_radius = 6;
|
||||
double min_relative_decrease = 7;
|
||||
double max_num_consecutive_invalid_steps = 8;
|
||||
double function_tolerance = 9;
|
||||
double gradient_tolerance = 10;
|
||||
double parameter_tolerance = 11;
|
||||
string linear_solver_type = 12;
|
||||
// ... và nhiều options khác
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📋 Danh sách đầy đủ các Cost Functions
|
||||
|
||||
### Scan Matching (2D)
|
||||
1. **OccupiedSpaceCostFunction2D**
|
||||
- File: `occupied_space_cost_function_2d.h/cc`
|
||||
- Purpose: Match point cloud với probability grid
|
||||
- Uses: `BiCubicInterpolator`, `AutoDiffCostFunction`
|
||||
|
||||
2. **TSDFMatchCostFunction2D**
|
||||
- File: `tsdf_match_cost_function_2d.h/cc`
|
||||
- Purpose: Match point cloud với TSDF grid
|
||||
- Uses: `AutoDiffCostFunction`
|
||||
|
||||
3. **TranslationDeltaCostFunctor2D**
|
||||
- File: `translation_delta_cost_functor_2d.h`
|
||||
- Purpose: Constrain translation gần với target
|
||||
- Uses: `AutoDiffCostFunction`
|
||||
|
||||
4. **RotationDeltaCostFunctor2D**
|
||||
- File: `rotation_delta_cost_functor_2d.h`
|
||||
- Purpose: Constrain rotation gần với initial angle
|
||||
- Uses: `AutoDiffCostFunction`
|
||||
|
||||
### Scan Matching (3D)
|
||||
5. **CeresScanMatcher3D**
|
||||
- Similar to 2D nhưng với 3D transforms
|
||||
|
||||
### Pose Graph Optimization (2D)
|
||||
6. **AutoDiffSpaCostFunction2D**
|
||||
- File: `spa_cost_function_2d.h/cc`
|
||||
- Purpose: Constraint giữa submap và node poses
|
||||
- Uses: `AutoDiffCostFunction`
|
||||
|
||||
7. **AnalyticalSpaCostFunction2D**
|
||||
- File: `spa_cost_function_2d.h/cc`
|
||||
- Purpose: Analytical version (nhanh hơn)
|
||||
- Uses: `CostFunction` (manual derivatives)
|
||||
|
||||
8. **LandmarkCostFunction2D**
|
||||
- File: `landmark_cost_function_2d.h`
|
||||
- Purpose: Constrain landmarks
|
||||
- Uses: `AutoDiffCostFunction`
|
||||
|
||||
### Pose Graph Optimization (3D)
|
||||
9. **SpaCostFunction3D**
|
||||
- File: `spa_cost_function_3d.h`
|
||||
- Purpose: 3D pose constraints
|
||||
- Uses: Quaternion parameterization
|
||||
|
||||
10. **LandmarkCostFunction3D**
|
||||
- File: `landmark_cost_function_3d.h`
|
||||
- Purpose: 3D landmark constraints
|
||||
|
||||
11. **RotationCostFunction3D**
|
||||
- File: `rotation_cost_function_3d.h`
|
||||
- Purpose: Rotation constraints trong 3D
|
||||
|
||||
12. **AccelerationCostFunction3D**
|
||||
- File: `acceleration_cost_function_3d.h`
|
||||
- Purpose: Acceleration constraints cho smooth trajectories
|
||||
|
||||
### IMU Extrapolation
|
||||
13. **IMU Cost Functions**
|
||||
- Various cost functions cho gravity, velocity constraints
|
||||
- Uses: `CeresPose`, `QuaternionParameterization`
|
||||
|
||||
---
|
||||
|
||||
## 📊 Thống kê Sử dụng
|
||||
|
||||
### Phân bố theo Module:
|
||||
- **Scan Matching**: ~40% code sử dụng Ceres
|
||||
- **Pose Graph Optimization**: ~45% code sử dụng Ceres
|
||||
- **IMU Extrapolation**: ~10% code sử dụng Ceres
|
||||
- **Utilities**: ~5% (configuration, helper classes)
|
||||
|
||||
### Số lượng Cost Functions:
|
||||
- **2D**: 8 cost functions
|
||||
- **3D**: 6 cost functions
|
||||
- **Common**: 3 cost functions (landmarks, etc.)
|
||||
|
||||
### Solver Types:
|
||||
- **DENSE_QR**: Scan matching (nhỏ, real-time)
|
||||
- **SPARSE_SCHUR**: Pose graph optimization (lớn, hiệu quả)
|
||||
|
||||
---
|
||||
|
||||
## ⚠️ Thách thức khi Chuyển đổi sang C#
|
||||
|
||||
### 1. **Core APIs phải có:**
|
||||
- ✅ `Problem` - Container cho optimization
|
||||
- ✅ `Solver` - Nonlinear solver
|
||||
- ✅ `AutoDiffCostFunction` - Automatic differentiation
|
||||
- ✅ `CostFunction` - Base class
|
||||
- ✅ `LossFunction` (HuberLoss) - Robust loss
|
||||
- ✅ `Parameterization` (QuaternionParameterization) - Constraint handling
|
||||
- ✅ `BiCubicInterpolator` - Grid interpolation
|
||||
|
||||
### 2. **Features quan trọng:**
|
||||
- **Dynamic residuals** - Số lượng points trong point cloud không cố định
|
||||
- **Multi-parameter blocks** - Nhiều poses cùng optimize
|
||||
- **Parameter constraints** - Fix certain parameters
|
||||
- **Robust optimization** - Huber loss cho outliers
|
||||
|
||||
### 3. **Performance Requirements:**
|
||||
- Real-time scan matching (milliseconds)
|
||||
- Large-scale pose graph optimization (hàng nghìn nodes)
|
||||
- Efficient sparse solvers cho large problems
|
||||
|
||||
---
|
||||
|
||||
## 💡 Kết luận
|
||||
|
||||
Ceres Solver là **thành phần CORE không thể thiếu** của Cartographer:
|
||||
|
||||
1. **Scan Matching**: Cần cho local SLAM - tìm vị trí robot
|
||||
2. **Pose Graph Optimization**: Cần cho global SLAM - optimize toàn bộ map
|
||||
3. **IMU Integration**: Cần cho pose extrapolation
|
||||
|
||||
**Không có Ceres Solver = Không có SLAM algorithm**
|
||||
|
||||
Khi chuyển đổi sang C#, cần:
|
||||
- ✅ Quyết định phương án thay thế sớm
|
||||
- ✅ Đảm bảo có đầy đủ APIs cần thiết
|
||||
- ✅ Test performance để đảm bảo real-time requirements
|
||||
|
||||
---
|
||||
|
||||
---
|
||||
|
||||
## 📊 API Coverage Analysis
|
||||
|
||||
### Coverage với C API + CeresWrapper
|
||||
|
||||
Xem **[CERES_COVERAGE_ANALYSIS.md](./CERES_COVERAGE_ANALYSIS.md)** để biết:
|
||||
- ✅ APIs đã có (C API + CeresWrapper)
|
||||
- ❌ APIs còn thiếu (Critical cho Cartographer)
|
||||
- 📊 Coverage statistics
|
||||
- 🎯 Khuyến nghị implementation roadmap
|
||||
|
||||
### Kết luận nhanh:
|
||||
- **Current Coverage**: ~75% - **KHÔNG ĐỦ** cho full Cartographer
|
||||
- **Critical Missing**: DynamicAutoDiffCostFunction, ProductManifold, Problem Query Methods, IterationCallback
|
||||
- **Recommendation**: Implement Phase 1 APIs (~6-10 hours) trước khi có thể implement đầy đủ CartographerSharp
|
||||
|
||||
---
|
||||
|
||||
**Last Updated**: Generated for Cartographer C# Port
|
||||
**Status**: Detailed analysis complete
|
||||
**Priority**: ⭐⭐⭐⭐⭐ (Critical)
|
||||
Reference in New Issue
Block a user