本文主要是介绍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.); // 检查平移权重是否大于0problem.AddResidualBlock( // 添加平移误差残差块TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( // 创建自动微分平移误差成本函数options_.translation_weight(), target_translation),nullptr /* loss function */, ceres_pose_estimate);CHECK_GT(options_.rotation_weight(), 0.); // 检查旋转权重是否大于0problem.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); // 检查最大迭代次数是否大于0CHECK_GT(proto.num_threads(), 0); // 检查线程数是否大于0return 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(); // 计算变换矩阵HVec3d b = Vec3d::Zero(); // 计算变换矩阵bcost = 0;int effective_num = 0; // 有效点数// 遍历sourcefor (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_dis2if (nn_idx.size() > 0 && dis[0] < max_dis2) {// effective_num自增1effective_num++;// 定义一个3x2的矩阵JMat32d 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 dxVec3d dx = H.ldlt().solve(b); // 求解线性方程组Hx=b得到dxif (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) {// 设置迭代次数为10int iterations = 10;// 初始化cost和lastCost为0double cost = 0, lastCost = 0;// 将init_pose赋值给current_poseSE2 current_pose = init_pose;// 设置最近邻时的最远距离为0.3const float max_dis = 0.3;// 设置最小有效点数为20const 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为0cost = 0;// 初始化有效点数为0int effective_num = 0;// 遍历sourcefor (size_t i = 0; i < source_scan_->ranges.size(); ++i) {// 获取当前距离rfloat r = source_scan_->ranges[i];// 如果r小于最小距离或大于最大距离,则跳过此次循环if (r < source_scan_->range_min || r > source_scan_->range_max) {continue;}// 计算角度anglefloat angle = source_scan_->angle_min + i * source_scan_->angle_increment;// 计算thetafloat theta = current_pose.so2().log();// 计算pwVec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));// 创建Point2d对象ptPoint2d 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_ptsstd::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();// 计算误差edouble e = line_coeffs[0] * pw[0] + line_coeffs[1] * pw[1] + line_coeffs[2];// 更新b向量b += -J * e;// 更新costcost += e * e;}}// 如果有效点数量小于最小有效点数,则返回falseif (effective_num < min_effect_pts) {return false;}// 求解dxVec3d dx = H.ldlt().solve(b);// 如果dx中有NaN值,则跳出循环if (isnan(dx[0])) {break;}// 更新costcost /= 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]);// 更新lastCostlastCost = cost;}// 更新init_poseinit_pose = current_pose;// 输出估计的位姿信息LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()<< ", theta: " << current_pose.so2().log();// 返回truereturn 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;// 创建一个二维点对象pPoint2d 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();// 设置目标云对象的密集度为falsetarget_cloud_->is_dense = false;// 将目标云对象设置为kdtree的输入云kdtree_.setInputCloud(target_cloud_);
}} // namespace sad
这篇关于Cartographer中的2D扫描匹配算法的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!