(02)Cartographer源码无死角解析-(74) 2D后端优化→OptimizationProblem2D-里程计、local位姿、GPU残差

news/2024/4/16 3:46:06

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX官方认证
 

一、前言

上一篇博客介绍的 landmark 数据的残差与优化,最后本人分享了如下:

核心理解 : \color{red}核心理解: 核心理解: 在上一篇博客中介绍的约束残差(节点相对与子图位姿的残差),其主是优化每个节点以及每个子图的位姿。可想而知,其是离散的。但是轨迹是一条连续平滑的曲线,所以本质上我们希望通过插值获取到的位姿也是准确的。恰好, landmark帧的时间点,也激光雷达数据帧的时间点是不一样的,所以如果 landmark帧的时间点位于两个激光雷达数据帧的时间点,那么可以把landmark帧相对于机器人的位姿作为一个约束,对两节点插值之后的位姿进行优化,从而广播到两个节点,间接对两个节点进行优化。

现在回到 OptimizationProblem2D::Solve() 函数,到目前为止,已经把:

  // Add cost functions for landmarks.// Step: landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,&problem, options_.huber_scale());

讲解完成了,不过看起来,任务量还是很多的,似乎还剩下很多代码都没有讲解。起始不然,因为后面的代码比较简单,另外再前面已经接触过类似的代码,分析起来就没有那么困难。

二、残差准备

接着代码 OptimizationProblem2D::Solve() 函数调用的 AddLandmarkCostFunctions() 之后继续分析。首先其又运行了一个for循环,且其中也嵌套一个层for循环,先来看第一层循环,代码如下:

  // 遍历多个轨迹, 添加里程计与local结果的残差for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {// 获取每个节点的轨迹idconst int trajectory_id = node_it->id.trajectory_id;// 获取这条轨迹的最后一个位置的迭代器const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);// 如果轨迹是frozen的, 则无需处理直接跳过if (frozen_trajectories.count(trajectory_id) != 0) {node_it = trajectory_end;continue;}auto prev_node_it = node_it;

需要注意,这里的for循环时对轨迹遍历,并不是对节点的遍历,如果 node_data_ 中包含了多条轨迹的节点,node_data_.begin() 表示 trajectory_id==0 的第一个节点,而 node_data_.end() 表示 trajectory_id 等于最大轨迹的第一个节点。先获得轨迹起始节点 node_it 对应轨迹标识 trajectory_id。然后获得这条轨迹结束位姿的迭代器,记录在 trajectory_end,接着判断一下目前节点对应的轨迹是否为冻结状态,如果为冻结状态则 continue 跳出第一层循环。然后把当前 node_it 记录给 prev_node_it,也就是 prev_node_it 记录的目前轨迹的起始节点。接着代码分析,可以看到其嵌套了一个循环:

    // 遍历一个轨迹的所有节点, 添加里程计与local结果的残差for (++node_it; node_it != trajectory_end; ++node_it) {const NodeId first_node_id = prev_node_it->id;const NodeSpec2D& first_node_data = prev_node_it->data;prev_node_it = node_it;const NodeId second_node_id = node_it->id;const NodeSpec2D& second_node_data = node_it->data;// 如果节点的索引不连续, 跳过if (second_node_id.node_index != first_node_id.node_index + 1) {continue;}

该循环就是对一条轨迹的节点进行遍历,prev_node_it 主要用于记录上一节点,然后判断当前节点与上一节点索引是否连续,如果不连续,则会 continue 跳出循环,该节点不参与下面的残差计算。

另外,其还获取了当前节点与上一节点的节点数据,分别存储于 first_node_data 与 second_node_data。

三、里程计插值

首先根据两个连续的节点数据 first_node_data 与 second_node_data 的时间,然后对里程计数据进行插值处理,获得里程计在这两个时间点的 local 系下的位姿,里程计数据的线性插值,被调代码如下:

      // Add a relative pose constraint based on the odometry (if available).// 根据里程计数据进行插值得到的2个节点间的坐标变换std::unique_ptr<transform::Rigid3d> relative_odometry =CalculateOdometryBetweenNodes(trajectory_id, first_node_data,second_node_data);

函数 CalculateOdometryBetweenNodes() 实现于 optimization_problem_2d.cc 文件中,代码注释如下:

/*** @brief 根据里程计数据算出两个节点间的相对坐标变换* * @param[in] trajectory_id 轨迹的id* @param[in] first_node_data 前一个节点数据* @param[in] second_node_data 后一个节点数据* @return std::unique_ptr<transform::Rigid3d> 两个节点的坐标变换*/
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(const int trajectory_id, const NodeSpec2D& first_node_data,const NodeSpec2D& second_node_data) const {if (odometry_data_.HasTrajectory(trajectory_id)) {// 插值得到time时刻的里程计数据const std::unique_ptr<transform::Rigid3d> first_node_odometry =InterpolateOdometry(trajectory_id, first_node_data.time);const std::unique_ptr<transform::Rigid3d> second_node_odometry =InterpolateOdometry(trajectory_id, second_node_data.time);if (first_node_odometry != nullptr && second_node_odometry != nullptr) {// 根据里程计数据算出的相对坐标变换// 需要注意的是, 实际上在optimization_problem中, node的位姿都是2d平面上的// 而odometry的pose是带姿态的, 因此要将轮速计插值出来的位姿转到平面上transform::Rigid3d relative_odometry =transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *first_node_odometry->inverse() * (*second_node_odometry) *transform::Rigid3d::Rotation(second_node_data.gravity_alignment.inverse());return absl::make_unique<transform::Rigid3d>(relative_odometry);}}return nullptr;
}

其代码也比较简单,就是先判断一下 odometry_data_ 中有没有 trajectory_id 对应的里程计数据,如果有就根据 first_node_data 与 second_node_data 获得需要插值求得位姿的时刻,然后进行插值。这里的插值函数 InterpolateOdometry() 其权重依旧由 time 与前后两个里程计节点的时间距离来决定的。求得 first_node_data.time 与 second_node_data.time 对应插值里程计位姿之后的 local 位姿之后,再进一步获得两节点基于里程计的相对位姿变换。

四、里程计残差

      // Step: 第三种残差 节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项// 如果存在里程计则可增加一个残差if (relative_odometry != nullptr) {problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{*relative_odometry, options_.odometry_translation_weight(),options_.odometry_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}

如果存在里程计则可增加一个残差,该残差以 relative_odometry 作为帧值(根据里程计计算出来两节点的位姿变换),然后让根据两节点global系下的节点位姿计算出两节点的相对位姿,优化过程就是让后者向前者靠近。

四、local残差

这是一种比较好理解的残差,再前端求得节点位姿时,使用了扫描匹配,以及Ceres优化,尽量让一帧点云全部打在障碍物上,很明显其时比较优的解,这里我们认为其时正确的,对于相邻的两个节点,都是通过这种方式估算出来的 local 位姿,因为间隔时间短,他们之间的累计误差时可以忽略的,也就是说, local 系下两个节点的相对变换基本同理基本时正确的,可以把其作为一个约束来对待,让 global 系下两个节点之间的位姿变换通过Ceres优化,逼近于local系下对应两个节点的位姿变换。代码如下所示:

      // Add a relative pose constraint based on consecutive local SLAM poses.// 计算相邻2个节点在local坐标系下的坐标变换const transform::Rigid3d relative_local_slam_pose =transform::Embed3D(first_node_data.local_pose_2d.inverse() *second_node_data.local_pose_2d);// Step: 第四种残差 节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换 的差值作为残差项problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{relative_local_slam_pose,options_.local_slam_pose_translation_weight(),options_.local_slam_pose_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}}

五、GPS残差-准备工作与约束计算

对于GPS残差部分,又是两个for循环进行嵌套的代码,且与前面十分相似如下,第一层for循环依旧是对轨迹的遍历。

 // 遍历多个轨迹, 添加gps的残差std::map<int, std::array<double, 3>> C_fixed_frames;for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {const int trajectory_id = node_it->id.trajectory_id;const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {node_it = trajectory_end;continue;}const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);bool fixed_frame_pose_initialized = false;

其先判断一下该轨迹是否存在GPU数据,没有没有,则 continue 遍历下一条轨迹。如果存在则获得该轨迹信息,轨迹信息主要包含了如下内容:

  struct TrajectoryData {double gravity_constant = 9.8;std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};absl::optional<transform::Rigid3d> fixed_frame_origin_in_map;};

先把 fixed_frame_pose_initialized 设置为 false,接着就是对轨迹的每个节点进行遍历了:

    // 遍历一个轨迹的所有节点, 添加gps的残差for (; node_it != trajectory_end; ++node_it) {const NodeId node_id = node_it->id;const NodeSpec2D& node_data = node_it->data;// 根据节点的时间对gps数据进行插值, 获取这个时刻的gps数据的位姿const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);// 要找到第一个有效的数据才能进行残差的计算if (fixed_frame_pose == nullptr) {continue;}// gps数据到gps第一个数据间的坐标变换const Constraint::Pose constraint_pose{*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),options_.fixed_frame_pose_rotation_weight()};      

先获得遍历轨迹节的time,利用 FixedFramePoseData 数据进行插值,获得 GPS该时刻的位姿 fixed_frame_pose,需要注意 gps数据是到gps第一个数据间的坐标变换,那么可以为这个变换创建一个约束constraint_pose,其中 constraint_pose.zbar_ij 就是 *fixed_frame_pose。

五、GPS残差-global系到gps系变换

在建立好 约束 constraint_pose 之后,如果 fixed_frame_pose_initialized 不为true,也就说目前遍历的是该轨迹的第一个节点,那么则对GPS位姿进行一个初始化处理:

      // 计算gps坐标系原点在global坐标系下的坐标if (!fixed_frame_pose_initialized) {transform::Rigid2d fixed_frame_pose_in_map;// 如果设置了gps数据的原点if (trajectory_data.fixed_frame_origin_in_map.has_value()) {fixed_frame_pose_in_map = transform::Project2D(trajectory_data.fixed_frame_origin_in_map.value());} // 第一次优化之前执行的是这里else {// 计算gps第一个数据在global坐标系下的坐标, 相当于gps数据的坐标系原点在global坐标系下的坐标fixed_frame_pose_in_map =node_data.global_pose_2d *transform::Project2D(constraint_pose.zbar_ij).inverse();}// 保存gps坐标系原点在global坐标系下的坐标C_fixed_frames.emplace(trajectory_id,FromPose(fixed_frame_pose_in_map));fixed_frame_pose_initialized = true;}

其先判断一下该轨迹是否设置了GPS原点,也就是 trajectory_data.fixed_frame_origin_in_map 参数是否设置,如果设置了,则把其映射到2D平面赋值给 fixed_frame_pose_in_map。

如果没有设置,node_data.global_pose_2d 表示global下机器人位姿,ransform::Project2D(constraint_pose.zbar_ij) 表示机器人在GPS下的位姿,

          fixed_frame_pose_in_map =node_data.global_pose_2d *transform::Project2D(constraint_pose.zbar_ij).inverse();

那么 fixed_frame_pose_in_map 就表示GPS系相对于 Global系的位姿了。也就是说
fixed_frame_pose_in_map 表示GPU第一个数据在global系下的位姿,计算出来之后,fixed_frame_pose_initialized 赋值为 true,就不会再为该轨迹计算 fixed_frame_pose_in_map 了。求得 fixed_frame_pose_in_map 会被添加到 C_fixed_frames 之中。

六、GPS残差-残差构建

最后就是最核心的残差构建部分了,主体代码如下所示:

      // Step: 第五种残差 节点与gps坐标系原点在global坐标系下的相对坐标变换 与 通过gps数据进行插值得到的相对坐标变换 的差值作为残差项problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose),options_.fixed_frame_pose_use_tolerant_loss()? new ceres::TolerantLoss(options_.fixed_frame_pose_tolerant_loss_param_a(),options_.fixed_frame_pose_tolerant_loss_param_b()): nullptr,C_fixed_frames.at(trajectory_id).data(), // 会自动调用AddParameterBlockC_nodes.at(node_id).data());}

constraint_pose 是利用GPS数据插值出来当前遍历节点 node_it 对应的位姿。ceres::TolerantLoss() 同样用于残差结果的抑制异常点。根据 C_fixed_frames.at(trajectory_id).data() 表示GPU系在global系下的位姿,
C_nodes.at(node_id).data()) 表示当前节点在global系下的位姿,那么可以求得当前节点相对于GPS系的位姿。再与约束 constraint_pose 做残差。

七、开始优化,且更新数据

  // Solve. 进行求解ceres::Solver::Summary summary;ceres::Solve(common::CreateCeresSolverOptions(options_.ceres_solver_options()),&problem, &summary);// 如果开启了优化的log输出, 就输出ceres的报告if (options_.log_solver_summary()) {LOG(INFO) << summary.FullReport();}// 将优化后的所有数据进行更新 Store the result.for (const auto& C_submap_id_data : C_submaps) {submap_data_.at(C_submap_id_data.id).global_pose =ToPose(C_submap_id_data.data);}for (const auto& C_node_id_data : C_nodes) {node_data_.at(C_node_id_data.id).global_pose_2d =ToPose(C_node_id_data.data);}for (const auto& C_fixed_frame : C_fixed_frames) {trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =transform::Embed3D(ToPose(C_fixed_frame.second));}for (const auto& C_landmark : C_landmarks) {landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();}

八、结语

到这里位姿,可以说后端优化或者位姿图的讲解基本就完成了,总的来说,后端优化中共有五种残差,分别如下所示:

1、基于节点与子图(子图内,子图间)约束的残差
2、基于Landmark的残差
3、基于odometry里程计的残差
4、基于节点local系下的残差
5、基于GPS数据的残差

下一篇博客,就带代价对后端优化进行一个整理的复盘,让大家的理解更加


http://www.ppmy.cn/news/725295.html

相关文章

SpringBoot操作Excel实现导入和导出功能(详细讲解+Gitee源码)

前言&#xff1a;在日常的开发中&#xff0c;避免不了操作Excel&#xff0c;比如从系统当中导出一个报表&#xff0c;或者通过解析客户上传的Excel文件进行批量解析数据入库等等&#xff0c;本篇博客主要汇总日常开发中如何使用开源的Apache提供的POI流操作Excel进行导入导出功…

《守望先锋》中的网络同步技术

youtube上面有原版&#xff0c;国内熟肉&#xff1a;https://v.qq.com/x/page/w03103llegc.html 一对好基友在讲技术&#xff0c;看着莫名的喜感 主要聊了overwatch中的网络同步技术&#xff0c;以及如何去优化。 philosophy 这个词发现在暴雪的各类分享中常常使用&#xff0…

暴雪守望先锋显示连接暴雪服务器超时,守望先锋 连接暴雪游戏服务器超时

******************************************************************************** [守望先锋]客户端到服务器Ping测试&#xff1a;服务器&#xff0c;223.252.226.255&#xff1b;结果&#xff1a; 正在 Ping 223.252.226.255 具有 32 字节的数据: 来自 223.252.226.255 的…

[gdc17]《守望先锋》的EntityComponent架构

gdc17上面由暴雪的Tim Ford带来。 由于原文是gdcvault上面的付费内容&#xff0c;贴下腾讯的gad上的翻译&#xff1a;http://gad.qq.com/article/detail/7212152?bsh_bid1732845294 本文可以说是对于entity/component系统&#xff08;entity component system简称ECS了&…

绝地求生+守望先锋?2019首款黑马游戏Apex英雄凭什么如此火爆?

Apex英雄好玩吗&#xff1f;要说2019目前最火爆的游戏非Apex英雄莫属。2019年2月5日&#xff0c;首款吃鸡黑马游戏诞生了&#xff0c;怎么形容这款游戏的火爆程度呢&#xff1f;给大家看一个数据&#xff1a; Apex英雄上线不到72小时&#xff0c;就收货了1000万玩家&#xff1b…

守望先锋为何如此火爆

16340217 1 数据科学与计算机学院 必要的公式之质能方程 Emc2 背景模式分析有话说 必要的公式之质能方程背景模式 攻防战运载目标占领要点突击护送 分析有话说 背景 21世纪20年代初&#xff0c;人类设计了由人工智能创造的智能机械&#xff08;简称为智械&#xff09;&…

Eigen 类型三维点的世界坐标转换为 OpenCV Mat 类型

cv::Mat WorldPos; cv::eigen2cv(pMP->GetWorldPos(), WorldPos); vPoints.push_back(WorldPos);cv::Mat WorldPos;:这行代码声明了一个 OpenCV Mat 对象 WorldPos,用于存储三维点在世界坐标系下的位置。 cv::eigen2cv(pMP-

docker学习(十)Docker 持久化存储

了解联合文件系统后&#xff0c;我们知道&#xff0c;镜像是只读的&#xff0c;类似共享文件形式让多个容器使用。如果要在容器里修改文件&#xff0c;即镜像里的文件&#xff0c;那该如何修改&#xff1f; 为了解决这个问题&#xff0c;docker 引入了 写时复制&#xff08;cop…

ehvierwer登录与不登录_lol手游为什么登不进去?英雄联盟手游登录不上原因分析[多图]-资讯...

最近英雄联盟刚刚开启公测&#xff0c;但是出现了非常严重的问题&#xff0c;很多人表示LOL登录不上去&#xff0c;出现这种情况是什么原因呢&#xff1f;相信很多玩家遇见这种情况还是比较着急的&#xff0c;想知道是什么原因造成的&#xff0c;应该怎么处理&#xff0c;下面小…

lol手游显示无法连接谷歌服务器,《lol手游》谷歌账号登录失败如何解决 谷歌账号登录失败解决教程...

导 读 lol手游谷歌账号登录失败怎么办&#xff1f;登录不上的原因有很多种&#xff0c;不过玩家在使用谷歌账号&#xff0c;并且是按照正规途径下载的游戏&#xff0c;却也依旧无法顺利进入到游戏当中去&#xff0c;这也给人一种很绝望的感觉&#xff0c;究竟该如何操作&#x…

英雄联盟手游登录注册地与服务器不匹配,lol手游无法使用当前地区账号登录原因说明,相关解决方法分享...

在lol的手游中&#xff0c;最近注册登录的玩家越来越多&#xff0c;上线登录的时间也越来越频繁&#xff0c;很多服务器也处于无法登陆的状态&#xff0c;刚刚也有玩家发现Unable to login with an account from this region的登录提示&#xff0c;这是表示无法使用当前地区的账…

OpenResty cosocket

cosocket 是各种 lua-resty-* 非阻塞库的基础 cosocket coroutine socket 需要 Lua 协程特性的支持&#xff0c;也需要 Nginx 事件机制的支持&#xff0c;两者结合在一起实现非阻塞网络 I/O。 遇到网络 I/O 时会交出控制权&#xff0c;把网络事件注册到 Nginx 监听列表中&a…

ehvierwer登录与不登录_英雄联盟手游登录不进去怎么办 lol手游错误代码合集及解决方案...

英雄联盟手游登录不进去怎么办&#xff1f;lol手游开测后很多玩家都被五花八门的错误代码拦在门外&#xff0c;导致无法进入游戏。今天优游网小编为大家带来的就是lol手游错误代码合集以及对应的解决方法了&#xff0c;大家一起来看看吧&#xff01; 英雄联盟手游登录不进去怎么…

Elasticsearch 脚本编写基础

文章目录 脚本语言脚本使用格式脚本中访问文档字段script_fields 对查询字段进行计算 脚本中使用参数存储脚本创建或更新存储脚本路径参数正文参数 在查询中使用删除存储脚本 脚本语言 painless ES 的脚本语言可以通过 lang 来设置&#xff0c;如果不设置&#xff0c;默认为 P…

使用 Zabbix 监控 RocketMQ列举监控项和触发器

在使用 Zabbix 监控 RocketMQ 的过程中&#xff0c;以下是一些可能的监控项和触发器&#xff1a; 监控项 集群总体健康状况生产者和消费者的连接数量Broker 的状态消息的生产和消费速度队列深度&#xff08;即队列中的消息数量&#xff09;磁盘空间使用内存使用CPU使用网络流…

校园电动车管理与充电指引—微信小程序

先上页面 1.注册与登陆 用户打开微信APP&#xff0c;搜索“校园电动车管理和充电指引”或者点击微信好久分享的链接或扫描小程序二维码便可以搜索到小程序,新用户将进入登陆页面。可先点击最下方“马上注册”或者直接选择学生用户身份并且点击微信一键登陆微信授权后直接注册…

Find My资讯|奥迪推出e-MTB电动山地自行车,自行车防丢还得看Find My

奥迪&#xff08;Audi&#xff09;近日和意大利公司 Fantic 合作&#xff0c;以参加达喀尔拉力赛的越野车 RS Q e-tron 为灵感&#xff0c;推出了电动山地自行车 e-MTB。 这款电动山地自行车采用 250W、36V 的 S-MAG 引擎&#xff0c;最高扭矩为 90Nm&#xff0c;除了无助力方…

如何建成消防可实时监测的电动车棚充电可视化火灾报警系统

一、背景需求 电动车火灾事故频繁发生&#xff0c;电动车不规范充电已然成为引起火灾的常见原因之一。如何充电才能更安全、避免火灾发生、进行管理和监控……这些都是市民比较关注的问题。除了杜绝飞线充电、安排巡查人员、8小时制充电外&#xff0c;还有哪些办法可以预防&am…

自行车租赁管理系统

1、项目介绍 自行车租赁管理系统拥有两种角色 管理员&#xff1a;订单管理、服务站管理、自行车管理、管理员信息管理等 用户&#xff1a;查看自行车信息、租借自行车、购物车、查看所有订单等 2、项目技术 后端框架&#xff1a; Hibernate、Servlet、mvc模式 前端技术&a…

被补贴“耽误”了十年的电动车,造车新势力陷入生死考

文|Ihahe 来源|智能相对论&#xff08;aixdlun&#xff09; 当2019年度新能源车补贴政策正式下发&#xff0c;电动车行业蒙圈了。 虽然是早已料到的结果&#xff0c;但是补贴退坡如此之大还是让人始料未及。 一、电动车行业被政策抛弃了&#xff1f; 的确可以这么认为&…
最新文章