Cartographer中的2D扫描匹配算法

Cartographer中的2D扫描匹配算法

基础知识

Ceres Solver 入门教程
Ceres的Options详解

原理公式

待续。。。

Cartographer中代码实现详细注释

位置：cartographer_ws/src/cartographer/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc

#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" // 引入头文件

#include <utility> // 引入工具库
#include <vector> // 引入向量库

#include "Eigen/Core" // 引入Eigen库
#include "cartographer/common/internal/ceres_solver_options.h" // 引入Ceres求解器选项头文件
#include "cartographer/common/lua_parameter_dictionary.h" // 引入Lua参数字典头文件
#include "cartographer/mapping/2d/grid_2d.h" // 引入2D网格头文件
#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" // 引入占用空间成本函数头文件
#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h" // 引入旋转增量成本函数头文件
#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h" // 引入平移增量成本函数头文件
#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" // 引入TSDF匹配成本函数头文件
#include "cartographer/transform/transform.h" // 引入变换头文件
#include "ceres/ceres.h" // 引入Ceres库
#include "glog/logging.h" // 引入日志库

namespace cartographer { // 定义命名空间
namespace mapping { // 定义子命名空间
namespace scan_matching { // 定义子子命名空间

// 创建CeresScanMatcherOptions2D对象，从参数字典中获取权重和求解器选项
// 创建一个CeresScanMatcherOptions2D对象，用于配置扫描匹配器的相关参数
proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
    common::LuaParameterDictionary* const parameter_dictionary) {
  // 创建一个CeresScanMatcherOptions2D对象
  proto::CeresScanMatcherOptions2D options;

  // 从参数字典中获取占用空间权重，并设置到options对象中
  options.set_occupied_space_weight(
      parameter_dictionary->GetDouble("occupied_space_weight"));

  // 从参数字典中获取平移权重，并设置到options对象中
  options.set_translation_weight(
      parameter_dictionary->GetDouble("translation_weight"));

  // 从参数字典中获取旋转权重，并设置到options对象中
  options.set_rotation_weight(
      parameter_dictionary->GetDouble("rotation_weight"));

  // 从参数字典中获取Ceres求解器选项，并设置到options对象的ceres_solver_options属性中
  *options.mutable_ceres_solver_options() =
      common::CreateCeresSolverOptionsProto(
          parameter_dictionary->GetDictionary("ceres_solver_options").get());

  // 返回配置好的CeresScanMatcherOptions2D对象
  return options;
}


// CeresScanMatcher2D类的构造函数，初始化选项和求解器选项
// CeresScanMatcher2D 类的构造函数，用于初始化对象
CeresScanMatcher2D::CeresScanMatcher2D(
    const proto::CeresScanMatcherOptions2D& options) // 传入一个 CeresScanMatcherOptions2D 类型的参数 options
    : options_(options), // 将传入的 options 赋值给成员变量 options_
      ceres_solver_options_( // 创建一个 CeresSolverOptions 类型的对象 ceres_solver_options_
          common::CreateCeresSolverOptions(options.ceres_solver_options())) { // 调用 CreateCeresSolverOptions 函数，传入 options 的 ceres_solver_options() 成员，并将返回值赋给 ceres_solver_options_
  ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; // 设置 ceres_solver_options_ 的 linear_solver_type 成员为 DENSE_QR
}
    


// CeresScanMatcher2D类中的Match方法，用于进行二维扫描匹配
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(),  // 初始化Ceres位姿估计数组
                                   initial_pose_estimate.translation().y(),
                                   initial_pose_estimate.rotation().angle()};
  ceres::Problem problem;  // 创建Ceres问题对象
  CHECK_GT(options_.occupied_space_weight(), 0.);  // 检查占用空间权重是否大于0
// 根据网格类型选择不同的残差块
switch (grid.GetGridType()) {
    // 当网格类型为概率网格时
    case GridType::PROBABILITY_GRID:
        // 添加占用空间成本函数残差块
        problem.AddResidualBlock(
            // 创建占用空间成本函数
            CreateOccupiedSpaceCostFunction2D(
                // 计算权重，权重等于占用空间权重除以点云大小的平方根
                options_.occupied_space_weight() /
                    std::sqrt(static_cast<double>(point_cloud.size())),
                // 传入点云和网格
                point_cloud, grid),
            nullptr /* loss function */, ceres_pose_estimate); // 无损失函数，使用默认值
        break;
    // 当网格类型为TSDF时
    case GridType::TSDF:
        // 添加TSDF匹配成本函数残差块
        problem.AddResidualBlock(
            // 创建TSDF匹配成本函数
            CreateTSDFMatchCostFunction2D(
                // 计算权重，权重等于占用空间权重除以点云大小的平方根
                options_.occupied_space_weight() /
                    std::sqrt(static_cast<double>(point_cloud.size())),
                // 传入点云和网格
                point_cloud, static_cast<const TSDF2D&>(grid)),
            nullptr /* loss function */, ceres_pose_estimate); // 无损失函数，使用默认值
        break;
}
  CHECK_GT(options_.translation_weight(), 0.);  // 检查平移权重是否大于0
  problem.AddResidualBlock(  // 添加平移误差残差块
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(  // 创建自动微分平移误差成本函数
          options_.translation_weight(), target_translation),
      nullptr /* loss function */, ceres_pose_estimate);
  CHECK_GT(options_.rotation_weight(), 0.);  // 检查旋转权重是否大于0
  problem.AddResidualBlock(  // 添加旋转误差残差块
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(  // 创建自动微分旋转误差成本函数
          options_.rotation_weight(), ceres_pose_estimate[2]),
      nullptr /* loss function */, ceres_pose_estimate);

  ceres::Solve(ceres_solver_options_, &problem, summary);  // 使用Ceres求解器求解问题

  *pose_estimate = transform::Rigid2d(  // 更新位姿估计结果
      {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

}  // namespace scan_matching
}  // namespace mapping
}  // namespace cartographer

位置：cartographer_ws/src/cartographer/cartographer/common/internal/ceres_solver_options.cc

#include "cartographer/common/internal/ceres_solver_options.h"  // 引入Ceres求解器选项头文件

namespace cartographer {  // 定义cartographer命名空间
namespace common {  // 定义common命名空间

// 从Lua参数字典中创建CeresSolverOptionsProto对象
proto::CeresSolverOptions CreateCeresSolverOptionsProto(
    common::LuaParameterDictionary* parameter_dictionary) {
  proto::CeresSolverOptions proto;  // 创建一个CeresSolverOptions对象
  proto.set_use_nonmonotonic_steps(
      parameter_dictionary->GetBool("use_nonmonotonic_steps"));  // 设置是否使用非单调步骤
  proto.set_max_num_iterations(
      parameter_dictionary->GetNonNegativeInt("max_num_iterations"));  // 设置最大迭代次数
  proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads"));  // 设置线程数
  CHECK_GT(proto.max_num_iterations(), 0);  // 检查最大迭代次数是否大于0
  CHECK_GT(proto.num_threads(), 0);  // 检查线程数是否大于0
  return proto;  // 返回创建的CeresSolverOptions对象
}

// 从CeresSolverOptionsProto对象创建ceres::Solver::Options对象
ceres::Solver::Options CreateCeresSolverOptions(
    const proto::CeresSolverOptions& proto) {
    //
  ceres::Solver::Options options;  // 创建一个ceres::Solver::Options对象
  options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps();  // 设置是否使用非单调步骤
  options.max_num_iterations = proto.max_num_iterations();  // 设置最大迭代次数
  options.num_threads = proto.num_threads();  // 设置线程数
  return options;  // 返回创建的ceres::Solver::Options对象
}

}  // namespace common
}  // namespace cartographer

基于高斯-牛顿迭代法的2D-ICP—高翔老师的代码

//
// Created by xiang on 2022/3/15.
//

#include "ch6/icp_2d.h"
#include "common/math_utils.h"

#include <glog/logging.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/impl/kdtree.hpp>

namespace sad {

// 基于高斯-牛顿迭代法的2D ICP
// 高斯-牛顿迭代法是一种求解非线性方程组的数值方法，其基本思想是利用泰勒级数展开将非线性函数近似为线性函数，从而将原问题转化为求解线性方程组的问题。

// 具体来说，假设要求解的非线性方程组为：

// f(x) = 0, g(x) = 0

// 其中f(x)和g(x)都是可导函数。我们可以在点x0处对f(x)进行泰勒级数展开，得到：

// f(x) = f(x0) + f'(x0)(x - x0) + f''(x0)(x - x0)^2/2! + ...

// 将上式代入原方程组中，可以得到一个关于x的新方程组：

// f(x0) + f'(x0)(x - x0) + f''(x0)(x - x0)^2/2! + ... = 0
// g(x0) + g'(x0)(x - x0) + g''(x0)(x - x0)^2/2! + ... = 0

// 这个新方程组是一个线性方程组，可以使用线性代数的方法求解。求解出x之后，还需要判断是否满足精度要求，如果不满足则继续迭代，直到满足精度要求为止。

// 高斯-牛顿迭代法的优点是可以快速收敛，而且对于凸函数和二次函数等特殊形式的非线性函数具有较好的效果。但是，对于非凸函数或者存在多个局部极小值的情况下，可能会出现不收敛的情况。
bool Icp2d::AlignGaussNewton(SE2& init_pose) {
    int iterations = 10; // 迭代次数
    double cost = 0, lastCost = 0; // 代价函数值和上一次的代价函数值
    SE2 current_pose = init_pose; // 当前位姿
    const float max_dis2 = 0.01; // 最近邻时的最远距离（平方）
    const int min_effect_pts = 20; // 最小有效点数

    // 遍历源点云数据，找到最近邻点，并更新H和b。
    // 在每次迭代中，计算变换矩阵H和向量b。
    for (int iter = 0; iter < iterations; ++iter) {
        Mat3d H = Mat3d::Zero(); // 计算变换矩阵H
        Vec3d b = Vec3d::Zero(); // 计算变换矩阵b
        cost = 0;

        int effective_num = 0; // 有效点数

        // 遍历source
        for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
            float r = source_scan_->ranges[i];
            // 设置最大最小激光雷达数据 或判断
            if (r < source_scan_->range_min || r > source_scan_->range_max) {
                continue;
            }

            float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
            float theta = current_pose.so2().log(); // 当前位姿的旋转角度
            Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle)); // 当前位姿下的激光雷达扫描点
            Point2d pt;
            pt.x = pw.x();
            pt.y = pw.y();

            // 最近邻
            std::vector<int> nn_idx;
            std::vector<float> dis;
            kdtree_.nearestKSearch(pt, 1, nn_idx, dis); // 在目标点云中找到距离最近的一个点
            // 判断nn_idx的大小是否大于0，以及dis[0]是否小于max_dis2
            if (nn_idx.size() > 0 && dis[0] < max_dis2) {
                // effective_num自增1
                effective_num++;
                // 定义一个3x2的矩阵J
                Mat32d J;
                // 给矩阵J赋值
                J << 1, 0, 0, 1, -r * std::sin(angle + theta), r * std::cos(angle + theta);
                // 将矩阵J与其转置相乘，结果累加到H上
                H += J * J.transpose();
                // 定义一个2维向量e，其值为pt的x坐标减去目标云中第一个点的x坐标，y坐标减去目标云中第一个点的y坐标
                Vec2d e(pt.x - target_cloud_->points[nn_idx[0]].x, pt.y - target_cloud_->points[nn_idx[0]].y);
                // 将-J与向量e相乘，结果累加到b上
                b += -J * e;
                // 计算向量e的点积，结果累加到cost上
                cost += e.dot(e);
            }

        }

        if (effective_num < min_effect_pts) {
            return false; // 如果有效点数小于最小有效点数，返回false
        }

        // solve for dx
        Vec3d dx = H.ldlt().solve(b); // 求解线性方程组Hx=b得到dx
        if (isnan(dx[0])) {
            break; // 如果dx的第一个元素是NaN，跳出循环
        }

        cost /= effective_num; // 计算平均代价函数值
        if (iter > 0 && cost >= lastCost) {
            break; // 如果本次迭代的代价函数值大于等于上一次的代价函数值，跳出循环
        }

        LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num; // 输出迭代信息

        current_pose.translation() += dx.head<2>(); // 更新当前位姿的平移部分
        current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]); // 更新当前位姿的旋转部分
        lastCost = cost; // 更新上一次的代价函数值
    }

    init_pose = current_pose; // 更新初始位姿
    LOG(INFO) << "estimated pose: " << current_pose.translation().transpose() // 输出估计的位姿
              << ", theta: " << current_pose.so2().log();

    return true; // 返回true表示算法执行成功
}



bool Icp2d::AlignGaussNewtonPoint2Plane(SE2& init_pose) {
    // 设置迭代次数为10
    int iterations = 10;
    // 初始化cost和lastCost为0
    double cost = 0, lastCost = 0;
    // 将init_pose赋值给current_pose
    SE2 current_pose = init_pose;
    // 设置最近邻时的最远距离为0.3
    const float max_dis = 0.3;
    // 设置最小有效点数为20
    const int min_effect_pts = 20;

    // 进行iterations次迭代
    for (int iter = 0; iter < iterations; ++iter) {
        // 初始化H和b为0矩阵和向量
        Mat3d H = Mat3d::Zero();
        Vec3d b = Vec3d::Zero();
        // 重置cost为0
        cost = 0;

        // 初始化有效点数为0
        int effective_num = 0;

        // 遍历source
        for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
            // 获取当前距离r
            float r = source_scan_->ranges[i];
            // 如果r小于最小距离或大于最大距离，则跳过此次循环
            if (r < source_scan_->range_min || r > source_scan_->range_max) {
                continue;
            }

            // 计算角度angle
            float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
            // 计算theta
            float theta = current_pose.so2().log();
            // 计算pw
            Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
            // 创建Point2d对象pt
            Point2d pt;
            pt.x = pw.x();
            pt.y = pw.y();

            // 查找5个最近邻
            std::vector<int> nn_idx;
            std::vector<float> dis;
            kdtree_.nearestKSearch(pt, 5, nn_idx, dis);

            // 创建有效点向量effective_pts
            std::vector<Vec2d> effective_pts;
            // 遍历最近邻索引
            for (int j = 0; j < nn_idx.size(); ++j) {
                // 如果距离小于最大距离，则将对应的点添加到有效点向量中
                if (dis[j] < max_dis) {
                    effective_pts.emplace_back(
                        Vec2d(target_cloud_->points[nn_idx[j]].x, target_cloud_->points[nn_idx[j]].y));
                }
            }

            // 如果有效点数量小于3，则跳过此次循环
            if (effective_pts.size() < 3) {
                continue;
            }

            // 拟合直线，组装J、H和误差
            Vec3d line_coeffs;
            if (math::FitLine2D(effective_pts, line_coeffs)) {
                // 增加有效点数量
                effective_num++;
                // 计算J矩阵
                Vec3d J;
                J << line_coeffs[0], line_coeffs[1],
                    -line_coeffs[0] * r * std::sin(angle + theta) + line_coeffs[1] * r * std::cos(angle + theta);
                // 更新H矩阵
                H += J * J.transpose();

                // 计算误差e
                double e = line_coeffs[0] * pw[0] + line_coeffs[1] * pw[1] + line_coeffs[2];
                // 更新b向量
                b += -J * e;

                // 更新cost
                cost += e * e;
            }
        }

        // 如果有效点数量小于最小有效点数，则返回false
        if (effective_num < min_effect_pts) {
            return false;
        }

        // 求解dx
        Vec3d dx = H.ldlt().solve(b);
        // 如果dx中有NaN值，则跳出循环
        if (isnan(dx[0])) {
            break;
        }

        // 更新cost
        cost /= effective_num;
        // 如果本次迭代的cost大于等于上次迭代的cost，则跳出循环
        if (iter > 0 && cost >= lastCost) {
            break;
        }

        // 输出迭代信息
        LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;

        // 更新current_pose的平移和旋转部分
        current_pose.translation() += dx.head<2>();
        current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
        // 更新lastCost
        lastCost = cost;
    }

    // 更新init_pose
    init_pose = current_pose;
    // 输出估计的位姿信息
    LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
              << ", theta: " << current_pose.so2().log();

    // 返回true
    return true;
}


// 定义一个名为Icp2d的类，其中包含一个名为BuildTargetKdTree的成员函数
// 求最邻近值
// k-d树是一种用于存储k维空间中数据的数据结构，它可以高效地查询与给定点最邻近的点。k-d树是二叉树的一种变体，每个节点都包含一个k维向量和一个分割轴，根据分割轴将k维空间划分为两个子空间。

// k-d树的构建过程如下：

// 1. 选择一个维度作为分割轴，将数据集按照该维度的值进行排序。
// 2. 选取排序后的第一个点作为根节点，将其加入k-d树中。
// 3. 递归地对数据集中的其余点进行同样的操作，将它们插入到k-d树中。
// 4. 在插入过程中，如果当前节点的子空间中已经存在一个点与待插入点的分割轴值相同，则选择下一个维度作为分割轴；否则，选择当前节点的子空间中所有点中分割轴值最小的那个点作为分割点，并将当前节点分为两个子节点。
// 5. 重复步骤3和4，直到所有的点都被插入到k-d树中。

// k-d树的查询过程如下：

// 1. 选择一个起始点，从根节点开始遍历k-d树。
// 2. 如果当前节点的子空间中包含目标点，则返回该点；否则，继续向下遍历。
// 3. 如果当前节点的子空间中不存在目标点，则选择下一个维度作为分割轴，并沿着该轴向左或向右移动到下一个节点。
// 4. 重复步骤2和3，直到找到目标点或者遍历完整棵树。
void Icp2d::BuildTargetKdTree() {
    // 如果目标扫描为空，则输出错误信息并返回
    if (target_scan_ == nullptr) {
        LOG(ERROR) << "target is not set";
        return;
    }

    // 重置目标云对象，创建一个新的Cloud2d对象
    target_cloud_.reset(new Cloud2d);

    // 遍历目标扫描的范围
    for (size_t i = 0; i < target_scan_->ranges.size(); ++i) {
        // 如果当前范围值小于最小范围或大于最大范围，则跳过此次循环
        if (target_scan_->ranges[i] < target_scan_->range_min || target_scan_->ranges[i] > target_scan_->range_max) {
            continue;
        }

        // 计算实际角度
        double real_angle = target_scan_->angle_min + i * target_scan_->angle_increment;

        // 创建一个二维点对象p
        Point2d p;
        // 根据实际角度和距离计算点的x和y坐标
        p.x = target_scan_->ranges[i] * std::cos(real_angle);
        p.y = target_scan_->ranges[i] * std::sin(real_angle);
        // 将点p添加到目标云对象的点集合中
        target_cloud_->points.push_back(p);
    }

    // 设置目标云对象的宽度为点集合的大小
    target_cloud_->width = target_cloud_->points.size();
    // 设置目标云对象的密集度为false
    target_cloud_->is_dense = false;
    // 将目标云对象设置为kdtree的输入云
    kdtree_.setInputCloud(target_cloud_);
}

}  // namespace sad