QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
921bb91084bea5e308faa1d219837ebdc9abd6b8
BQP/docs/CartographerSharp/CERES_USAGE.md
QUYVN bccfb156d7 Initial commit
2026-07-13 09:25:40 +07:00

14 KiB
Raw Blame History

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:

// 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:

// 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:

// 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

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

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

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

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

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

ceres::AutoDiffLocalParameterization<Functor, params, tangent_size>

Sử dụng: Custom local parameterizations với auto-differentiation

5. Interpolation

ceres::BiCubicInterpolator

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:

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)

  1. CeresScanMatcher3D
    • Similar to 2D nhưng với 3D transforms

Pose Graph Optimization (2D)

  1. AutoDiffSpaCostFunction2D

    • File: spa_cost_function_2d.h/cc
    • Purpose: Constraint giữa submap và node poses
    • Uses: AutoDiffCostFunction

  2. AnalyticalSpaCostFunction2D

    • File: spa_cost_function_2d.h/cc
    • Purpose: Analytical version (nhanh hơn)
    • Uses: CostFunction (manual derivatives)

  3. LandmarkCostFunction2D

    • File: landmark_cost_function_2d.h
    • Purpose: Constrain landmarks
    • Uses: AutoDiffCostFunction

Pose Graph Optimization (3D)

  1. SpaCostFunction3D

    • File: spa_cost_function_3d.h
    • Purpose: 3D pose constraints
    • Uses: Quaternion parameterization

  2. LandmarkCostFunction3D

    • File: landmark_cost_function_3d.h
    • Purpose: 3D landmark constraints

  3. RotationCostFunction3D

    • File: rotation_cost_function_3d.h
    • Purpose: Rotation constraints trong 3D

  4. AccelerationCostFunction3D

    • File: acceleration_cost_function_3d.h
    • Purpose: Acceleration constraints cho smooth trajectories

IMU Extrapolation

  1. 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 để 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 View Git Blame Copy Permalink