cartographer中的扫描匹配

请添加图片描述

cartographer中的扫描匹配

cartographer中使用了两种扫描匹配方法:CSM(相关性扫描匹配方法(暴力匹配))、ceres优化匹配方法

CSM可以简单地理解为暴力搜索,即每一个激光数据与子图里的每一个位姿进行匹配,直到找到最优位姿,进行三层for循环,不过谷歌采用多分辨率搜索的方式,来降低计算量。基于优化的方法是谷歌的ceres库来实现,原理就是构造误差函数,使其对最小化,其精度高,但是对初始值敏感。

扫描匹配在cartographer中的调用

在cartographer的前端local_trajectory_builder_2d中会对传入的点云数据做各种处理,在对点云数据进行体素滤波之后传给ScanMatch进行扫描匹配。

// 扫描匹配, 进行点云与submap的匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

ScanMatch函数中调用real_time_correlative_scan_matcher_.Match函数进行相关性扫描匹配

​ 调用ceres_scan_matcher_.Match函数使用ceres优化匹配

// 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准if (options_.use_online_correlative_scan_matching()) {const double score = real_time_correlative_scan_matcher_.Match(pose_prediction, filtered_gravity_aligned_point_cloud,*matching_submap->grid(), &initial_ceres_pose);kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);}auto pose_observation = absl::make_unique<transform::Rigid2d>();ceres::Solver::Summary summary;// 使用ceres进行扫描匹配ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,filtered_gravity_aligned_point_cloud,*matching_submap->grid(), pose_observation.get(),&summary);

ScanMatch函数

ScanMatch需要传入的参数:

time //点云时间
pose_prediction //先验位姿
filtered_gravity_aligned_point_cloud //匹配用的点云

ScanMatch返回值:

std::unique_ptr<transform::Rigid2d> //匹配后的二维位姿

首先判断submaps是否为空,为空的话直接返回传入的先验位姿pose_prediction

if (active_submaps_.submaps().empty()) {return absl::make_unique<transform::Rigid2d>(pose_prediction);
}

不为空,向下继续执行

将submaps的第一个子图赋值给matching_submap,用于匹配

std::shared_ptr<const Submap2D> matching_submap =active_submaps_.submaps().front();

将传入的先验位姿赋值给matching_submap,作为ceres优化匹配的初始位姿

std::shared_ptr<const Submap2D> matching_submap =active_submaps_.submaps().front();

根据配置文件的use_online_correlative_scan_matching参数,选择是否使用相关性匹配对先验位姿进行校准。

配置参数在src/cartographer/configuration_files/trajectory_builder_2d.lua文件中

-- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息-- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果use_online_correlative_scan_matching = false,real_time_correlative_scan_matcher = {linear_search_window = 0.1,             -- 线性搜索窗口的大小angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小-- 用于计算各部分score的权重translation_delta_cost_weight = 1e-1,   -- 平移rotation_delta_cost_weight = 1e-1,      -- 旋转},

若use_online_correlative_scan_matching参数选择为TRUE,就调用real_time_correlative_scan_matcher_.Match函数,进行相关性扫描匹配,校准先验位姿,返回的是匹配得分

if (options_.use_online_correlative_scan_matching()) {const double score = real_time_correlative_scan_matcher_.Match(pose_prediction, filtered_gravity_aligned_point_cloud,*matching_submap->grid(), &initial_ceres_pose);kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}

若use_online_correlative_scan_matching参数选择为FALSE,则直接使用传入的先验位姿pose_prediction作为ceres优化的初始位姿,调用ceres_scan_matcher_.Match函数进行ceres优化匹配

auto pose_observation = absl::make_unique<transform::Rigid2d>();ceres::Solver::Summary summary;// 使用ceres进行扫描匹配ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,filtered_gravity_aligned_point_cloud,*matching_submap->grid(), pose_observation.get(),&summary);

相关性扫描匹配

相关性扫描匹配方法实现在real_time_correlative_scan_matcher_.Match这个函数

传入参数:

initial_pose_estimate //预测出来的先验位姿
point_cloud //用于匹配的点云 点云的原点位于local坐标系原点
grid //用于匹配的栅格地图

输出的参数:

pose_estimate //校正后的位姿

返回值:

double  best_candidate.score  //匹配得分

代码步骤:

  1. 将点云旋转到预测的方向上
  2. 生成按照不同角度旋转后的点云集合
  3. 将旋转后的点云集合按照预测出的平移量进行平移,获取平移后的点在地图中的索引
  4. 生成所有的候选解
  5. 计算所有候选解的加权得分
  6. 获取最优解
  7. 将计算出的偏差量加上原始位姿获得校正后的位姿

step1. 将点云旋转到预测的方向上

传入的点云是经过变换到local坐标系原点的点云数据,需要将传入的点云数据进行旋转变换,变换到预测的方向上。

//获取初始位姿的角度即旋转量
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
//将点云数据进行坐标变换
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(point_cloud,transform::Rigid3f::Rotation(Eigen::AngleAxisf(initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));

根据配置参数初始化 SearchParameters数据结构,定义搜索的空间,包括平移的窗口和角度窗口

const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(),rotated_point_cloud, grid.limits().resolution());

参数还是前面说的那个配置文件中

这里是调用了SearchParameters的构造函数,进入SearchParameters的构造函数

 // 求得 point_cloud 中雷达数据的 最大的值(最远点的距离)for (const sensor::RangefinderPoint& point : point_cloud) {const float range = point.position.head<2>().norm();max_scan_range = std::max(range, max_scan_range);}

计算论文中提到的角度分辨率
在这里插入图片描述

其中:d是雷达的最远距离,r是栅格的分辨率

求θ

根据余弦定理:
c o s θ = b 2 + c 2 − a 2 2 b c cosθ = \frac{b^2 + c^2 - a^2}{2bc} cosθ=2bcb2+c2a2
则有
$$
cosθ = \frac{d^2 + d^2 - r^2}{2dd}

 = 1 - \frac{r^2}{2d^2}

即 即
θ = arccos(1-\frac{r2}{2d2})
$$

// 根据论文里的公式 求得角度分辨率 angular_perturbation_step_size
const double kSafetyMargin = 1. - 1e-3;
angular_perturbation_step_size =kSafetyMargin * std::acos(1. - common::Pow2(resolution) /(2. * common::Pow2(max_scan_range)));

根据传入的角度搜索窗除以角度分辨率,就能够得到划分的角度个数范围

// 范围除以分辨率得到个数
num_angular_perturbations =std::ceil(angular_search_window / angular_perturbation_step_size);

将划分得到的角度个数范围扩大2倍再加1,得到要生成的旋转点云的个数。划分得到的角度个数是一个正数,点云会从他的负的最小开始算,所以会乘以2倍,还有0所以会加1,就是点云的个数。

num_scans = 2 * num_angular_perturbations + 1;

用传入的线性搜索窗除以分辨率可以得到xy方向的搜索范围,单位是栅格

const int num_linear_perturbations =std::ceil(linear_search_window / resolution);

确定每一个点云的最大最小边界

linear_bounds.reserve(num_scans);
for (int i = 0; i != num_scans; ++i) {linear_bounds.push_back(LinearBounds{-num_linear_perturbations, num_linear_perturbations,-num_linear_perturbations, num_linear_perturbations});

step2. 生成按照不同角度旋转后的点云集合

const std::vector<sensor::PointCloud> rotated_scans =GenerateRotatedScans(rotated_point_cloud, search_parameters);

search_parameters存了需要旋转的点云的个数num_scans,乘以角分辨率之后就是不同的角度

进入GenerateRotatedScans函数

首先计算起始角度,划分得到的角度范围num_angular_perturbations是从负值到正值的,起始位置就从负的最小角开始,再乘以分辨率得到需要的角度

double delta_theta = -search_parameters.num_angular_perturbations *search_parameters.angular_perturbation_step_size;

最后进行遍历,生成旋转不同角度后的点云集合

for (int scan_index = 0; scan_index < search_parameters.num_scans;++scan_index,delta_theta += search_parameters.angular_perturbation_step_size) {// 将 point_cloud 绕Z轴旋转了delta_thetarotated_scans.push_back(sensor::TransformPointCloud(point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(delta_theta, Eigen::Vector3f::UnitZ()))));
}

step3. 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引

const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(grid.limits(), rotated_scans,Eigen::Translation2f(initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y()));

调用DiscretizeScans函数,将旋转后的点云集合按照预测出的平移量进行平移,获取平移后的点在地图中的索引

先定义一个DiscreteScan2D结构的变量 discrete_scans,用于存放平移后的点云在地图中的索引

然后申请相应点云数量的空间

std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());

进入第一层for循环,遍历每一个角度的点云

for (const sensor::PointCloud& scan : scans) {// discrete_scans中的每一个 DiscreteScan2D 的size设置为这一帧点云中所有点的个数discrete_scans.emplace_back();discrete_scans.back().reserve(scan.size());...}

进入第二层for循环,对点云中的每个点进行平移,获取平移后的栅格索引

for (const sensor::RangefinderPoint& point : scan) {// 对scan中的每个点进行平移const Eigen::Vector2f translated_point =Eigen::Affine2f(initial_translation) * point.position.head<2>();// 将平移后的点 对应的栅格的索引放入discrete_scansdiscrete_scans.back().push_back(map_limits.GetCellIndex(translated_point));}

通过point.position.head<2>取出点云的x,y坐标,然后乘以一个平移量Affine2f(initial_translation),对这个点进行平移。

const Eigen::Vector2f translated_point =Eigen::Affine2f(initial_translation) * point.position.head<2>();

通过平移后的点的坐标传入GetCellIndex函数,获得物理坐标在栅格地图中的栅格的索引

map_limits.GetCellIndex(translated_point));

最后返回平移后的点在地图中的索引

return discrete_scans

step4. 生成所有的候选解

std::vector<Candidate2D> candidates =GenerateExhaustiveSearchCandidates(search_parameters);

进入GenerateExhaustiveSearchCandidates函数,首先获取所有候选解的个数

遍历经过旋转后的所有角度的点云,判断每一个角度上的点云会有多少种可能性,将其记录到num_candidates中。

首先计算x坐标上的点数,然后计算对应y坐标上的点数,将x乘以y得到总的可能性。

举例说明,假设栅格中的一个点在x方向有十个栅格的的距离,在y方向上有五个栅格的距离,那么他总共可平移的栅格就是50个,然后总共有8个点,那么所有的平移栅格就是400个。

for (int scan_index = 0; scan_index != search_parameters.num_scans;++scan_index) {const int num_linear_x_candidates =(search_parameters.linear_bounds[scan_index].max_x -search_parameters.linear_bounds[scan_index].min_x + 1);const int num_linear_y_candidates =(search_parameters.linear_bounds[scan_index].max_y -search_parameters.linear_bounds[scan_index].min_y + 1);num_candidates += num_linear_x_candidates * num_linear_y_candidates;
}

然后生成候选解,首先遍历所有角度的点云个数,然后遍历所有的点云的x坐标,最后遍历每个x偏移量,y偏移量,最后将点云索引,x坐标,y坐标,配置参数传给 candidates 生成所有的候选解。

for (int scan_index = 0; scan_index != search_parameters.num_scans;++scan_index) {for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;++x_index_offset) {for (int y_index_offset =search_parameters.linear_bounds[scan_index].min_y;y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;++y_index_offset) {candidates.emplace_back(scan_index, x_index_offset, y_index_offset,search_parameters);}}
}

最后检查得到的候选解的size是否等于候选解的个数num_candidates,相等就返回所有候选解

CHECK_EQ(candidates.size(), num_candidates);
return candidates;

step5. 计算所有候选解得加权得分

ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

进入ScoreCandidates函数

遍历所有候选解,判断传入的地图类型(栅格地图(PROBABILITY_GRID)或三维点云地图(TSDF))

将点云在栅格地图中的索引,x、y的偏移量传给ComputeCandidateScore函数,计算每个栅格的得分。

for (Candidate2D& candidate : *candidates) {switch (grid.GetGridType()) {case GridType::PROBABILITY_GRID:candidate.score = ComputeCandidateScore(static_cast<const ProbabilityGrid&>(grid),discrete_scans[candidate.scan_index], candidate.x_index_offset,candidate.y_index_offset);break;

进入ComputeCandidateScore函数

遍历点云在栅格地图中的xy坐标,加上xy的偏移量得到点云的平移和旋转之后的在栅格地图中的坐标

for (const Eigen::Array2i& xy_index : discrete_scan) {// 对每个点都加上像素坐标的offset, 相当于对点云进行平移const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,xy_index.y() + y_index_offset);

获取平移后的点云在栅格中占用的概率

const float probability =probability_grid.GetProbability(proposed_xy_index);

将获得的所有点的占用概率累加,得到此候选解得概率得分

  candidate_score += probability;}

将累加得分除以点的个数,获得此候选解的平均得分,并返回

candidate_score /= static_cast<float>(discrete_scan.size());
CHECK_GT(candidate_score, 0.f);
return candidate_score;

最后回到ScoreCandidates函数,对每个得分根据参数配置的平移和旋转的权重进行加权

candidate.score *=std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *options_.translation_delta_cost_weight() +std::abs(candidate.orientation) *options_.rotation_delta_cost_weight()));

step6. 获取最优解

使用std::max_element获取候选解中得分最高的一组最为最优解

const Candidate2D& best_candidate =*std::max_element(candidates.begin(), candidates.end());

step7. 将计算出的偏差量加上原始位姿获得校正后的位姿

*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;

基于ceres的扫描匹配

ceres基本原理

残差方程

基于ceres的扫描匹配(即非线性优化)的实现在ceres_scan_matcher_.Match函数,代码文件位置:src/cartographer/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc、.h

CreateCeresScanMatcherOptions2D的构造函数主要是对配置文件中的参数进行读取,主要还是在CeresScanMatcher2D类中实现。类CeresScanMatcher2D只定义了两个成员变量。 其中,options_ 用于记录扫描匹配器的相关配置,而变量ceres_solver_options_则是优化过程的配置。

CeresScanMatcher2D类的核心就在于Match这个函数的实现,这个函数的目的就是扫描匹配

在给定机器人的初始位姿估计initial_pose_estimate的情况下,尽可能的将扫描的点云数据point_cloud放置到栅格地图grid上, 同时返回优化后的位姿估计pose_estimate和优化迭代信息summary。而参数target_translation主要用于约束位姿估计的xy坐标, 在Local SLAM的业务主线调用该函数的时候传递的是位姿估计器的估计值,Cartographer认为优化后的机器人位置应当与该估计值的偏差不大。

输入参数:

 * @param[in] target_translation 预测出来的先验位置, 只有xy* @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点* @param[in] grid 用于匹配的栅格地图

输出参数:

 * @param[out] pose_estimate 优化之后的位姿* @param[out] summary 优化迭代信息

首先是创建一个double数组,用于存放输入的初始位姿x,y和θ,然后创建一个problem的对象用于描述将要求解的问题。

double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y(),initial_pose_estimate.rotation().angle()};
ceres::Problem problem;

然后就是描述地图、平移和旋转的残差,在配置文件中为这三类来源定义了权重。

CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock(CreateOccupiedSpaceCostFunction2D(options_.occupied_space_weight() /std::sqrt(static_cast<double>(point_cloud.size())),point_cloud, grid),nullptr /* loss function */, ceres_pose_estimate);CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.translation_weight(), target_translation), // 平移的目标值, 没有使用校准后的平移(这里可能有点问题)nullptr /* loss function */, ceres_pose_estimate);      // 平移的初值// 旋转的残差, 固定了角度不变
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.rotation_weight(), ceres_pose_estimate[2]), // 角度的目标值nullptr /* loss function */, ceres_pose_estimate);       // 角度的初值

Cartographer为三类不同的残差源分别定义了一个类,并通过其静态函数CreateAutoDiffCostFunction提供使用Ceres原生的自动求导方法的代价函数计算。

通过其中RotationDeltaCostFunctor2D类来看看CreateAutoDiffCostFunction这个函数的实现

CreateAutoDiffCostFunction是类RotationDeltaCostFunctor2D公开的静态成员。有两个参数,其中scaling_factor是角度偏差的权重,可以通过配置文件指定; target_angle则是参考角度, 实际的调用传参是优化之前的角度估计。该函数最终构造和返回的是Ceres的AutoDiffCostFunction对象,从第2行中的模板列表中可以看到类RotationDeltaCostFunctor2D只提供一个残差项,其参与优化的参数有3个。

class RotationDeltaCostFunctor2D {public:// 静态成员函数, 返回CostFunctionstatic ceres::CostFunction* CreateAutoDiffCostFunction(const double scaling_factor, const double target_angle) {return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>(new RotationDeltaCostFunctor2D(scaling_factor, target_angle));}
template <typename T>
bool operator()(const T* const pose, T* residual) const {residual[0] = scaling_factor_ * (pose[2] - angle_);return true; 
}

左边是类RotationDeltaCostFunctor2D对运算符"()"的重载,在第四行中计算角度偏差量并乘上权重得到残差项的代价。 这里的scaling_factor_和angle_是在类RotationDeltaCostFunctor2D中定义的两个私有成员变量分别记录了权重和参考角度。它们通过构造函数和静态函数CreateAutoDiffCostFunction的传参赋值。

类TranslationDeltaCostFunctor2D和OccupiedSpaceCostFunction2D也是以相同的套路实现的,把构造函数定义成私有的,防止用户直接构造对象。 在静态函数CreateAutoDiffCostFunction中以这两个类型为基础构建AutoDiffCostFunction对象。类TranslationDeltaCostFunctor2D定义了成员变量x_和y_用于记录位移偏差量的参考坐标。 类OccupiedSpaceCostFunction2D则使用成员变量point_cloud_和grid_分别记录待匹配的激光点云数据和栅格地图。

左边是类TranslationDeltaCostFunctor2D对运算符"()"的重载,它有两个残差项的代价需要计算,分别对应着x轴和y轴上的偏差量。在第4,5行中计算位置偏差量并乘上权重(尺度因子)来更新对应残差项的代价。

// 平移量残差的计算, (pose[0] - x_)的平方ceres会自动加上
template <typename T>
bool operator()(const T* const pose, T* residual) const {residual[0] = scaling_factor_ * (pose[0] - x_);residual[1] = scaling_factor_ * (pose[1] - y_);return true;
}

这里计算残差,都是基于使用的传感器预测的位姿很准的情况。

所以在调参的时候,如果传感器不是很好,位姿估计器不准确,可以适当的将地图的权重调的大于平移和旋转的权重。若是调参一直调不好,可以将计算平移和旋转的残差注释掉。

最后看看地图部分的残差

同样也是通过CreateAutoDiffCostFunction函数构建costFunction,他的构造是将地图的权重除以点云的数量,得到一个平均值,然后再进行传入,所以地图的权重就会被分散,当点云的数量越来越多时,地图的权重就会被分散的越来越小。

problem.AddResidualBlock(CreateOccupiedSpaceCostFunction2D(options_.occupied_space_weight() /std::sqrt(static_cast<double>(point_cloud.size())),point_cloud, grid),nullptr /* loss function */, ceres_pose_estimate);

下面是类OccupiedSpaceCostFunction2D对运算符"()"的重载。在函数的一开始,先把迭代查询的输入参数pose转换为坐标变换Tε用临时变量transform记录。

template <typename T>
bool operator()(const T* const pose, T* residual) const {Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);Eigen::Rotation2D<T> rotation(pose[2]);Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();Eigen::Matrix<T, 3, 3> transform;transform << rotation_matrix, translation, T(0.), T(0.), T(1.);

然后使用Ceres库原生提供的双三次插值迭代器。

const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = grid_.limits();

针对每个hit点计算对应的残差代价。

for (size_t i = 0; i < point_cloud_.size(); ++i) {// Note that this is a 2D point. The third component is a scaling factor.const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),(T(point_cloud_[i].position.y())),T(1.));// 根据预测位姿对单个点进行坐标变换const Eigen::Matrix<T, 3, 1> world = transform * point;// 获取三次插值之后的栅格free值, Evaluate函数内部调用了GetValue函数interpolator.Evaluate((limits.max().x() - world[0]) / limits.resolution() - 0.5 +static_cast<double>(kPadding),(limits.max().y() - world[1]) / limits.resolution() - 0.5 +static_cast<double>(kPadding),&residual[i]);// free值越小, 表示占用的概率越大residual[i] = scaling_factor_ * residual[i];
}

参考:https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=%E5%9F%BA%E4%BA%8ECeres%E5%BA%93%E7%9A%84%E6%89%AB%E6%8F%8F%E5%8C%B9%E9%85%8D%E5%99%A8

使用ceres优化扫描匹配,除了要求hit点在占用栅格上出现的概率最大化之外, 还通过两个残差项约束了优化后的位姿估计在原始估计的附近。

总结

cartographer中使用了两种扫描匹配方法:CSM(相关性扫描匹配方法(暴力匹配))、ceres优化匹配方法

CSM可以简单地理解为暴力搜索,即每一个激光数据与子图里的每一个位姿进行匹配,直到找到最优位姿,进行三层for循环,不过谷歌采用多分辨率搜索的方式,来降低计算量。基于优化的方法是谷歌的ceres库来实现,原理就是构造误差函数,使其对最小化,其精度高,但是对初始值敏感。

在cartographer的前端local_trajectory_builder_2d中会对传入的点云数据做各种处理,在对点云数据进行体素滤波之后传给ScanMatch进行扫描匹配。

// 扫描匹配, 进行点云与submap的匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

ScanMatch函数中调用real_time_correlative_scan_matcher_.Match函数进行相关性扫描匹配

eres%E5%BA%93%E7%9A%84%E6%89%AB%E6%8F%8F%E5%8C%B9%E9%85%8D%E5%99%A8

使用ceres优化扫描匹配,除了要求hit点在占用栅格上出现的概率最大化之外, 还通过两个残差项约束了优化后的位姿估计在原始估计的附近。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/105447.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

为什么手机会莫名多出许多软件?

许多手机用户都曾遭遇过这样的问题&#xff0c;他们在使用手机的过程中&#xff0c;突然发现手机屏幕上出现了一些未知的软件。这些软件并非他们主动下载的&#xff0c;但它们却显现在屏幕上。这些软件从何而来&#xff1f; 其实&#xff0c;这些软件往往是在浏览网页、阅读小…

精品Python基于django就业数据分析平台求职招聘应聘-爬虫可视化大屏

《[含文档PPT源码等]精品基于django就业数据分析平台-爬虫》该项目含有源码、文档、PPT、配套开发软件、软件安装教程、项目发布教程等&#xff01; 软件开发环境及开发工具&#xff1a; 开发语言&#xff1a;python 使用框架&#xff1a;Django 前端技术&#xff1a;JavaS…

从基础到卷积神经网络(第13天)

1. PyTorch 神经网络基础 1.1 模型构造 1. 块和层 首先&#xff0c;回顾一下多层感知机 import torch from torch import nn from torch.nn import functional as Fnet nn.Sequential(nn.Linear(20, 256), nn.ReLU(), nn.Linear(256, 10))X torch.rand(2, 20) # 生成随机…

容器化应用程序管理与分发工具集 | 开源专题 No.34

kubernetes/kubernetes Stars: 101.7k License: Apache-2.0 Kubernetes 是一个开源系统&#xff0c;用于管理跨多个主机的容器化应用程序。它提供了部署、维护和扩展应用程序的基本机制。Kubernetes 借鉴了 Google 在大规模运行生产负载方面十五年的经验&#xff0c;并结合了社…

【Excel】【latex】将EXCEL中单元格的计算关系还原为公式,用c#重构

在excel中&#xff0c;将很多个单元格&#xff0c;及其内部的公式&#xff0c;用文本的形式复制出来 Ctrl ~将 Excel 切换到公式视图&#xff0c;可以看到单元格中的公式&#xff0c;而不是公式的结果。 像平常一样复制和粘贴单元格。粘贴时&#xff0c;会看到的是单元格中的…

Android studio控制台 输出乱码解决方法

在AS的安装目录&#xff0c;找到 studio64.exe.vmoptions 文件&#xff0c; 用编辑器打开文件&#xff0c;在最后面加上下面的代码&#xff1a; -Defile.encodingUTF-8然后 重启AS。 注意&#xff1a; 下面两种方式也能打开studio64.exe.vmoptions 文件&#xff0c;但是需要确…

【ELK 使用指南】ELK + Filebeat 分布式日志管理平台部署

ELK和EFLK 一、前言1.1 日志分析的作用1.2 需要收集的日志1.3 完整日志系统的基本特征 二、ELK概述2.1 ELK简介2.2 为什么要用ELK?2.3 ELK的组件 三、ELK组件详解3.1 Logstash3.1.1 简介3.1.2 Logstash命令常用选项3.1.3 Logstash 的输入和输出流3.1.4 Logstash配置文件 3.2 E…

关于网络协议的若干问题(四)

1、QUIC 是一个精巧的协议&#xff0c;它有哪些特性&#xff1f; 答&#xff1a;QUIC 还有其他特性&#xff0c;一个是快速建立连接。另一个是拥塞控制&#xff0c;QUIC 协议当前默认使用了 TCP 协议的 CUBIC&#xff08;拥塞控制算法&#xff09;。 CUBIC 进行了不同的设计&…

Hadoop问题:start-all.sh显示未找到命令

在sbin文件夹下是start-all.sh可以运行的&#xff0c;但是到了别的文件夹下就不行了&#xff0c;于是想到了是文件路径问题&#xff0c;因为hadoop环境是和java环境一起配置的导致sbin写成了bin 解决办法&#xff1a; 打开.bashrc配置hadoop的环境变量 sudo vim ~/.bashrc …

nodejs+vue教学辅助管理系统

目 录 摘 要 I ABSTRACT II 目 录 II 第1章 绪论 1 1.1背景及意义 1 1.2 国内外研究概况 1 1.3 研究的内容 1 第2章 相关技术 3 2.1 nodejs简介 4 2.2 express框架介绍 6 2.4 MySQL数据库 4 第3章 系统分析 5 3.1 需求分析 5 3.2 系统可行性分析 5 3.2.1技术可行性&#xff1a;…

Folium 笔记:使用PopUp突出subzone的空间分布

0 效果图 点开某一个区域后&#xff0c;内容是这个区域的用地类型分布 1 读取数据 import folium import matplotlib.pyplot as plt import re import geopandas as gpd subzonegpd.read_file(MasterPlan2019PlanningAreaBoundaryNoSea.geojson) subzone 2 提取subzone 信息 …

从零开始的stable diffusion

stable diffusion真的是横空出世&#xff0c;开启了AIGC的元年。不知你是否有和我一样的困惑&#xff0c;这AI工具好像并不是那么听话&#xff1f; 前言 我们该如何才能用好stable diffusion这个工具呢&#xff1f;AI究竟在stable diffusion中承担了什么样的角色&#xff1f;如…

分享一个查询OpenAI Chatgpt key余额查询的工具网站

OpenAI Key 余额查询工具 欢迎使用 OpenAI Key 余额查询工具网站&#xff01;这个工具可以帮助您轻松地验证您的 OpenAI API 密钥&#xff0c;并查看您的余额。 http://tools.lbbit.top/check_key/ 什么是 OpenAI Key 余额查询工具&#xff1f; OpenAI Key 余额查询工具是一…

python实现图像的直方图均衡化

直方图均衡化是一种用于增强图像对比度的图像处理技术。它通过重新分配图像中的像素值&#xff0c;使得图像的像素值分布更加均匀&#xff0c;增强图像的对比度&#xff0c;从而改善图像的视觉效果。 直方图均衡化的过程如下&#xff1a; 灰度转换&#xff1a;如果图像是彩色…

露营装备经营商城小程序搭建

近几年露营人群逐渐增加&#xff0c;相应的装备商也多了起来&#xff0c;各种分类商品在一定程度上销量都非常不错&#xff0c;然而传统线下门店经营方面&#xff0c;面对的痛点也不少。 通过【雨科】平台搭建露营装备商城&#xff0c;让客户多场景平台随时购物&#xff0c;多流…

【Java学习之道】线程的概念与作用

引言 今天我们将探索多线程编程的基础概念和作用。对于初学者来说&#xff0c;掌握多线程编程是迈向Java高级技能的重要一步。通过本章的学习&#xff0c;你将了解线程是什么以及它在程序开发中的重要性&#xff0c;为你进一步深入学习和实际工作打下坚实的基础。让我们一起来…

《向量数据库指南》——Milvus Cloud和Elastic Cloud 特性对比

随着以 Milvus 为代表的向量数据库在 AI 产业界越来越受欢迎,诸如 Elasticsearch 之类的传统数据库和检索系统也开始行动起来,纷纷在快速集成专门的向量检索插件方面展开角逐。 例如,在提供类似插件的传统数据库中,Elasticsearch 8.0 首屈一指,推出了包括向量插入和最相似…

【软考】9.2 串/数组/矩阵/广义表/树

《字符串》 一种特殊的线性表&#xff0c;数据元素都为字符模式匹配&#xff1a;寻找子串第一次在主串出现的位置 模式匹配算法 1. 暴力破解法&#xff08;布鲁特-福斯算法&#xff09; 主串与子串一个个匹配效率低 2. KMP算法 主串后缀和子串前缀能否找到一样的元素&#xf…

ARM day9

src/key_it.c #include "key_it.h" #include "led.h" void key_it_config() {//RCC使能GPIOF时钟RCC->MP_AHB4ENSETR | (0x1<<5);//设置PF9 PF7 PF8GPIO输入//PF9GPIOF->MODER & (~(0x3<<18));//PF8GPIOF->MODER & (~(0x3&l…

互联网Java工程师面试题·Java 并发编程篇·第八弹

目录 33、Java 死锁以及如何避免&#xff1f; 34、死锁的原因 35、怎么唤醒一个阻塞的线程 36、不可变对象对多线程有什么帮助 37、什么是多线程的上下文切换 38、如果你提交任务时&#xff0c;线程池队列已满&#xff0c;这时会发生什么这里区分一下&#xff1a; 39、J…