BQP/docs/CartographerSharp/CERES_INTEGRATION_TASKS.md

CeresSharp Integration Tasks - Chi tiết Công việc

📋 Tổng quan

Tài liệu này mô tả cụ thể những gì cần làm để tích hợp CeresSharp vào CartographerSharp, hoàn thiện các components còn thiếu.

✅ Trạng thái hiện tại

• CeresSharp: ✅ Đã có sẵn (100% complete - 216+ APIs)
• Ceres Solver Version: 2.2.0
• CartographerSharp: ⏳ Đang chờ CeresSharp integration

🎯 Nơi Triển Khai: CartographerSharp

Quan trọng: Tất cả implementation sẽ được làm trong CartographerSharp, không phải CeresSharp.

Lý do:

• CeresSharp = Generic optimization library (cung cấp building blocks)
• CartographerSharp = Application layer (sử dụng CeresSharp để implement Cartographer algorithms)
• Các cost functions là Cartographer-specific, không phải generic Ceres functionality

Cấu trúc Files:

CartographerSharp/
├── Mapping/
│   ├── Internal/
│   │   ├── 2D/
│   │   │   └── ScanMatching/
│   │   │       ├── CeresScanMatcher2D.cs ✅ (skeleton)
│   │   │       ├── OccupiedSpaceCostFunction2D.cs ⏳ (cần implement)
│   │   │       ├── TranslationDeltaCostFunctor2D.cs ⏳ (cần implement)
│   │   │       └── RotationDeltaCostFunctor2D.cs ⏳ (cần implement)
│   │   └── Optimization/
│   │       ├── OptimizationProblem2D.cs ✅ (skeleton)
│   │       └── SpaCostFunction2D.cs ⏳ (cần implement)
│   └── ...
└── CartographerSharp.csproj ⏳ (cần thêm ProjectReference đến CeresSharp)

📦 Dependencies

1. Project Reference

File: CartographerSharp.csproj

Thêm reference đến CeresSharp:

<Project Sdk="Microsoft.NET.Sdk">
  <PropertyGroup>
    <TargetFramework>net10.0</TargetFramework>
    <ImplicitUsings>enable</ImplicitUsings>
    <Nullable>enable</Nullable>
    <LangVersion>preview</LangVersion>
  </PropertyGroup>

  <ItemGroup>
    <!-- Reference to CeresSharp -->
    <ProjectReference Include="../CeresSharp/CeresSharp.csproj" />
    
    <!-- Existing dependencies -->
    <PackageReference Include="Microsoft.Extensions.Logging" Version="10.0.0" />
    <PackageReference Include="NLog.Extensions.Logging" Version="5.4.0" />
    <PackageReference Include="SkiaSharp" Version="2.88.9" />
  </ItemGroup>
</Project>

2. Using Directives

Thêm vào các files cần dùng CeresSharp:

using CeresSharp;
using CeresSharp.Core;
using CeresSharp.Enums;

---

🎯 Các Components Cần Hoàn Thiện

1. CeresScanMatcher2D ⏳

File: Mapping/Internal/2D/ScanMatching/CeresScanMatcher2D.cs

Trạng thái hiện tại: Skeleton implementation với TODOs

Cần implement:

1.1. Cost Functions cho Scan Matching

a) OccupiedSpaceCostFunction2D

File mới: Mapping/Internal/2D/ScanMatching/OccupiedSpaceCostFunction2D.cs

Mục đích: Tính cost dựa trên occupied space trong grid

C++ Reference: refs/cartographer/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h/cc

Cần làm:

1. Tạo class OccupiedSpaceCostFunction2D implement CeresSharp.CostFunction
2. Sử dụng CeresSharp.BiCubicInterpolator để interpolate grid values
3. Transform point cloud points theo pose estimate
4. Tính residual = 1.0 - interpolated_probability cho mỗi point
5. Weight = occupied_space_weight / sqrt(point_cloud.size())

CeresSharp APIs cần dùng:

• CeresSharp.BiCubicInterpolator - Cho grid interpolation
• CeresSharp.AutoDiffCostFunction - Cho automatic differentiation
• CeresSharp.Problem.AddResidualBlock() - Thêm cost function vào problem

Code structure:

// File: Mapping/Internal/2D/ScanMatching/OccupiedSpaceCostFunction2D.cs
using CeresSharp;
using CeresSharp.Core;
using CartographerSharp.Mapping.D2D;
using CartographerSharp.Sensor;

namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;

public class OccupiedSpaceCostFunction2D : CostFunction
{
    private readonly BiCubicInterpolator _interpolator;
    private readonly PointCloud _pointCloud;
    private readonly double _weight;
    
    // Implement Evaluate() method
    // Transform points, interpolate, compute residuals
}

b) TranslationDeltaCostFunctor2D

File mới: Mapping/Internal/2D/ScanMatching/TranslationDeltaCostFunctor2D.cs

Mục đích: Penalize translation deviation từ target translation

C++ Reference: refs/cartographer/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h/cc

Cần làm:

1. Tạo functor class với target_translation và weight
2. Residual = weight * (current_translation - target_translation)
3. Sử dụng AutoDiffCostFunction với 3 parameters (x, y, theta)

Code structure:

// File: Mapping/Internal/2D/ScanMatching/TranslationDeltaCostFunctor2D.cs
using System.Numerics;
using CeresSharp;
using CeresSharp.Core;

namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;

public class TranslationDeltaCostFunctor2D
{
    private readonly Vector2 _targetTranslation;
    private readonly double _weight;
    
    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
        double scalingFactor, Vector2 targetTranslation)
    {
        return new AutoDiffCostFunction<TranslationDeltaCostFunctor2D, 2, 3>(
            new TranslationDeltaCostFunctor2D(scalingFactor, targetTranslation)
        );
    }
    
    public void Evaluate(double[] parameters, double[] residuals, double[][] jacobians)
    {
        // parameters[0] = x, parameters[1] = y
        // residuals[0] = weight * (x - targetX)
        // residuals[1] = weight * (y - targetY)
    }
}

c) RotationDeltaCostFunctor2D

File mới: Mapping/Internal/2D/ScanMatching/RotationDeltaCostFunctor2D.cs

Mục đích: Penalize rotation deviation từ initial rotation

C++ Reference: refs/cartographer/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h/cc

Cần làm:

1. Tạo functor class với initial_rotation và weight
2. Residual = weight * (current_rotation - initial_rotation)
3. Sử dụng AutoDiffCostFunction với 1 parameter (theta)

Code structure:

// File: Mapping/Internal/2D/ScanMatching/RotationDeltaCostFunctor2D.cs
using CeresSharp;
using CeresSharp.Core;

namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;

public class RotationDeltaCostFunctor2D
{
    private readonly double _initialRotation;
    private readonly double _weight;
    
    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
        double scalingFactor, double targetAngle)
    {
        return new AutoDiffCostFunction<RotationDeltaCostFunctor2D, 1, 3>(
            new RotationDeltaCostFunctor2D(scalingFactor, targetAngle)
        );
    }
    
    public void Evaluate(double[] parameters, double[] residuals, double[][] jacobians)
    {
        // parameters[0] = theta
        // residuals[0] = weight * (theta - initialRotation)
    }
}

1.2. CeresScanMatcher2D.Match() Implementation

File: Mapping/Internal/2D/ScanMatching/CeresScanMatcher2D.cs

Cần làm:

1. Khởi tạo CeresSharp.Problem
2. Setup CeresSharp.SolverOptions:
   • LinearSolverType = LinearSolverType.DenseQr (cho 2D scan matching)
   • Configure từ CeresSolverOptions proto
3. Tạo parameter block: double[3] = [x, y, theta]
4. Add cost functions:
   • OccupiedSpaceCostFunction2D (cho ProbabilityGrid hoặc TSDF2D)
   • TranslationDeltaCostFunctor2D
   • RotationDeltaCostFunctor2D
5. Solve: CeresSharp.Solver.Solve(options, problem, out summary)
6. Extract result: poseEstimate = new Rigid2d(x, y, theta)

---

2. OptimizationProblem2D ⏳

File: Mapping/Internal/Optimization/OptimizationProblem2D.cs

Trạng thái hiện tại: Skeleton implementation với data structures

Cần implement:

2.1. Cost Functions cho Pose Graph Optimization

a) SpaCostFunction2D

File mới: Mapping/Internal/Optimization/SpaCostFunction2D.cs

Mục đích: Sparse Pose Adjustment (SPA) cost function cho constraints

C++ Reference: refs/cartographer/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h/cc

Cần làm:

1. Tạo SpaCostFunction2D class
2. Residual = relative_pose - (submap_pose^-1 * node_pose)
3. Weight bằng translation_weight và rotation_weight từ constraint
4. Sử dụng HuberLoss cho loop closure constraints (robust với outliers)

2.2. OptimizationProblem2D.Solve() Implementation

File: Mapping/Internal/Optimization/OptimizationProblem2D.cs

Cần làm:

1. Khởi tạo CeresSharp.Problem
2. Setup CeresSharp.SolverOptions:
   • LinearSolverType = LinearSolverType.SparseSchur (cho large problems)
   • Configure từ OptimizationProblemOptions
3. Add parameter blocks:
   • Mỗi submap: double[3] = [x, y, theta]
   • Mỗi node: double[3] = [x, y, theta]
4. Set frozen trajectories: problem.SetParameterBlockConstant(poseParams)
5. Add constraints:
   • Loop closure constraints: Sử dụng HuberLoss
   • Intra-submap constraints: Không dùng loss function
6. Solve: Solver.Solve(options, problem, out summary)
7. Update poses: Extract từ parameter blocks và update _submapData và _nodeData

---

🔧 Migration Notes - Ceres 2.2.0

Deprecated APIs

Cartographer C++ sử dụng Ceres cũ với các APIs đã deprecated:

API Cũ (Cartographer)	API Mới (Ceres 2.2.0)	Status trong CeresSharp
ceres::QuaternionParameterization	ceres::QuaternionManifold	✅ Có sẵn
ceres::LocalParameterization	ceres::Manifold	✅ Có sẵn
ceres::AutoDiffLocalParameterization	ceres::AutoDiffManifold	✅ Có sẵn
problem.SetParameterization()	problem.SetManifold()	✅ Có sẵn

Lưu ý: Khi implement, sử dụng Manifold APIs thay vì LocalParameterization (nếu cần cho 3D).

---

📝 Checklist Implementation

Phase 1: Setup Dependencies ✅

• Thêm ProjectReference đến CeresSharp trong CartographerSharp.csproj
• Verify build thành công với CeresSharp reference

Phase 2: Cost Functions ✅

• Implement OccupiedSpaceCostFunction2D.cs - Complete với BiCubicInterpolator integration
• Implement TranslationDeltaCostFunctor2D.cs - Complete với AutoDiffCostFunction
• Implement RotationDeltaCostFunctor2D.cs - Complete với AutoDiffCostFunction
• Implement ProbabilityGridAdapter cho BiCubicInterpolator - Complete với padding và grid data conversion
• Tests cho cost functions (có thể làm sau khi integrate vào CeresScanMatcher2D)

Phase 3: CeresScanMatcher2D ✅

• Complete CeresScanMatcher2D.Match() method - Complete với Problem setup, cost functions integration, và Solver
• Initialize SolverOptions với DENSE_QR linear solver cho 2D scan matching
• Integrate với LocalTrajectoryBuilder2D - Đã có integration, signature đã match
• Tests cho scan matching (có thể làm sau)

Phase 4: OptimizationProblem2D ✅

• Implement SpaCostFunction2D.cs - Complete với AutoDiffCostFunction, ComputeUnscaledError, ScaleError
• Complete OptimizationProblem2D.Solve() method - Complete với Problem setup, parameter blocks, constraints, frozen trajectories
• Handle frozen trajectories - Complete với SetParameterBlockConstant
• Integrate với PoseGraph2D.RunFinalOptimization() - Complete với data sync
• Tests cho pose graph optimization (có thể làm sau)

Phase 5: Integration ✅

• Update PoseGraph2D.RunFinalOptimization() để gọi OptimizationProblem2D.Solve() - Complete với data sync và pose updates
• End-to-end tests (optional - có thể làm sau)
• Performance benchmarks (optional - có thể làm sau)

---

🎯 Kết quả Mong đợi

Sau khi hoàn thành:

1. ✅ CeresScanMatcher2D: Fine alignment của scans với submap grids
2. ✅ OptimizationProblem2D: Global pose graph optimization với loop closure
3. ✅ CartographerSharp: Hoàn thiện Phase 3 (100%)

---

📚 Tài liệu Tham khảo

• CERES_USAGE.md - Chi tiết cách Ceres được sử dụng trong Cartographer
• CERES_READINESS_EVALUATION.md - Đánh giá mức độ sẵn sàng của CeresSharp
• refs/cartographer/ - C++ source code reference
• Ceres Solver Documentation