Cartographer回环中的关键一步,就是要计算节点node(scan)与子图submap之间的约束,此过程在 PoseGraph2D::ComputeConstraint 中实现.
PoseGraph2D::ComputeConstraint
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) {
//首先确认该submap已经finish
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const common::Time node_time = GetLatestNodeTime(node_id, submap_id);//获得该node时间和已和submap匹配的最晚node时间的其中更晚者
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.只需将匹配限制在本地搜索窗口即可
const transform::Rigid2d initial_relative_pose =
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d;//node相对于submap的位姿
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
initial_relative_pose);
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get());
}
}
在计算node和submap的约束的该函数中,首先确认该submap已经finish了;
之后判定如果节点和子图属于同一轨迹,或者 最近一段时间 有全局约束将该节点的轨迹与子图的轨迹联系起来,那么只需要将匹配限制在本地局部搜索窗口即可(MaybeAddConstraint).
如果有 全局定位采样脉冲 的话,则进行全域上的约束匹配(MaybeAddGlobalConstraint)
约束结果放入PoseGraph2D的成员变量constraint_builder_的成员变量constraints_里.
MaybeAddConstraint
ConstraintBuilder2D::MaybeAddConstraint: 在本地局部local窗口计算node和submap的约束.
void ConstraintBuilder2D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {//如果初始相对位姿的范数大于规定的最大范数则跳过
return;
}
if (!sampler_.Pulse()) return;//如果此脉冲应产生采样，则返回true,此处尚不明朗
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
}
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
//对SubmapScanMatcher*类型的scan_matcher求解器进行初始化
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
//调用ComputeConstraint进行位姿优化
auto constraint_task = common::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_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);
}
在MaybeAddConstraint中,首先判定node关于submap初始相对位姿的范数是否过大,如果超过一定限制则跳过该次约束计算;
在计算scan-match时首先调用了DispatchScanMatcherConstruction进行对 ConstraintBuilder2D::SubmapScanMatcher* 类型的scan_matcher求解器进行初始化;
之后构建任务task,task使用 ConstraintBuilder2D::ComputeConstraint 进行位姿优化,将优化的位姿等信息存入最后一个参数Constraint中;
最后将任务task压入线程池.
链接: PoseGraph2D::ComputeConstraint
ConstraintBuilder2D::ComputeConstraint
在此处这个ComputeConstraint中具体对node与submap的约束进行计算.将计算约束的任务压入线程池.
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;//获得node的全局pose
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
// 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) {//搜索窗口1e6 角度双侧180
kGlobalConstraintsSearchedMetric->Increment();//什么也不做?
if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
} else {
return;
}
} else {//按设定参数搜索 pose_graph.lua 7 30度
kConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(score);
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(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::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher.grid, &pose_estimate,
&unused_summary);
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(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->filtered_gravity_aligned_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {
const transform::Rigid2d difference =
initial_pose.inverse() * pose_estimate;
info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision(1) << 100. * score << "%.";
LOG(INFO) << info.str();
}
}
在ComputeConstraint计算约束时,首先判断是局部搜索还是全局搜索.
- 如果是全局搜索(match_full_submap=true),那我们的搜索窗口尺度为1e6米 ,角度为双侧180度.
通过 FastCorrelativeScanMatcher2D::MatchFullSubmap 对约束进行大窗口全域搜索(106,双边180),获得最佳匹配相对位姿(约束),返回本次得分是否高于历史,是则返回true,否则返回false. - 如果不是全局搜索
通过Match对约束进行指定参数的区域搜索,参数设定在pose_graph.lua中,默认为7米 30度.之后获得最佳匹配相对位姿(约束),返回本次得分是否高于历史,是则返回true,否则返回false.
之后锁住线程,把上面分支定界获得的最大分数加入分数直方图里.
把当前最优的候选放入ceres进行非线性优化,获得进一步最优的位姿变换(约束);
最后存入最后一个形参constraint中.
MatchFullSubmap
对约束进行大窗口全域搜索(106,双边180),获得最佳匹配相对位姿(约束),返回本次得分是否高于历史,是则返回true,否则返回false.
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// Compute a search window around the center of the submap that includes it
// fully.
const SearchParameters search_parameters(
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution());
const transform::Rigid2d center = transform::Rigid2d::Translation(
limits_.max() - 0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));//中心点的实际坐标
return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate);
}
首先设置线性搜索窗口和角度搜索窗口:全域搜索,此处线性搜索窗口为106米,角度搜索窗口为双边各180度;
获得地图坐标系中心点在现实中的实际坐标;
之后代入 MatchWithSearchParameters ,根据搜索窗口参数,获得最佳匹配相对位姿(约束),存入最后一个形参pose_estimate中.
返回本次得分是否高于历史,是则返回true,否则返回false.
链接: ConstraintBuilder2D::ComputeConstraint
MatchWithSearchParameters
根据搜索窗口参数,获得最佳匹配相对位姿(约束),存入最后一个形参pose_estimate中.
如果本次匹配得分高于上次,则更新pose_estimate,返回true;
否则返回false.
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 {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
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())));//将点云按初始旋转旋转到初始位置
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);//依照步长和搜索角度个数,依次旋转生成一系列待匹配点云
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));//将一系列待匹配点云平移到初始位置并各点放入地图坐标系
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());//ShrinkToFit?很迷的尽可能缩小的搜索框
//获得分数从大到小排列的像素偏移集
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);//discrete_scans是一系列平移到初始位置放入地图坐标系的待匹配点云
const Candidate2D best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
*score = best_candidate.score;
*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 true;
}
return false;
}
首先将待匹配点云按照初始位姿变换的旋转旋转到初始位置.
之后依照前面设定的搜索角度大小和旋转步长,依次旋转生成一系列待匹配点云.
然后将这一系列待匹配点云按照初始位姿变换的平移平移到初始位置并将各点都放入地图像素坐标系中,通过ShrinkToFit尽可能缩小搜索框大小.
再然后通过 ComputeLowestResolutionCandidates 在最低分辨率下对搜索框内的各个像素偏移计算对应分数,并按分数从大到小将这些像素偏移进行排列.
最后 BranchAndBound 在各个分辨率上优化相对位姿,获得得分最高的候选像素偏移.
如果本次计算后得分高于上一次计算,则记录下此次计算出的优化位姿,返回true,否则返回false.
链接: MatchFullSubmap
ComputeLowestResolutionCandidates
对一系列等步长旋转的点云逐个在各个像素偏移上分别计算得分,最后按从小到大排列在lowest_resolution_candidates中.
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const {
std::vector<Candidate2D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters);//生成候选的最低分辨率像素偏移集
// 计算每个Candidates的得分 根据传入的地图在这个地图上进行搜索来计算得分
// 按照匹配得分 从大到小 排序，返回排列好的candidates
// 第一个参数是 vector中最后一个PrecomputationGrid2D
ScoreCandidates(
precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
discrete_scans, search_parameters, &lowest_resolution_candidates);
return lowest_resolution_candidates;//返回 分数从大到小排列的像素偏移集
}
输入一系列的经过旋转和平移的点云集.
首先通过 GenerateLowestResolutionCandidates 来生成在最低分辨率下候选计算的像素偏移集.
之后通过 ScoreCandidates 计算每个候选Candidate的得分,并按照匹配得分从大到小排序
最后返回分数从大到小排列的像素偏移集.
链接: MatchWithSearchParameters
GenerateLowestResolutionCandidates
生成候选的最低分辨率像素偏移集,输出为vector candidates(该struct中包含:旋转scan的id,x方向偏移最细像素数,y向偏移最细像素数,param参数)
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();//左移放大,顶层分辨率
int num_candidates = 0;
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
const int num_lowest_resolution_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
linear_step_size;//最低分辨率下x方向候选像素数
const int num_lowest_resolution_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
linear_step_size;//y方向同上
num_candidates += num_lowest_resolution_linear_x_candidates *
num_lowest_resolution_linear_y_candidates;//所有点云总候选像素数
}
std::vector<Candidate2D> candidates;
candidates.reserve(num_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 += linear_step_size) {
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 += linear_step_size) {
candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
search_parameters);//(旋转scan的id,x方向偏移最细像素数,y向,param)候选的最低分辨率像素集
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
首先通过左移放大,获得最粗分辨率的大小.
之后对一组间隔步长的旋转点云中的每一个点云,获得在最粗分辨率下窗口内候选像素偏移的候选数,累加获得总的候选数.
最后把各个候选偏移像素的具体信息依次存入容器candidates中并返回该容器.
链接: ComputeLowestResolutionCandidates
ScoreCandidates
该函数计算每个Candidates的得分 根据传入的地图在这个地图上进行搜索来计算得分,按照匹配得分从大到小排序，获得排列好的candidates候选偏移像素集.
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans/*一系列经旋转平移并放入最细地图中的scan*/,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates/*候选的(最低分辨率)像素偏移集*/) const {
for (Candidate2D& candidate : *candidates) {
int sum = 0;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {//对该点云的每一个点
const Eigen::Array2i proposed_xy_index(
xy_index.x() + candidate.x_index_offset,
xy_index.y() + candidate.y_index_offset);//按偏移值偏移
sum += precomputation_grid.GetValue(proposed_xy_index);//获取在cells_中存储的对应score(0-255),sum为所有点数分数和
}
candidate.score = precomputation_grid.ToScore(
sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));//每个点平均分数映射到[min_score,max_score]
}
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate2D>());//>被重定义,按score比较大小,从大到小排列候选的(最低分辨率)像素偏移集
}
在每个候选偏移情况下,首先对每个候选情况下点云的每个点按偏移值进行偏移,
之后通过GetValue获得cell_中预先存储的在各个位置的得分,
然后把每个点的平均分数映射到[min_score,max_score]上
最后按score比较大小,从大到小排列传入的候选像素偏移集(std::vector* const candidates)
链接: ComputeLowestResolutionCandidates
BranchAndBound
在分支定界中,深度优先地递归地查找得分最高的候选像素偏移,最终返回得分最高的像素偏移.
//递归地分支定界
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans/*一系列旋转不同角度的放入最细分辨率地图的scan*/,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates/*得分从大到小的候选偏移*/, const int candidate_depth/*预计算网格集末index,也是层数*/,
float min_score) const {
//迭代终止条件
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();//直接返回得分最大的,递归结束条件
}
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);//(scan_id,x偏移,y偏移,param)
best_high_resolution_candidate.score = min_score;
for (const Candidate2D& candidate : candidates) {
if (candidate.score <= min_score) {
break;
}
std::vector<Candidate2D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);//最低分辨率边长的一半
//下面两个for循环构建了更细一层分辨率的候选集
for (int x_offset : {0, half_width}) {
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {0, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);//获得了高一级分辨率的候选偏移集
}
}
//对该级分辨率下的候选集进行分数计算并从大到小排列,结果存入最后形参处
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
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;
}
分支定界.
对一层分辨率上的每一个候选,进行分辨率重新划分;
之后通过 ScoreCandidates 对该级分辨率下的候选集进行分数计算并从大到小排列;
记录下分数最高的候选,并递归更细分辨率;
递归完成后返回最高得分的候选.
链接: MatchWithSearchParameters
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
