slam建图与定位_cartographer代码阅读(7)后端约束构建

1.cartographer里的节点:当扫描匹配结束后，有新的一帧scan加入到submap中，这个扫描匹配的结果就叫做节点
global_trajectory_builder.cc

     // 将匹配后的结果 当做节点 加入到位姿图中auto node_id = pose_graph_->AddNode(matching_result->insertion_result->constant_data, trajectory_id_,matching_result->insertion_result->insertion_submaps);

2.子图内约束，子图的原点坐标与节点间的约束pose_graph_2d.cc

	// 遍历2个子图, 将节点加入子图的节点列表中, 计算子图原点与及节点间的约束(子图内约束)for (size_t i = 0; i < insertion_submaps.size(); ++i) {const SubmapId submap_id = submap_ids[i];// Even if this was the last node added to 'submap_id', the submap will// only be marked as finished in 'data_.submap_data' further below.CHECK(data_.submap_data.at(submap_id).state ==SubmapState::kNoConstraintSearch);// 将node_id放到子图保存的node_ids的set中data_.submap_data.at(submap_id).node_ids.emplace(node_id);// 计算 子图原点 指向 node坐标 间的坐标变换(子图内约束)const transform::Rigid2d constraint_transform =constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *local_pose_2d;// 新生成的 子图内约束 放入容器中data_.constraints.push_back(Constraint{submap_id,node_id,{transform::Embed3D(constraint_transform),options_.matcher_translation_weight(),options_.matcher_rotation_weight()},Constraint::INTRA_SUBMAP}); // 子图内约束} // end for

3.回环检测：当前的节点与所有已经完成的子图进行约束的计算pose_graph_2d.cc

  // Step: 当前节点与所有已经完成的子图进行约束的计算---实际上就是回环检测for (const auto& submap_id : finished_submap_ids) {// 计算旧的submap和新的节点间的约束ComputeConstraint(node_id, submap_id);}

4.回环检测（子图间约束）：计算所有节点与刚完成子图的约束pose_graph_2d.cc

	  // Step: 计算所有节点与刚完成子图间的约束---实际上就是回环检测if (newly_finished_submap) {const SubmapId newly_finished_submap_id = submap_ids.front();// We have a new completed submap, so we look into adding constraints for// old nodes.for (const auto& node_id_data : optimization_problem_->node_data()) {const NodeId& node_id = node_id_data.id;// 刚结束的子图内部的节点, 不再与这个子图进行约束的计算if (newly_finished_submap_node_ids.count(node_id) == 0) {// 计算新的submap和旧的节点间的约束ComputeConstraint(node_id, newly_finished_submap_id);}}}

5.全局搜索和局部搜索pose_graph_2d.cc
节点和子图时间差小于阈值或者是同一条轨迹 进行局部搜索；
节点和子图时间间隔间隔了一段时间间隔并且不属于同一条轨迹 全局搜索 纯定位模式；

		/*** @brief 进行子图间约束计算, 也可以说成是回环检测* * @param[in] node_id 节点的id* @param[in] submap_id submap的id*/
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,const SubmapId& submap_id) {bool maybe_add_local_constraint = false;bool maybe_add_global_constraint = false;const TrajectoryNode::Data* constant_data;const Submap2D* submap;{absl::MutexLock locker(&mutex_);CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);// 如果是未完成状态的地图不进行约束计算if (!data_.submap_data.at(submap_id).submap->insertion_finished()) {// Uplink server only receives grids when they are finished, so skip// constraint search before that.return;}// 获取该 node 和该 submap 中的 node 中较新的时间const common::Time node_time = GetLatestNodeTime(node_id, submap_id);// 两个轨迹的最后连接时间const common::Time last_connection_time =data_.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.maybe_add_local_constraint = true;
}
// 如果节点与子图不属于同一条轨迹 并且 间隔了一段时间, 同时采样器为true
// 才进行 全局搜索窗口 的约束计算(对整体子图进行回环检测)
else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {maybe_add_global_constraint = true;
}// 获取节点信息数据与地图数据
constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get());} // end {}

6.创建多分辨率地图constraint_builder_2d.cc

	// 为每个子图新建一个匹配器
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,const Grid2D* const grid) {CHECK(grid);// 如果匹配器里已经存在, 则直接返回对应id的匹配器if (submap_scan_matchers_.count(submap_id) != 0) {return &submap_scan_matchers_.at(submap_id);}// submap_scan_matchers_新增加一个 keyauto& submap_scan_matcher = submap_scan_matchers_[submap_id];kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());// 保存栅格地图的指针submap_scan_matcher.grid = grid;auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();auto scan_matcher_task = absl::make_unique<common::Task>();// 生成一个将初始化匹配器的任务, 初始化时会计算多分辨率地图, 比较耗时scan_matcher_task->SetWorkItem([&submap_scan_matcher, &scan_matcher_options]() {// 进行匹配器的初始化, 与多分辨率地图的创建submap_scan_matcher.fast_correlative_scan_matcher =absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(*submap_scan_matcher.grid, scan_matcher_options);});// 将初始化匹配器的任务放入线程池中, 并且将任务的智能指针保存起来submap_scan_matcher.creation_task_handle =thread_pool_->Schedule(std::move(scan_matcher_task));return &submap_scan_matchers_.at(submap_id);
}

7.基于分支定界进行粗匹配，使用ceres进行精匹配constraint_builder_2d.cc

/*** @brief 计算节点和子图之间的一个约束(回环检测)*        用基于分支定界算法的匹配器进行粗匹配,然后用ceres进行精匹配* * @param[in] submap_id submap的id* @param[in] submap 地图数据* @param[in] node_id 节点id* @param[in] match_full_submap 是局部匹配还是全子图匹配* @param[in] constant_data 节点数据* @param[in] initial_relative_pose 约束的初值* @param[in] submap_scan_matcher 匹配器* @param[out] constraint 计算出的约束*/
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) {CHECK(submap_scan_matcher.fast_correlative_scan_matcher);// Step:1 得到节点在local frame下的坐标const transform::Rigid2d initial_pose =ComputeSubmapPose(*submap) * initial_relative_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.// param: global_localization_min_score 对整体子图进行回环检测时的最低分数阈值// param: min_score 对局部子图进行回环检测时的最低分数阈值// Step:2 使用基于分支定界算法的匹配器进行粗匹配if (match_full_submap) {// 节点与全地图进行匹配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 {// 节点与局部地图进行匹配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;}}{absl::MutexLock 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.// Step:3 使用ceres进行精匹配, 就是前端扫描匹配使用的函数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);// Step:4 获取节点到submap坐标系原点间的坐标变换// pose_estimate 是 节点在 loacl frame 下的坐标const transform::Rigid2d constraint_transform =ComputeSubmapPose(*submap).inverse() * pose_estimate;// Step:5 返回计算后的约束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});// log相关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) // c++11: std::setprecision(2) 保留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();}
}

总结：
1.子图内约束包含：当前节点与当前子图原点的约束和当前节点和所有已经完成子图的约束，目的是为了构建局部地图
2.子图间的约束：当前完成子图与所有节点的约束 ,目的是为了构建全局地图
3.全局搜索窗与局部搜索窗的区别 局部搜索窗有距离范围限制