414 lines
14 KiB
Markdown
414 lines
14 KiB
Markdown
# 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)
|