网站栅格,wordpress好123,郑州网站建设douyanet,别人的做网站1.cartographer里的节点:当扫描匹配结束后#xff0c;有新的一帧scan加入到submap中#xff0c;这个扫描匹配的结果就叫做节点 global_trajectory_builder.cc // 将匹配后的结果 当做节点 加入到位姿图中auto node_id pose_graph_-AddNode(matching_result-insertio…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 for3.回环检测当前的节点与所有已经完成的子图进行约束的计算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 nodes trajectory to// the submaps 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_castconst 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_uniquecommon::Task();// 生成一个将初始化匹配器的任务, 初始化时会计算多分辨率地图, 比较耗时scan_matcher_task-SetWorkItem([submap_scan_matcher, scan_matcher_options]() {// 进行匹配器的初始化, 与多分辨率地图的创建submap_scan_matcher.fast_correlative_scan_matcher absl::make_uniquescan_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_ptrConstraintBuilder2D::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)) {// Weve 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) // c11: 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.全局搜索窗与局部搜索窗的区别 局部搜索窗有距离范围限制
