QUYVN/BQP

Fork 0

Files

QUYVN bccfb156d7 Initial commit

2026-07-13 09:25:40 +07:00

22 KiB

Raw Blame History

TSDF2D Support - Implementation Plan

Status: ✅ IMPLEMENTATION COMPLETED - All phases implemented with comprehensive unit tests

📋 Tổng Quan

TSDF (Truncated Signed Distance Function) 2D là một loại grid khác ngoài ProbabilityGrid cho 2D SLAM. TSDF lưu trữ:

TSD (Truncated Signed Distance): Khoảng cách có dấu tới bề mặt, được truncate trong phạm vi [-truncation_distance, truncation_distance]
Weight: Trọng số của measurement, sử dụng để tích hợp nhiều measurements

Ưu điểm của TSDF so với ProbabilityGrid:

Hỗ trợ subpixel accuracy tốt hơn
Xử lý uncertainty tốt hơn với weighted integration
Có thể extract surface với độ chính xác cao hơn

🎯 Components Cần Implement

1. TSDValueConverter (Core Utility)

2. TSDF2D Grid (Grid Implementation)

3. TSDFRangeDataInserter2D (Range Data Inserter)

4. NormalEstimation2D (Normal Estimation Utility)

5. InterpolatedTSDF2D (Interpolation for Scan Matching)

6. TSDFMatchCostFunction2D (Ceres Cost Function)

7. Proto Definitions (Configuration & Serialization)

📝 Chi Tiết Implementation

Phase 1: Core Utilities

1.1 TSDValueConverter

File: Mapping/Internal/2D/TSDValueConverter.cs

Purpose: Convert giữa TSD/Weight values và ushort values để lưu trữ hiệu quả trong grid.

Methods cần implement:

public class TSDValueConverter
{
    public TSDValueConverter(float maxTSD, float maxWeight, ValueConversionTables conversionTables);
    
    // TSD conversion
    public ushort TSDToValue(float tsd);
    public float ValueToTSD(ushort value);
    public float GetMinTSD();
    public float GetMaxTSD();
    public ushort GetUnknownTSDValue();
    public ushort GetUpdateMarker();
    
    // Weight conversion
    public ushort WeightToValue(float weight);
    public float ValueToWeight(ushort value);
    public float GetMinWeight();
    public float GetMaxWeight();
    public ushort GetUnknownWeightValue();
}

Key Implementation Details:

TSD được lưu trong correspondence_cost_cells như ProbabilityGrid
Highest bit (bit 15) của TSD value là update marker
Weight được lưu trong separate weight_cells array
Sử dụng lookup tables từ ValueConversionTables để convert hiệu quả

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

1.2 NormalEstimation2D

File: Mapping/Internal/2D/NormalEstimation2D.cs

Purpose: Estimate surface normals từ range data để tính toán SDF distance accurately.

Methods cần implement:

public static class NormalEstimation2D
{
    // Estimate normals for sorted range data
    public static List<float> EstimateNormals(
        RangeData sortedRangeData,
        NormalEstimationOptions2D options);
    
    // Helper: Get normal angle at index
    private static float GetNormalAngle(int index, ...);
}

Key Implementation Details:

Range data phải được sort theo angle từ origin (sử dụng RangeDataSorter)
Normal được estimate từ các points lân cận (trong sample_radius)
Normal được trả về dưới dạng angle (radians) cho mỗi hit point
Sử dụng num_normal_samples để average normals

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

Proto: Proto/Mapping/NormalEstimationOptions2DProto.cs (đã có proto definition)

Phase 2: TSDF2D Grid

2.1 TSDF2D Grid Class

File: Mapping/2D/TSDF2D.cs

Inheritance: TSDF2D : Grid2D

Key Properties:

List<ushort> _weightCells - Separate weight grid
TSDValueConverter _valueConverter - TSD/Weight converter
ValueConversionTables _conversionTables - Lookup tables

Methods cần implement:

public class TSDF2D : Grid2D
{
    public TSDF2D(MapLimits limits, float truncationDistance, float maxWeight, 
                  ValueConversionTables conversionTables);
    public TSDF2D(Proto.Mapping.Grid2D proto, ValueConversionTables conversionTables);
    
    // Cell accessors
    public void SetCell(Array2i cellIndex, float tsd, float weight);
    public float GetTSD(Array2i cellIndex);
    public float GetWeight(Array2i cellIndex);
    public (float tsd, float weight) GetTSDAndWeight(Array2i cellIndex);
    public bool CellIsUpdated(Array2i cellIndex);
    
    // Grid2D overrides
    public override GridType GetGridType() => GridType.TSDF;
    public override void GrowLimits(Vector2 point);
    public override Proto.Mapping.Grid2D ToProto();
    public override Grid2D ComputeCroppedGrid();
    public override bool DrawToSubmapTexture(...);
}

Key Implementation Details:

Constructor: Initialize với minCorrespondenceCost = -truncationDistance, maxCorrespondenceCost = truncationDistance
SetCell:
- Check update marker trước khi update
- Set update marker (bit 15) vào TSD value
- Store TSD trong _correspondenceCostCells
- Store weight trong _weightCells
GetTSD: Remove update marker và convert từ value về TSD
GetWeight: Convert từ weight value về float weight
GrowLimits: Override để grow cả _correspondenceCostCells và _weightCells
FinishUpdate: Remove update markers từ TSD cells (đã có trong Grid2D base)

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

2.2 TSDF2D Proto Support

File: Proto/Mapping/Grid2DProto.cs (update existing)

Changes needed:

Add TSDF2D? Tsdf2D { get; set; } property (nếu chưa có)
Update ToProto() và constructor trong TSDF2D để serialize/deserialize TSDF2D data

Proto structure:

message TSDF2D {
  float truncation_distance = 1;
  float max_weight = 2;
  repeated int32 weight_cells = 3;
}

C++ Reference: refs/cartographer/cartographer/mapping/proto/tsdf_2d.proto

Phase 3: Range Data Inserter

3.1 TSDFRangeDataInserterOptions2D Proto

File: Proto/Mapping/TSDFRangeDataInserterOptions2DProto.cs (new file)

Structure:

public struct TSDFRangeDataInserterOptions2D
{
    public double TruncationDistance { get; set; }
    public double MaximumWeight { get; set; }
    public bool UpdateFreeSpace { get; set; }
    public NormalEstimationOptions2D NormalEstimationOptions { get; set; }
    public bool ProjectSdfDistanceToScanNormal { get; set; }
    public int UpdateWeightRangeExponent { get; set; }
    public double UpdateWeightAngleScanNormalToRayKernelBandwidth { get; set; }
    public double UpdateWeightDistanceCellToHitKernelBandwidth { get; set; }
}

C++ Reference: refs/cartographer/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto

3.2 TSDFRangeDataInserter2D

File: Mapping/2D/TSDFRangeDataInserter2D.cs

Implements: IRangeDataInserter

Key Methods:

public class TSDFRangeDataInserter2D : IRangeDataInserter
{
    private readonly TSDFRangeDataInserterOptions2D _options;
    
    public TSDFRangeDataInserter2D(TSDFRangeDataInserterOptions2D options);
    public void Insert(RangeData rangeData, IGrid grid);
    
    private void InsertHit(Vector2 hit, Vector2 origin, float normal, TSDF2D tsdf);
    private void UpdateCell(Array2i cell, float updateSdf, float updateWeight, TSDF2D tsdf);
    private static void GrowAsNeeded(RangeData rangeData, float truncationDistance, TSDF2D tsdf);
}

Key Implementation Details:

Insert method:
- Cast grid to TSDF2D
- Grow grid limits if needed
- Sort range data by angle from origin (using RangeDataSorter)
- Estimate normals if needed (project_sdf_distance_to_scan_normal or angle-based weight)
- For each hit:
  - Cast ray from origin to hit
  - If update_free_space: Update cells along ray until truncation_distance behind hit
  - Else: Update cells within truncation_distance around hit
- Call FinishUpdate() on grid
InsertHit method:
- Calculate cells along ray (hoặc around hit)
- For each cell:
  - Compute SDF distance:
    - If project_sdf_distance_to_scan_normal: Project distance to scan normal
    - Else: Use Euclidean distance from cell to hit
  - Compute update weight:
    - Base weight: 1.0 / distance^update_weight_range_exponent
    - Angle weight: Gaussian kernel based on angle between scan normal and ray
    - Distance weight: Gaussian kernel based on distance from cell to hit
  - Call UpdateCell
UpdateCell method:
- Get current TSD and weight: (currentTSD, currentWeight) = tsdf.GetTSDAndWeight(cell)
- Compute new TSD: Weighted average
  - newTSD = (currentTSD * currentWeight + updateSdf * updateWeight) / (currentWeight + updateWeight)
  - Clamp to [-truncation_distance, truncation_distance]
- Compute new weight: newWeight = min(currentWeight + updateWeight, maxWeight)
- Call tsdf.SetCell(cell, newTSD, newWeight)
GrowAsNeeded:
- Similar to ProbabilityGrid inserter
- Include truncation distance when calculating bounding box

Helper Functions:

// Gaussian kernel for weight calculation
private static float GaussianKernel(float x, float sigma)
{
    return 1.0f / (Math.Sqrt(2.0 * Math.PI) * sigma) * 
           Math.Exp(-0.5 * x * x / (sigma * sigma));
}

// Range weight factor: 1.0 / range^exponent
private static float ComputeRangeWeightFactor(float range, int exponent)

// RangeDataSorter: Sort points by angle from origin
private class RangeDataSorter : IComparer<RangefinderPoint>

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

Phase 4: Scan Matching Support

4.1 InterpolatedTSDF2D

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

Purpose: Bilinear interpolation của TSDF values cho Ceres autodiff.

Methods:

public class InterpolatedTSDF2D
{
    private readonly TSDF2D _tsdf;
    
    public InterpolatedTSDF2D(TSDF2D tsdf);
    
    // Template method for Ceres autodiff
    public T GetCorrespondenceCost<T>(T x, T y) where T : struct
    {
        // Bilinear interpolation of TSD values
        // Returns MaxCorrespondenceCost if any interpolation point is unknown (weight == 0)
    }
    
    public T GetWeight<T>(T x, T y) where T : struct
    {
        // Bilinear interpolation of weight values
    }
    
    private Vector2 CenterOfLowerPixel(double x, double y);
    private T InterpolateBilinear<T>(T x, T y, float x1, float y1, float x2, float y2, 
                                      float q11, float q12, float q21, float q22);
}

Key Implementation Details:

Get 4 neighboring cells: (x1,y1), (x1+1,y1), (x1,y1+1), (x1+1,y1+1)
Check weights: If any weight == 0, return MaxCorrespondenceCost
Interpolate TSD values using bilinear interpolation
Works with Ceres Jet types (automatic differentiation)

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

4.2 TSDFMatchCostFunction2D

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

Purpose: Ceres cost function cho TSDF-based scan matching.

Methods:

public static class TSDFMatchCostFunction2D
{
    public static CostFunction CreateAutoDiffCostFunction(
        double scalingFactor,
        PointCloud pointCloud,
        TSDF2D grid)
    {
        // Create InterpolatedTSDF2D
        // Return AutoDiffCostFunction with TSDFMatchCostFunctor2D
    }
}

private struct TSDFMatchCostFunctor2D
{
    private readonly double _scalingFactor;
    private readonly PointCloud _pointCloud;
    private readonly InterpolatedTSDF2D _interpolatedTSDF;
    
    public void Evaluate(double[] parameters, double[] residuals, double[][] jacobians)
    {
        // parameters: [x, y, cos_theta, sin_theta]
        // Transform each point in pointCloud by pose
        // For each transformed point:
        //   residual = scaling_factor * interpolatedTSDF.GetCorrespondenceCost(x, y)
        // residuals length = pointCloud.Count
    }
}

Key Implementation Details:

Transform point cloud by pose: R * point + translation
Get interpolated correspondence cost for each transformed point
Residual = scaling_factor * correspondence_cost
Ceres sẽ minimize tổng squared residuals

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

4.3 Update Scan Matchers

4.3.1 RealTimeCorrelativeScanMatcher2D File: Mapping/Internal/2D/ScanMatching/RealTimeCorrelativeScanMatcher2D.cs

Changes needed:

Line 125: Implement TSDF scoring

case GridType.TSDF:
    if (grid is TSDF2D tsdfGrid)
    {
        foreach (var point in discreteScan)
        {
            var tsd = tsdfGrid.GetTSD(proposedXYIndex);
            // Score based on distance from zero-crossing (surface)
            // Cells with TSD near 0 are likely to be on surface
            candidateScore += -Math.Abs(tsd); // Closer to 0 = better
        }
        candidateScore /= discreteScan.Count;
    }
    break;

Note: TSDF scoring có thể đơn giản hơn ProbabilityGrid vì TSD gần 0 = surface.

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

Changes needed:

Line 112-114: Replace TODO với TSDF cost function

case GridType.TSDF:
    if (grid is TSDF2D tsdfGrid)
    {
        var tsdfMatchCost = TSDFMatchCostFunction2D.CreateAutoDiffCostFunction(
            _options.OccupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
            pointCloud,
            tsdfGrid
        );
        problem.AddResidualBlock(tsdfMatchCost, null, [poseParams]);
    }
    break;

Phase 5: Integration

5.1 Update RangeDataInserterOptionsProto

File: Proto/Mapping/RangeDataInserterOptionsProto.cs

Changes needed:

Uncomment and add TSDF options:

[JsonPropertyName("tsdf_range_data_inserter_options_2d")]
[JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
public TSDFRangeDataInserterOptions2D? TsdfRangeDataInserterOptions2D { get; set; }

Update constructor to accept TSDF options

5.2 Update ActiveSubmaps2D

File: Mapping/2D/ActiveSubmaps2D.cs

Changes needed:

Line 131: Replace NotImplementedException với TSDF2D creation

GridOptions2D.GridType.Tsdf => new TSDF2D(
    mapLimits,
    _options.GridOptions2D.TsdfOptions?.TruncationDistance ?? 0.3f, // Default
    _options.GridOptions2D.TsdfOptions?.MaxWeight ?? 10.0f, // Default
    _conversionTables
),

Note: Cần add TsdfOptions vào GridOptions2D proto nếu chưa có.

5.3 Update RangeDataInserterFactory

File: (tìm file tạo RangeDataInserter, có thể trong Mapping/2D/ hoặc Mapping/Internal/2D/)

Changes needed:

Add TSDF inserter creation:

case RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D:
    if (options.TsdfRangeDataInserterOptions2D.HasValue)
    {
        return new TSDFRangeDataInserter2D(options.TsdfRangeDataInserterOptions2D.Value);
    }
    throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF inserter");

🔗 Dependencies & Order

Implementation Order:

✅ Phase 1: Core Utilities
- TSDValueConverter
- NormalEstimation2D
- NormalEstimationOptions2DProto (nếu chưa có)
✅ Phase 2: TSDF2D Grid
- TSDF2D class
- Update Grid2DProto
✅ Phase 3: Range Data Inserter
- TSDFRangeDataInserterOptions2DProto
- TSDFRangeDataInserter2D
✅ Phase 4: Scan Matching
- InterpolatedTSDF2D
- TSDFMatchCostFunction2D
- Update RealTimeCorrelativeScanMatcher2D
- Update CeresScanMatcher2D
✅ Phase 5: Integration
- Update RangeDataInserterOptionsProto
- Update ActiveSubmaps2D
- Update RangeDataInserterFactory

🧪 Testing Plan

Unit Tests: ✅ COMPLETED

✅ TSDValueConverterTests
- ✅ Test TSD/Weight conversion (toValue, fromValue)
- ✅ Test bounds (min/max TSD/Weight)
- ✅ Test unknown values
- ✅ Test update marker
✅ TSDF2DTests
- ✅ Test SetCell/GetTSD/GetWeight
- ✅ Test GetTSDAndWeight
- ✅ Test CellIsUpdated
- ✅ Test GrowLimits
- ✅ Test ComputeCroppedGrid
- ✅ Test ToProto/FromProto
- ✅ Test out-of-bounds handling
- ✅ Test multiple updates with weighted average
✅ TSDFRangeDataInserter2DTests
- ✅ Test Insert with simple range data
- ✅ Test UpdateFreeSpace option
- ✅ Test ProjectSdfDistanceToScanNormal option
- ✅ Test weight calculation (range, angle, distance)
- ✅ Test empty range data handling
- ✅ Test wrong grid type error handling
✅ NormalEstimation2DTests
- ✅ Test normal estimation với known geometry (horizontal/vertical lines, rectangles)
- ✅ Test với different sample radii
- ✅ Test empty point clouds
- ✅ Test single point handling
✅ InterpolatedTSDF2DTests
- ✅ Test bilinear interpolation
- ✅ Test unknown cell handling
- ✅ Test GetWeight interpolation
- ✅ Test partially unknown cells
⏳ TSDFMatchCostFunction2DTests
- ⏳ Test cost function evaluation (có thể thêm sau nếu cần)
- ⏳ Test with Ceres solver (integration test, có thể thêm sau nếu cần)

Integration Tests: ⏳ Optional (có thể thêm sau nếu cần)

⏳ Full TSDF Pipeline
- Create TSDF2D submap
- Insert range data
- Perform scan matching (both correlative and Ceres)
- Verify pose estimation accuracy
⏳ TSDF vs ProbabilityGrid Comparison
- Compare mapping quality
- Compare scan matching accuracy

Note: Core functionality đã được test qua unit tests. Integration tests có thể được thêm sau khi cần validate với real-world data.

📚 Reference Files

C++ Implementation:

refs/cartographer/cartographer/mapping/internal/2d/tsd_value_converter.h/cc
refs/cartographer/cartographer/mapping/internal/2d/tsdf_2d.h/cc
refs/cartographer/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h/cc
refs/cartographer/cartographer/mapping/internal/2d/normal_estimation_2d.h/cc
refs/cartographer/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h
refs/cartographer/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h/cc

C++ Tests:

refs/cartographer/cartographer/mapping/internal/2d/tsdf_2d_test.cc
refs/cartographer/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc
refs/cartographer/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc
refs/cartographer/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc

Proto Files:

refs/cartographer/cartographer/mapping/proto/tsdf_2d.proto
refs/cartographer/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto
refs/cartographer/cartographer/mapping/proto/normal_estimation_options_2d.proto

⚠️ Notes & Considerations

Performance:
- TSDF computation phức tạp hơn ProbabilityGrid (normal estimation, weighted integration)
- Consider caching normal estimates nếu cần
- Weight calculation có thể tốn kém (Gaussian kernels)
Memory:
- TSDF2D cần thêm weight_cells array (same size as correspondence_cost_cells)
- Memory usage ~2x so với ProbabilityGrid
Accuracy:
- TSDF thường cho accuracy cao hơn, đặc biệt với subpixel features
- Normal estimation quality ảnh hưởng lớn đến SDF accuracy
Configuration:
- truncation_distance: Thường 0.1-0.5m
- maximum_weight: Thường 10-50
- update_weight_range_exponent: Thường 0-2
- Kernel bandwidths: Cần tune cho từng sensor
Compatibility:
- Ensure TSDF grids có thể serialize/deserialize correctly
- Backward compatibility với ProbabilityGrid configs

✅ Completion Checklist

Phase 1: Core Utilities ✅ COMPLETED
- TSDValueConverter
- NormalEstimation2D
- NormalEstimationOptions2DProto
Phase 2: TSDF2D Grid ✅ COMPLETED
- TSDF2D class
- Update Grid2DProto (TSDF2DProto)
Phase 3: Range Data Inserter ✅ COMPLETED
- TSDFRangeDataInserterOptions2DProto
- TSDFRangeDataInserter2D
- RangeDataSorter helper
- RayToPixelMask utility
Phase 4: Scan Matching ✅ COMPLETED
- InterpolatedTSDF2D
- TSDFMatchCostFunction2D
- Update RealTimeCorrelativeScanMatcher2D
- Update CeresScanMatcher2D
Phase 5: Integration ✅ COMPLETED
- Update RangeDataInserterOptionsProto
- Update ActiveSubmaps2D (CreateGrid và CreateRangeDataInserter)
- Update GridOptions2DProto (TSDFOptions2D)
Testing ✅ COMPLETED
- Unit tests cho tất cả components
  - TSDValueConverterTests
  - TSDF2DTests
  - NormalEstimation2DTests
  - TSDFRangeDataInserter2DTests
  - InterpolatedTSDF2DTests
- Integration tests (có thể thêm sau nếu cần)
- Performance benchmarks (có thể thêm sau nếu cần)

Last Updated: 2024-12-19 Status: ✅ IMPLEMENTATION COMPLETED - All phases implemented with comprehensive unit tests

22 KiB Raw Blame History

TSDF2D Support - Implementation Plan

📋 Tổng Quan

🎯 Components Cần Implement

1. TSDValueConverter (Core Utility)

2. TSDF2D Grid (Grid Implementation)

3. TSDFRangeDataInserter2D (Range Data Inserter)

4. NormalEstimation2D (Normal Estimation Utility)

5. InterpolatedTSDF2D (Interpolation for Scan Matching)

6. TSDFMatchCostFunction2D (Ceres Cost Function)

7. Proto Definitions (Configuration & Serialization)

📝 Chi Tiết Implementation

Phase 1: Core Utilities

1.1 TSDValueConverter

1.2 NormalEstimation2D

Phase 2: TSDF2D Grid

2.1 TSDF2D Grid Class

2.2 TSDF2D Proto Support

Phase 3: Range Data Inserter

3.1 TSDFRangeDataInserterOptions2D Proto

3.2 TSDFRangeDataInserter2D

Phase 4: Scan Matching Support

4.1 InterpolatedTSDF2D

4.2 TSDFMatchCostFunction2D

4.3 Update Scan Matchers

Phase 5: Integration

5.1 Update RangeDataInserterOptionsProto

5.2 Update ActiveSubmaps2D

5.3 Update RangeDataInserterFactory

🔗 Dependencies & Order

Implementation Order:

🧪 Testing Plan

Unit Tests: ✅ COMPLETED

Integration Tests: ⏳ Optional (có thể thêm sau nếu cần)

📚 Reference Files

C++ Implementation:

C++ Tests:

Proto Files:

⚠️ Notes & Considerations

✅ Completion Checklist

22 KiB

Raw Blame History