一、前言
在阅读D-LIOM文章的时候看不太懂他们写的约束构建，返回来细致的看一下原版Carto关于这部分的代码，有时间的话可能也解读一下D-LIOM。关于Cartographer_3d后端约束建立的梳理和想法，某些变量可能与开源版本不一致，代码整体结构没有太大修改（源码版本Carto1.0Master）。比之前的水文可能会细致一点。
二、关于Cartographer_3d
在pose_graph_xd.cc中通过ComputeConstraint函数构造约束任务并调用constraint_builder_相关函数，这个函数在两个地方被调用，分别对应新节点-旧子图、旧节点-新子图，就不展开废话了。
void PoseGraph3D::ComputeConstraint(const NodeIdCarto& node_id,
const SubmapIdCarto& submap_id) {
//只与Finished子图建立约束
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
//optimization_problem_中，节点的全局位姿（可能已经经过优化）
const transform::Rigid3d global_node_pose =
optimization_problem_->node_data().at(node_id).global_pose;
//optimization_problem_中，子图的全局位姿（可能已经经过优化）
const transform::Rigid3d global_submap_pose =
optimization_problem_->submap_data().at(submap_id).global_pose;
//前者的逆
const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse();
//构建一个vector对象，类型是TrajectoryNode
//里面主要存两大块东西，constant_data即节点信息，来自pose_graph，
//global_pose是用后端全局优化过的位姿修正过的pose_graph里面存着的位姿
std::vector<TrajectoryNodeCarto> submap_nodes;
for (const NodeIdCarto& submap_node_id : submap_data_.at(submap_id).node_ids) {
submap_nodes.push_back(
TrajectoryNodeCarto{trajectory_nodes_.at(submap_node_id).constant_data,
global_submap_pose_inverse *
trajectory_nodes_.at(submap_node_id).global_pose});
}
//时间判断，如果符合要求就添加约束（一条轨迹、几条轨迹之间的约束）
const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
node_time <
last_connection_time +
common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
// If the node and the submap belong to the same trajectory or if there has
// been a recent global constraint that ties that node's trajectory to the
// submap's trajectory, it suffices to do a match constrained to a local
// search window.
//如果节点和子图属于同一轨迹，或者存在
//是最近的全局约束，将该节点的轨迹绑定到
//子图的轨迹，它足以做一个约束到局部的匹配搜索窗口。
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
global_node_pose, global_submap_pose);
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
// In this situation, 'global_node_pose' and 'global_submap_pose' have
// orientations agreeing on gravity. Their relationship regarding yaw is
// arbitrary. Finding the correct yaw component will be handled by the
// matching procedure in the FastCorrelativeScanMatcher, and the given yaw
// is essentially ignored.
//在这种情况下，'global_node_pose'和'global_submap_pose'有
//重力方向一致。它们关于偏航的关系是
//任意的。FastCorrelativeScanMatcher中的匹配过程将处理查找正确的偏航角
//以及给定的偏航角基本上被忽略。
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
global_node_pose.rotation(), global_submap_pose.rotation());
}
}如果是普通的3d建图，不涉及多条轨迹的建立的话，走的应该都是constraint_builder_的MaybeAddConstraint接口，不过MaybeAddConstraint和MaybeAddGlobalConstraint的差别其实也不太大，只是是否进行全局匹配，与所有的子图进行匹配。
约束的构建在constraint_builder_3d.cc中的MaybeAddConstraint函数，他有七个传入量（以同一条轨迹的节点与子图分析）：
| SubmapIdCarto& submap_id | 子图id，为Finished_submap |
| Submap3D* const submap | 子图信息，由PoseGraph3D::Addnode构建 |
| NodeIdCarto& node_id | 节点id，由PoseGraph3D::Addnode构建 |
| TrajectoryNodeCarto::Data* const constant_data | 节点信息，同上 |
| std::vector<TrajectoryNodeCarto>& submap_nodes | 由PoseGraph3D::ComputeConstraint构建，上文有写 |
| transform::Rigid3d& global_node_pose | 上文有写，来自 optimization_problem_的全局位姿 |
| transform::Rigid3d& global_submap_pose | 上文有写，来自 optimization_problem_的全局位姿 |
void ConstraintBuilder3D::MaybeAddConstraint(
const SubmapIdCarto& submap_id, const Submap3D* const submap,
const NodeIdCarto& node_id, const TrajectoryNodeCarto::Data* const constant_data,
const std::vector<TrajectoryNodeCarto>& submap_nodes,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose) {
//距离判断,太远就不构建约束了
if ((global_node_pose.translation() - global_submap_pose.translation())
.norm() > options_.max_constraint_distance()) {
return;
}
if (!sampler_.Pulse()) return;
//线程相关操作，防止误修改
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
}
//头部新建一个空约束，然后指针指向空约束，c++11的新规范
constraints_.emplace_back();
// kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
//构建扫描匹配器
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
//构造约束任务，塞到任务队列里
auto constraint_task = common::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
constant_data, global_node_pose, global_submap_pose,
*scan_matcher, constraint);
});
//进行线程池调度
constraint_task->AddDependency(scan_matcher->creation_task_handle);
auto constraint_task_handle =
thread_pool_->Schedule(std::move(constraint_task));
finish_node_task_->AddDependency(constraint_task_handle);
}核心函数是DispatchScanMatcherConstruction，构建扫描匹配器，ComputuConstraint，计算节点和子图之间的具体约束。
//构建扫描匹配器
const ConstraintBuilder3D::SubmapScanMatcher*
ConstraintBuilder3D::DispatchScanMatcherConstruction(
const SubmapIdCarto& submap_id, const std::vector<TrajectoryNodeCarto>& submap_nodes,
const Submap3D* submap) {
//如果已经构建过了
if (submap_scan_matchers_.count(submap_id) != 0) {
return &submap_scan_matchers_.at(submap_id);
}
//将子图的高低分辨率地图、匹配参数传入
auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
submap_scan_matcher.high_resolution_hybrid_grid =
&submap->high_resolution_hybrid_grid();
submap_scan_matcher.low_resolution_hybrid_grid =
&submap->low_resolution_hybrid_grid();
auto& scan_matcher_options =
options_.fast_correlative_scan_matcher_options_3d();
//构造匹配任务，传入匹配器、点云地图、子图内节点信息，用FastCorrelativeScanMatcher匹配器
auto scan_matcher_task = common::make_unique<common::Task>();
scan_matcher_task->SetWorkItem(
[&submap_scan_matcher, &scan_matcher_options, submap_nodes]() {
submap_scan_matcher.fast_correlative_scan_matcher =
common::make_unique<scan_matching3d::FastCorrelativeScanMatcher3D>(
*submap_scan_matcher.high_resolution_hybrid_grid,
submap_scan_matcher.low_resolution_hybrid_grid, submap_nodes,
scan_matcher_options);
});
submap_scan_matcher.creation_task_handle =
thread_pool_->Schedule(std::move(scan_matcher_task));
return &submap_scan_matchers_.at(submap_id);
}//计算约束
void ConstraintBuilder3D::ComputeConstraint(
const SubmapIdCarto& submap_id, const NodeIdCarto& node_id, bool match_full_submap,
const TrajectoryNodeCarto::Data* const constant_data,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint) {
//先快速计算一个match_result
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'high_resolution_point_cloud' in node j and
// - the initial guess 'initial_pose' (submap i <- node j).
//约束的位姿估计计算由 节点内高分辨率点云地图 与 节点到子图的位姿估计 计算得到
std::unique_ptr<scan_matching3d::FastCorrelativeScanMatcher3D::Result>
match_result;
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
//如果是不同轨迹之间的节点和子图
if (match_full_submap) {
// kGlobalConstraintsSearchedMetric->Increment();
//构建FastCorrelativeScanMatcher3D，分支定界计算match_result
match_result =
submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
global_node_pose.rotation(), global_submap_pose.rotation(),
*constant_data, options_.global_localization_min_score());
if (match_result != nullptr) {
CHECK_GT(match_result->score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
// kGlobalConstraintsFoundMetric->Increment();
// kGlobalConstraintScoresMetric->Observe(match_result->score);
// kGlobalConstraintRotationalScoresMetric->Observe(
// match_result->rotational_score);
// kGlobalConstraintLowResolutionScoresMetric->Observe(
// match_result->low_resolution_score);
} else {
return;
}
} else {
//如果是同一轨迹之间的话
// kConstraintsSearchedMetric->Increment();
match_result = submap_scan_matcher.fast_correlative_scan_matcher->Match(
global_node_pose, global_submap_pose, *constant_data,
options_.min_score());
if (match_result != nullptr) {
// We've reported a successful local match.
CHECK_GT(match_result->score, options_.min_score());
// kConstraintsFoundMetric->Increment();
// kConstraintScoresMetric->Observe(match_result->score);
// kConstraintRotationalScoresMetric->Observe(
// match_result->rotational_score);
// kConstraintLowResolutionScoresMetric->Observe(
// match_result->low_resolution_score);
} else {
return;
}
}
//match_result计算完之后，在直方图表里面加入计算结果
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(match_result->score);
rotational_score_histogram_.Add(match_result->rotational_score);
low_resolution_score_histogram_.Add(match_result->low_resolution_score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
//ceres_scan_matcher_计算精确的节点、子图位姿匹配结果
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
match_result->pose_estimate,
{{&constant_data->high_resolution_point_cloud,
submap_scan_matcher.high_resolution_hybrid_grid},
{&constant_data->low_resolution_point_cloud,
submap_scan_matcher.low_resolution_hybrid_grid}},
&constraint_transform, &unused_summary);
//重新构建constraint
constraint->reset(new Constraint{
submap_id,
node_id,
{constraint_transform, options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
//打印、记录结果
if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with "
<< constant_data->high_resolution_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {
// Compute the difference between (submap i <- node j) according to loop
// closure ('constraint_transform') and according to global SLAM state.
const transform::Rigid3d difference = global_node_pose.inverse() *
global_submap_pose *
constraint_transform;
info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference);
}
info << " with score " << std::setprecision(1) << 100. * match_result->score
<< "%.";
LOG(ERROR) << info.str();
}
}DispatchScanMatcherConstruction主要为每一个submap都构建一个扫描匹配器，把活动子图的高低分辨率地图，以及子图内的节点加进去，然后用来和后面的东西进行匹配。
ComputuConstraint的话就是分支定界的思路，先用FastCorrelativeScanMatcher3D快速搜索确定一个初始match_result，判断有没有进行后面计算的必要，搜索的对象是节点高分辨率点云地图对应DispatchScanMatcherConstruction里面构建的子图的高分辨率地图，然后再用ceres_scan_matcher_进行一个细致的匹配计算。这样整个Constraint就构建完成了。
三、关于D-LIOM
D-LIOM是同济大学团队开源的一项基于Carto3d的工作，链接在这里。主要添加了前端IMU-激光里程计的紧耦合系统，以及对后端位姿图的建立的一些优化。
在pose_graph_3d.cc中，他们更改了子图-节点约束的建立方式，不使用原来新节点-旧子图、新子图-旧节点的方案，采用了全新的ComputeConstraintsForSubmap函数。在新完成建立的子图时，调用这个函数进行工作。
//计算子图之间的约束
void PoseGraph3D::ComputeConstraintsForSubmap(
const SubmapId& submap_id){
// LOG(WARNING)<<"submap_id.submap_index: " << submap_id.submap_index;
//优化过的全局位姿、局部坐标系下位姿、全局位姿的逆
const transform::Rigid3d global_submap_pose =
optimization_problem_->submap_data().at(submap_id).global_pose;
const transform::Rigid3d local_submap_pose =
submap_data_.at(submap_id).submap->local_pose();
const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse();
//取出对应子图下的所有node，和轨迹信息组成一个向量对
std::vector<std::pair<NodeId, TrajectoryNode> > submap_nodes;
for (const NodeId& submap_node_id : submap_data_.at(submap_id).node_ids) {
submap_nodes.push_back({submap_node_id,
//轨迹信息包含点云数据，以及转换到地图坐标系下的位姿
TrajectoryNode{trajectory_nodes_.at(submap_node_id).constant_data,
/* global_submap_pose_inverse *
trajectory_nodes_.at(submap_node_id).global_pose */
//子图位姿的逆左乘节点位姿，求得节点在地图坐标系下的位姿
local_submap_pose.inverse()
* trajectory_nodes_.at(submap_node_id).constant_data->local_pose}
});
}
//调用约束构建函数
constraint_builder_.DispatchScanMatcherConstruction(
submap_id, local_submap_pose, submap_nodes,
submap_data_.at(submap_id).submap.get());
}这里和carto3d不一样，直接调用了DispatchScanMatcherConstruction函数，一步到位实现carto3d里面的两个功能。
//约束构建函数
void ConstraintBuilder3D::DispatchScanMatcherConstruction(
const SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose,
const std::vector<std::pair<NodeId, TrajectoryNode>>& submap_nodes,
const Submap3D* submap) {
//线程锁，异常检查
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING) << "DispatchScanMatcherConstruction was called"
<< " while WhenDone was scheduled.";
}
if (submap_scan_matchers_.count(submap_id) != 0) {
return;
}
//子图匹配器初始化
auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
submap_scan_matcher.high_resolution_hybrid_grid =
&submap->high_resolution_hybrid_grid();
submap_scan_matcher.low_resolution_hybrid_grid =
&submap->low_resolution_hybrid_grid();
auto& scan_matcher_options =
options_.fast_correlative_scan_matcher_options_3d();
//一个空容器，装入子图内所有node
std::vector<TrajectoryNode> nodes_wiouout_id={};
for(const auto& node: submap_nodes){
nodes_wiouout_id.emplace_back(node.second);
}
auto scan_matcher_task = common::make_unique<common::Task>();
//捕获列表临时变量要以值拷贝的形式传入
scan_matcher_task->SetWorkItem(
[=, &submap_scan_matcher, &scan_matcher_options]() {
cartographer::common::TicToc tic_toc;
tic_toc.Tic();
//构造3dFCS求解器,高低分辨率地图、地图内节点、匹配参数
submap_scan_matcher.fast_correlative_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
*submap_scan_matcher.high_resolution_hybrid_grid,
submap_scan_matcher.low_resolution_hybrid_grid, nodes_wiouout_id,
scan_matcher_options);
sum_t_cost_ += tic_toc.Toc();
//传入用的是local_submap_pose，换了个名字
submap_scan_matcher.global_submap_pose = global_submap_pose;
submap_scan_matcher.nodes_in_submap = submap_nodes;
//为子图计算特征，在submap_scanmatcher里增加新的任务
ExtractFeaturesForSubmap(submap_id);
});
submap_scan_matcher.creation_task_handle =
thread_pool_->Schedule(std::move(scan_matcher_task));
auto submap_constraints_task = common::make_unique<common::Task>();
submap_constraints_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraintsBetweenSubmaps(submap_id);
});
submap_constraints_task->AddDependency(
submap_scan_matcher.creation_task_handle);
auto submap_constraints_task_handle =
thread_pool_->Schedule(std::move(submap_constraints_task));
tasks_tracker_.push_back(submap_constraints_task_handle);
// finish_node_task_->AddDependency(submap_constraints_task_handle);
when_done_task_->AddDependency(submap_constraints_task_handle);
}构建submap_scan_matcher和原版差不太多，主要有两个函数起到重要作用，一是ExtractFeaturesForSubmap，为子图计算出一个特征，用来进行子图之间的互相匹配，二就是ComputeConstraintsBetweenSubmaps，函数如其名，进行子图之间的互相匹配。这里简单把注释过的代码贴一下，由于主要用的是opencv里面的二维图相关的匹配库，笔者没做过cv方面的工作看不太懂，于是只能简单梳理一下流程。
//为子图计算特征
void ConstraintBuilder3D::ExtractFeaturesForSubmap(
const SubmapId& submap_id){
cartographer::common::TicToc tic_toc;
tic_toc.Tic();
const double resolution = submap_scan_matchers_.at(
submap_id).high_resolution_hybrid_grid->resolution();
//如果没有栅格，就生成栅格
if(submap_scan_matchers_.at(submap_id).prj_grid.empty()){
// generate cv Mat for imcomming submap
//生成cvmat格式的栅格，存到对应位置
submap_scan_matchers_.at(submap_id).prj_grid = ProjectToCvMat(
submap_scan_matchers_.at(submap_id).high_resolution_hybrid_grid,
submap_scan_matchers_.at(submap_id).global_submap_pose,
submap_scan_matchers_.at(submap_id).ox,
submap_scan_matchers_.at(submap_id).oy,
submap_scan_matchers_.at(submap_id).resolution);
cv::Mat& grid = submap_scan_matchers_.at(submap_id).prj_grid;
cv::threshold(
grid, grid, options_.cv_binary_threshold(), 255, CV_THRESH_BINARY);
int se = options_.cv_structure_element_size();
auto ele = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(se, se));
cv::erode(grid, grid, ele);
surf_detector_->detectAndCompute(grid, cv::noArray(),
submap_scan_matchers_.at(submap_id).key_points,
submap_scan_matchers_.at(submap_id).descriptors);
// compute constraints between submaps.
//计算子图之间的约束
cv::Point2f tl(submap_scan_matchers_.at(submap_id).ox,
submap_scan_matchers_.at(submap_id).oy);
//std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
//遍历的是所有子图
for(const auto& entry: submap_scan_matchers_){
const SubmapId& id = entry.first;
const SubmapScanMatcher& scan_matcher = entry.second;
//不和自己匹配
if(id == submap_id) continue;
//没点云，不匹配
if(scan_matcher.prj_grid.empty()) continue;
// skip adjacent submaps for they are sharing most of scans and
// intra-constraints have been added.
//离得太近，不匹配
if(submap_id.trajectory_id == id.trajectory_id
&& std::abs(submap_id.submap_index - id.submap_index) <= 2){
continue;
}
//knn_match，看不懂捏
std::vector< std::vector<cv::DMatch> > knn_matches;
if(submap_scan_matchers_.at(submap_id).key_points.size() >= 2
&& submap_scan_matchers_.at(id).key_points.size() >= 2){
surf_matcher_->knnMatch(
submap_scan_matchers_.at(submap_id).descriptors,
submap_scan_matchers_.at(id).descriptors, knn_matches, 2);
}else{
continue;
}
//判断match是否满足要求，是的话加进good_matches
const double r = options_.good_match_ratio_of_distance();
std::vector<cv::DMatch> good_matches = {};
for (size_t i = 0; i < knn_matches.size(); i++){
if (knn_matches[i][0].distance < r * knn_matches[i][1].distance){
good_matches.push_back(knn_matches[i][0]);
}
}
//两个子图之间good_matches的数量足够
if(good_matches.size() >= options_.minimum_good_match_num()){
std::vector<cv::Point2f> from_pts={};
std::vector<cv::Point2f> to_pts={};
for(int i = 0; i < good_matches.size(); ++i){
auto gmt = good_matches[i];
cv::Point2f tl_to(submap_scan_matchers_.at(id).ox,
submap_scan_matchers_.at(id).oy);
from_pts.push_back(tl + submap_scan_matchers_.at(
submap_id).key_points.at(gmt.queryIdx).pt * resolution);
to_pts.push_back(tl_to + submap_scan_matchers_.at(
id).key_points.at(gmt.trainIdx).pt * resolution);
}
std::vector<uchar> inliers;
//RANSAC 去除离群点的算法
cv::Mat transform = cv::estimateAffinePartial2D(
from_pts, to_pts, inliers,
cv::RANSAC, options_.ransac_thresh_of_2d_transform_estimate());
if(!transform.empty()){
double scale = std::sqrt(
transform.at<double>(0, 0) * transform.at<double>(0, 0)
+ transform.at<double>(0, 1) * transform.at<double>(0, 1));
if(std::abs(scale - 1) > options_.scale_estimated_tolerance()) {
continue; //no constraint exists
}else{
double theta = std::atan2(
transform.at<double>(1,0), transform.at<double>(1,1));
transform::Rigid2d::Vector pos;
transform::Rigid2d::Rotation2D rot(theta);
pos << transform.at<double>(0,2), transform.at<double>(1,2);
transform::Rigid2d rt2(pos, rot);
// insert submap-to-submap constraint
//增加一个子图之间的相互约束
submap_scan_matchers_.at(submap_id).matched_submaps.insert(
{id, transform::Embed3D(rt2)});
}
}
}
}
}
sum_t_cost_ += tic_toc.Toc();
}主要用到knn匹配算法
//根据高分辨率点云生成栅格
cv::Mat ProjectToCvMat(const HybridGrid* hybrid_grid,
const transform::Rigid3d& transform,
double& ox, double& oy, double& resolution){
resolution = hybrid_grid->resolution();
transform::Rigid3f gravity_aligned = transform::Rigid3d::Rotation(
transform.rotation()).cast<float>();
// 去除全局yaw角影响
double yaw = transform::GetYaw(transform);
auto inv_yaw_rot = transform::Embed3D(transform::Rigid2d::Rotation(-yaw));
gravity_aligned = inv_yaw_rot.cast<float>() * gravity_aligned;
// Compute a bounding box for the texture.
Eigen::Array3i min_index(INT_MAX, INT_MAX, INT_MAX);
Eigen::Array3i max_index(INT_MIN, INT_MIN, INT_MIN);
// TODO(wz): redundant codes
std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
const float resolution_inverse = 1.f / resolution;
constexpr float kXrayObstructedCellProbabilityLimit = 0.501f;
//取栅格中的所有元素
for (auto it = HybridGrid::Iterator(*hybrid_grid);
!it.Done(); it.Next()) {
//概率值
const uint16 probability_value = it.GetValue();
const float probability = ValueToProbability(probability_value);
//太小就不要了
if (probability < kXrayObstructedCellProbabilityLimit) {
// We ignore non-obstructed cells.
continue;
}
// transform to gravity aligned
//子图坐标系下的概率格子的中心坐标，去除重力影响
const Eigen::Vector3f cell_center_submap =
hybrid_grid->GetCenterOfCell(it.GetCellIndex());
const Eigen::Vector3f cell_center_aligned
= gravity_aligned * cell_center_submap;
//格子的坐标，以及概率值
const Eigen::Array4i voxel_index_and_probability(
common::RoundToInt(cell_center_aligned.x() * resolution_inverse),
common::RoundToInt(cell_center_aligned.y() * resolution_inverse),
common::RoundToInt(cell_center_aligned.z() * resolution_inverse),
probability_value);
voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
const Eigen::Array3i pixel_index = voxel_index_and_probability.head<3>();
min_index = min_index.cwiseMin(pixel_index);
max_index = max_index.cwiseMax(pixel_index);
}
//
//原点
ox = min_index.x() * resolution;
oy = min_index.y() * resolution;
const int width = max_index.x() - min_index.x() + 1;
const int height = max_index.y() - min_index.y() + 1;
cv::Mat result = cv::Mat(height, width, CV_8UC1, cv::Scalar(255));
// Update grid's probabilities
std::vector<PixelData> accumulated_pixel_data(width * height);
for (const Eigen::Array4i& voxel_index_and_probability :
voxel_indices_and_probabilities) {
//x和y
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
if ((pixel_index < min_index.head<2>()).any()
|| (pixel_index > max_index.head<2>()).any()) {
// Out of bounds. This could happen because of floating point inaccuracy.
continue;
}
//转化到新坐标系下面
// comforming to cv mat's convention
const int x = pixel_index[0] - min_index.x();
const int y = pixel_index[1] - min_index.y();
PixelData& pixel = accumulated_pixel_data[y * width + x];
++pixel.count;
pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
const float probability =
ValueToProbability(voxel_index_and_probability[3]);
pixel.probability_sum += probability;
pixel.max_probability = std::max(pixel.max_probability, probability);
}
for(int k = 0; k < accumulated_pixel_data.size(); ++k){
int ix = k % width;
int iy = k / width;
auto probability = accumulated_pixel_data.at(k).probability_sum;
int cell_value = common::RoundToInt((probability - kMinProbability)
* (255.f / (kMaxProbability - kMinProbability)));
result.at<uchar>(iy, ix) = cell_value;
}
return result;
}根据高分辨率点云生成cv栅格的过程也简单贴一下。
在submap_constraints_task里面新增加约束后，后面的优化应该就和carto本体差不太多了。
四、总结
简单理了一下Carto原版和D-LIOM的思路，用作学习笔记用，如有冒犯请告知，欢迎交流指点。
581
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
