Cartographer sử dụng nhiều phương pháp scan-matching khác nhau. Bài viết này phân tích ba phương pháp chính: Real-Time Correlative Scan Matching (RT CSM), Fast Correlative Scan Matching (dùng branch-and-bound), và Ceres-based scan matching. Những phương pháp này đóng vai trò then chốt trong việc ước lượng vị trí robot dựa trên dữ liệu LiDAR và bản đồ lưới (grid map).
1. Real-Time Correlative Scan Matching (RT CSM)
Phương pháp này thực hiện tìm kiếm vét cạn trong không gian 3 chiều (x, y, theta) trên một submap cục bộ. Mã nguồn nằm tại: cartographer/mapping/internal/2d/scanmatching/real_time_correlative_scan_matcher_2d.cc.
Hàm chính là RealTimeCorrelativeScanMatcher2D::Match, nhận đầu vào là vị trí ước lượng ban đầu, đám mây điểm, và grid map. Kết quả trả về là vị trí có độ tin cậy cao nhất.
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate) const {
// Lấy góc xoay ban đầu
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
// Xoay đám mây điểm về hệ tọa độ ban đầu
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
// Định nghĩa không gian tìm kiếm: cửa sổ tịnh tiến và góc
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, grid.limits().resolution());
// Tạo ra các đám mây điểm đã được xoay theo từng góc rời rạc
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
// Chuyển đám mây điểm về hệ tọa độ thế giới (dựa trên vị trí ước lượng)
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
// Tạo tất cả các ứng viên trong không gian (x, y)
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
// Tính điểm cho từng ứng viên dựa trên grid
ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
// Lấy ứng viên có điểm số cao nhất
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
// Chuyển đổi sang tọa độ toàn cục
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return best_candidate.score;
}
Nguyên lý hoạt động: Giống như một vòng lặp ba lớp: lặp theo góc, theo x, và theo y. Với mỗi tổ hợp, đám mây điểm được chiếu lên grid và tính tổng xác suất. Điểm số cao nhất chính là vị trí ước lượng.
for (theta)
for (x)
for (y)
score = sum(grid_value_at(point_cloud_transformed)) / num_points
Điểm tối ưu so với KartoSLAM:
- Đánh đổi không gian lấy tốc độ: Các vòng lặp x, y không thực hiện tính toán trực tiếp mà chỉ lưu chỉ mục. Vòng lặp góc được thực hiện trước để tạo ra N đám mây điểm đã xoay. Điều này cho phép xử lý hàng loạt bằng vector và thư viện chuẩn.
- Giảm thiểu phép tính sin/cos: Vì góc được xoay trước, số lần tính sin/cos chỉ bằng số góc rời rạc. Các phép tính sau đó chỉ là cộng trừ.
Lưu ý: Kết quả cuối cùng được nhân với một trọng số dựa trên khoảng cách đến vị trí ban đầu (dạng phân phối Gaussian), nhằm ưu tiên các vị trí gần với ước lượng ban đầu.
2. Fast Correlative Scan Matching (Branch-and-Bound)
Phương pháp này được sử dụng trong vòng lặp phát hiện (loop closure), nơi không gian tìm kiếm rất lớn. Nó dùng thuật toán Branch and Bound để tăng tốc độ tìm kiếm. Mã nguồn: cartographer/mapping/internal/2d/scanmatching/fast_correlative_scan_matcher_2d.cc.
2.1. PrecomputationGridStack2D: Xây dựng bản đồ đa phân giải
Trước khi match, bản đồ gốc được xử lý để tạo ra một ngăn xếp bản đồ (grid stack) với nhiều độ phân giải khác nhau. Độ phân giải giảm dần theo cấp số nhân (2, 4, 8,...). Mỗi ô ở độ phân giải thấp hơn có giá trị là giá trị lớn nhất của các ô tương ứng ở độ phân giải cao hơn. Điều này đảm bảo tính chất upper bound: điểm số của một node ở tầng trên luôn lớn hơn hoặc bằng điểm số của tất cả các node con của nó.
2.2. Hàm MatchWithSearchParameters
Quy trình tương tự RT CSM, nhưng thay vì vét cạn, nó gọi hàm BranchAndBound.
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// ... (xoay và chuyển đổi đám mây điểm, giống RT CSM) ...
// Lấy danh sách ứng viên ở độ phân giải thấp nhất
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
// Áp dụng Branch and Bound
const Candidate2D best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
// ... (trả về kết quả) ...
}
2.3. Hàm BranchAndBound
Đây là cốt lõi của thuật toán. Nó tìm kiếm đệ quy trên cây các ứng viên.
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
// Nếu là tầng cuối (độ phân giải gốc), trả về ứng viên tốt nhất
if (candidate_depth == 0) {
return *candidates.begin();
}
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;
for (const Candidate2D& candidate : candidates) {
// Cắt tỉa: nếu điểm của ứng viên nhỏ hơn min_score, bỏ qua tất cả các ứng viên tiếp theo (vì đã sắp xếp giảm dần)
if (candidate.score <= min_score) {
break;
}
std::vector<Candidate2D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
// Sinh ra 4 ứng viên con
for (int x_offset : {0, half_width}) {
for (int y_offset : {0, half_width}) {
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
// Tính điểm cho các ứng viên ở tầng cao hơn (độ phân giải cao hơn)
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
// Gọi đệ quy và cập nhật ứng viên tốt nhất
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}
Nguyên lý hoạt động:
- Upper Bound: Mỗi node ở tầng trên có điểm số là giá trị lớn nhất có thể đạt được ở các node con của nó (nhờ bản đồ đa phân giải).
- Lower Bound (min_score): Là điểm số tốt nhất tìm được cho đến thời điểm hiện tại.
- Cắt tỉa: Nếu điểm của một node nhỏ hơn
min_score, thì tất cả các nhánh con của nó đều không thể có điểm cao hơn, do đó bị cắt bỏ.
3. Ceres-based Scan Matching
Đây là phương pháp cuối cùng và chính xác nhất. Thay vì tìm kiếm trên grid, nó giải bài toán tối ưu bằng phương pháp bình phương tối thiểu (least squares) với thư viện Ceres. Mã nguồn: cartographer/mapping/internal/2d/scanmatching/ceres_scan_matcher_2d.cc.
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
// Cost function 1: Mức độ phù hợp của đám mây điểm với grid
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid),
nullptr, ceres_pose_estimate);
// Cost function 2: Sai lệch tịnh tiến so với vị trí ước lượng
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr, ceres_pose_estimate);
// Cost function 3: Sai lệch góc so với góc ước lượng
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr, ceres_pose_estimate);
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}
Các thành phần của hàm mất mát (cost function):
- OccupiedSpaceCostFunction2D: Đây là thành phần chính. Nó sử dụng nội suy hai khối (bicubic interpolation) trên grid để làm cho hàm mục tiêu trở nên khả vi. Đám mây điểm được chiếu lên grid, và giá trị tại các điểm nội suy (tương ứng với
CorrespondenceCost = 1 - probability) được cộng dồn. - TranslationDeltaCostFunctor2D: Phạt sự sai lệch giữa vị trí ước lượng và vị trí ban đầu.
- RotationDeltaCostFunctor2D: Phạt sự sai lệch giữa góc ước lượng và góc ban đầu.
Kết luận: Ceres matcher cho phép ước lượng vị trí với độ chính xác dưới mức pixel (sub-pixel) nhờ nội suy. Kết quả của RT CSM hoặc Fast CSM thường được dùng làm giá trị khởi tạo cho Ceres matcher.