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.全局搜索窗与局部搜索窗的区别 局部搜索窗有距离范围限制

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

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

相关文章

2023年一建学霸笔记

2023年一建学霸笔记

考点:单方取消或辞去委托承担的民事责任女《民法典》规定&#xff0c;因解除合同造成对方损失的&#xff0c;除不可归责于该当事人的事由外&#xff0c;无偿委托合同的解除方应当赔偿因解除时间不当造成的直接损失&#xff0c;有偿委托合同的解除方应当赔偿对方的直接损失和合同…
阅读更多...
简单理解TCP,UDP,HTTP

简单理解TCP,UDP,HTTP

我们都知道TCP、UDP、HTTP内部有很复杂的过程&#xff0c;很多人没办法理解的那么深&#xff0c;只想知道这是个什么鬼。 1、TCP、UDP、HTTP 是什么? TCP/IP是个协议组&#xff0c;可分为三个层次&#xff1a;网络层、传输层和应用层。在网络层有IP协议、ICMP协议、ARP协议、…
阅读更多...
关于云服务器ECS、宝塔的安装配置以及图床的使用

关于云服务器ECS、宝塔的安装配置以及图床的使用

一、阿里云服务器的申请以及宝塔的安装 安装配置服务器的原理&#xff1a; step1&#xff1a;地址栏输入阿里云服务器官网地址 step2&#xff1a;在首页依次点击以下内容&#xff1a; step3&#xff1a;选择立即购买&#xff0c;并填写以下内容&#xff1a; step4&#xff1a…
阅读更多...
Postman和Jmeter做接口测试的区别

Postman和Jmeter做接口测试的区别

1. 用例组织方式 Jmeter的组织方式相对比较扁平&#xff0c;它首先没有WorkSpace的概念&#xff0c;直接是TestPlan&#xff0c;TestPlan下创建的Threads Group就相当于TestCase&#xff0c;并没有TestSuite的层级。 Postman功能上更简单&#xff0c;组织方式也更轻量级&#…
阅读更多...
[SQL挖掘机] - 子查询介绍

[SQL挖掘机] - 子查询介绍

介绍: 子查询&#xff08;Subquery&#xff09;&#xff0c;也被称为嵌套查询或内部查询&#xff0c;是指在一个查询语句中嵌套使用的查询。它是将一个查询语句作为另一个查询语句的一部分来构建更复杂的查询逻辑。 子查询通常出现在主查询的条件、选择列表或 FROM 子句中&am…
阅读更多...
opencv 之 外接多边形（矩形、圆、三角形、椭圆、多边形）使用详解

opencv 之 外接多边形（矩形、圆、三角形、椭圆、多边形）使用详解

opencv 之 外接多边形&#xff08;矩形、圆、三角形、椭圆、多边形&#xff09;使用详解 本文主要讲述opencv中的外接多边形的使用&#xff1a; 多边形近似外接矩形、最小外接矩形最小外接圆外接三角形椭圆拟合凸包 将重点讲述最小外接矩形的使用 1. API介绍 #多边形近似 v…
阅读更多...
Redisson实现简单消息队列：优雅解决缓存清理冲突

Redisson实现简单消息队列：优雅解决缓存清理冲突

在项目中&#xff0c;缓存是提高应用性能和响应速度的关键手段之一。然而&#xff0c;当多个模块在短时间内发布工单并且需要清理同一个接口的缓存时&#xff0c;容易引发缓存清理冲突&#xff0c;导致缓存失效的问题。为了解决这一难题&#xff0c;我们采用Redisson的消息队列…
阅读更多...
带你体验stable discussion文生图，实现自己的真人写真工具

带你体验stable discussion文生图，实现自己的真人写真工具

前言 Midjourney 由于精致的画图风格备受好评&#xff0c;但由于其网络环境以及会员费&#xff0c;导致入门门槛过高&#xff0c;拦住了很多对AIGC感兴趣的小伙伴。今天带大家体验一下最近很火的Stable Diffusion&#xff0c;满足大家的AI爱好,无需科学上网&#xff0c;本地部…
阅读更多...
数据分析系统中的六边形战士——奥威BI系统

数据分析系统中的六边形战士——奥威BI系统

数据分析软件可以对收集的数据进行分析和报告&#xff0c;帮助企业获得更深入的数据洞察力&#xff0c;从而推动企业数字化运营决策&#xff0c;提高决策效率与质量。进入大数据时代&#xff0c;企业对数据分析软件的要求也在水涨船高&#xff0c;传统的数据分析软件显然已不能…
阅读更多...
Git 学习笔记

Git 学习笔记

Git 仓库中的提交记录保存的是你的目录下所有文件的快照&#xff0c;就像是把整个目录复制&#xff0c;然后再粘贴一样&#xff0c;但比复制粘贴优雅许多&#xff01; Git 希望提交记录尽可能地轻量&#xff0c;因此在你每次进行提交时&#xff0c;它并不会盲目地复制整个目录。…
阅读更多...
华为盘古大模型：能源领域的颠覆性突破

华为盘古大模型：能源领域的颠覆性突破

近日&#xff0c;华为盘古大模型在能源领域横空出世&#xff0c;引发了广泛关注和期待。作为一项具有颠覆性影响的技术创新&#xff0c;华为盘古大模型在能源行业中展现出巨大的潜力和前景。其优质的计算能力和智能优化算法&#xff0c;将为能源产业带来翻天覆地的变革。 盘古大…
阅读更多...
一篇文章搞定数据同步工具SeaTunnel

一篇文章搞定数据同步工具SeaTunnel

文章目录 前言第 1 章 Seatunnel 概述第 2 章 Seatunnel 安装和使用第 3 章 SeaTunnel 基本原理第 4章应用案例后记 前言 SeaTunnel安装包及代码&#xff1a; 链接: https://pan.baidu.com/s/1JvgAZpqoOPJ0ecfxUbLo4Q 提取码: pur8 –来自百度网盘超级会员v4的分享 第 1 章 …
阅读更多...
成为一名数字IC后端工程师需要掌握哪些技能？（内附学习视频）

成为一名数字IC后端工程师需要掌握哪些技能？（内附学习视频）

众所周知&#xff0c;数字后端设计是IC设计中必不可少的一个环节&#xff0c;数字后端工程师是将门级网表转换成标准的GDS文件&#xff0c;又称为设计实现或物理设计。正所谓前端保证功能正确&#xff0c;后端保证芯片的实现正确。 数字后端工程师是做什么的&#xff1f; 数字…
阅读更多...
多线程（JavaEE初阶系列3）

多线程（JavaEE初阶系列3）

目录 前言&#xff1a; 1.中断一个线程 2.等待一个线程-join() 2.1join()无参调用的使用 2.2join()有参调用的使用 3.线程的状态 3.1观察线程的所有状态 4.多线程带来的风险—线程安全 4.1观察线程不安全 4.2该问题出现的原因 4.3线程不安全问题的解决 4.3.1synchro…
阅读更多...
解决JMeter+Grafana+influxdb 配置出现transaction无数据情形

解决JMeter+Grafana+influxdb 配置出现transaction无数据情形

问题描述 JMeterGrafanainfluxdb 配置时&#xff0c;Darren洋发现jmeter中明明已经配置好了事务条件以及接口实例信息&#xff0c;但就是在grafana的头部导航栏中的transaction按钮下来没有相应事务数据信息&#xff0c;经过相关资料查询&#xff0c;Darren洋发现执行以下两个步…
阅读更多...
【嵌入式开发 Linux 常用命令系列 7 -- awk 常用方法】

【嵌入式开发 Linux 常用命令系列 7 -- awk 常用方法】

文章目录 AWK 功能介绍awk 与 $ 关系awk 与其他命令组合使用awk 常用命令参数 上篇文章&#xff1a;嵌入式开发 Linux 常用命令系列 6 – 字符提取 cut 命令使用 下篇文章&#xff1a;嵌入式开发 Linux 常用命令系列 8 – 二进制转为16进制常用命令 AWK 功能介绍 AWK是一种强大…
阅读更多...
自然语言处理从入门到应用——LangChain：模型（Models）-[基础知识]

自然语言处理从入门到应用——LangChain：模型（Models）-[基础知识]

分类目录&#xff1a;《自然语言处理从入门到应用》总目录 《自然语言处理从入门到应用——LangChain&#xff1a;模型&#xff08;Models&#xff09;》部分的文章介绍了LangChain中使用的不同类型的模型。在本文中&#xff0c;我们将阐述模型&#xff08;Models&#xff09;类…
阅读更多...
LangChain Agents深入剖析及源码解密上（三）

LangChain Agents深入剖析及源码解密上（三）

AutoGPT案例V1版本 AutoGPT是一个实验性的开源应用程序，展示了GPT-4语言模型的功能，AutoGPT程序由GPT-4驱动，将大语言模型的思考链接在一起，以自主实现设定的任何目标。作为GPT-4完全自主运行的首批例子之一，AutoGPT突破了人工智能的可能性。LangChain框架复现了https://g…
阅读更多...
c#栈应用——实现四则运算

c#栈应用——实现四则运算

前言&#xff1a;四则运算&#xff0c;大家都不陌生&#xff0c;在上小学的时候&#xff0c;数学中学到过的知识&#xff0c;那么如何在程序中实现呢&#xff1f;下面&#xff0c;我们就用程序来实现9(3-2)*(5-3)/4*3&#xff0c;这个算式的值。计算的时候&#xff0c;有一个规…
阅读更多...
MySQL学习笔记 ------ 分页查询

MySQL学习笔记 ------ 分页查询

#进阶8&#xff1a;分页查询 ★ /* 应用场景&#xff1a;当要显示的数据&#xff0c;一页显示不全&#xff0c;需要分页提交sql请求 语法&#xff1a; select 查询列表 from 表 【join type join 表2 on 连接条件 where 筛选条件 group by 分组字段 …
阅读更多...
最新文章

Copyright @ 2022~2023