graph slam tutorial :从推导到应用3

       为了更好地理解graph based slam的过程,本文以二维平面的激光SLAM为例子,先简单介绍如何根据传感器信息构建图,即图优化的前端(front-end)。然后再针对上篇博客的疑问,结合matlab程序,分析图优化的后端(back-end)。

       对于二维平面的激光SLAM,数据包括两部分,odometry和laser range data,所以构图过程如下:

  • 当机器人前进0.5m或者旋转超过0.5弧度时,将新的机器人位姿添加到图的顶点,并且记录相应的激光数据。
  • 当前激光数据和上一次的激光数据进行匹配,通过scan match获得两个相邻位姿之间的变换关系,将该变换关系加入到图的边。
  • 当机器人重新回到一个已知区域时,激光数据和以前的诺干组数据进行匹配进行闭环检测。如果匹配成功,在相应顶点之间加入一条边。

      对于3D的SLAM图优化的前端和上述过程基本差不多。

图优化后端

1.构建误差函数

      在构建好图以后,就得根据误差函数求雅克比矩阵,然后根据雅克比矩阵求b以及系统信息矩阵H了。对于2维SLAM,我们知道机器人某一时刻的位姿可以表示成。在各类论文中(主要是弗莱堡大学Grisetti派系的),把误差函数设定为如下形式:


其中函数t2v()表示将位姿矩阵转为向量,直接看代码。

% A =  cos -sin x
%      sin cos y
%      0    0   1
% v = (x,y,theta)
function v = t2v(A)
% T2V homogeneous transformation to vector
v(1:2,1) = A(1:2,3); % 第三列第1,2行,即x,y
v(3,1) = atan2(A(2,1), A(1,1));
end

并且上式中是位姿向量矩阵形式,使用一个v2t()的函数就行了。

function A = v2t(v)
% V2T vector to homogeneous transformation
c = cos(v(3));
s = sin(v(3));
A = [c, -s, v(1);
     s,  c, v(2);
     0   0  1];
end

误差函数表达式中要注意表示位姿j到位姿i之间的变换矩阵,也可以说是坐标j和坐标i之间的差异,至于为什么这里两个矩阵相乘就能表示它们的差异,这是由于它们是位姿矩阵,看下面的分析。

        先来一个简单版的推导:

        表示位姿i在世界坐标系w中的表示,也表示将坐标系i中的一点转换到世界坐标系w中的转换矩阵,同理表示坐标系j中的一点转换到世界坐标系w中的转换矩阵,也可以说是坐标系j在世界坐标系w中的表示。

        现在可以看看那两个矩阵相乘的含义了:,最后的结果是Xij,而它表示的就是坐标系j到坐标系i的转换矩阵。

        上面这个推导用下标推算简单说明了情况,背后的数学推导还是有必要看看的。现在开始严格一点的数学推导。我们先不分析分析和分析是一样的,所以直接分析,即为什么测量出的转换矩阵和理论计算的转换矩阵的误差err要这样计算?

预备知识:对于分块矩阵如何求逆。

                          

现在开始推导。不妨假设j到i变换的变换矩阵估计值为:

                 

注意,矩阵中的组成分别是旋转矩阵R和平移向量t。i,j之间变换的测量值变换矩阵为:

         

误差计算:


                                                      

                                                       

误差计算中最后一步是把上面的矩阵向量化。

      旋转矩阵表示逆时针旋转,表示顺时针旋转,两个旋转矩阵相乘再向量化的物理意义不就是两个旋转矩阵角度的差异嘛。表示两个位移向量的差异。因此,同理也表示位姿j到位姿i之间的变化。


2.计算雅克比矩阵

      误差函数有了,接下来最重要的一步是计算雅克比矩阵,先把误差函数由矩阵形式一步一步向量化,先把如下


将上面的矩阵再进一步转化为误差向量。


把上面的误差向量对机器人位姿i求偏导得到Aij。先对平移向量求偏导,再对角度求偏导得


同理,对位姿j求偏导得到Bij。


程序中对应的计算如下:

%compute the homoeneous transforms of the previous solutions
zt_ij = v2t(z_ij);%向量转化为矩阵
vt_i = v2t(v_i);
vt_j = v2t(v_j);

%compute the displacement between x_i and x_j
f_ij=(inv(vt_i) * vt_j);% Xi的逆乘以Xj

%this below is too long to explain, to understand it derive it by hand
theta_i = v_i(3);
ti = v_i(1:2,1);
tj = v_j(1:2,1);
dt_ij = tj-ti;

si = sin(theta_i);
ci = cos(theta_i);

% 这里的A,B是还没有乘以Rz转置的
A= [-ci, -si, [-si, ci]*dt_ij; si, -ci, [-ci, -si]*dt_ij; 0, 0, -1 ];
B =[  ci, si, 0           ; -si, ci, 0            ; 0, 0, 1 ];

ztinv = inv(zt_ij);
e = t2v(ztinv * f_ij); % 误差向量
ztinv(1:2,3) = 0;% 偏导A,B计算公式中只用了z的旋转矩阵R,所以把平移向量强制清0
A = ztinv*A; % 乘以Z的转置,即Z的逆,这是由于旋转矩阵的关系
B = ztinv*B;
有了雅克比矩阵以后,只要将计算的b,H累加起来就行了

对应代码如下:

    %compute the blocks of H^k
    b_i = -A' * omega * e;
    b_j = -B' * omega * e;
    H_ii=  A' * omega * A;
    H_ij=  A' * omega * B;
    H_jj=  B' * omega * B;
    
    %accumulate the blocks in H and b
    H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3)+ H_ii;
    H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) + H_jj;
    H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) + H_ij;
    H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) + H_ij';
    b((id_i-1)*3+1:id_i*3,1) = ...
        b((id_i-1)*3+1:id_i*3,1) + b_i;
    b((id_j-1)*3+1:id_j*3,1) = ...
        b((id_j-1)*3+1:id_j*3,1) + b_j;
       所有部分计算完毕以后,就是计算增量,迭代直到收敛。
下面是matlab代码中,图优化前和优化后的机器人轨迹对比:

  

整个代码是grisetti课程里面代码改成的matlab版。我不生产代码,只是github的搬运工,O(∩_∩)O 下载地址戳这里在看代码前,建议看看关于图顶点和边的数据说明。

图的数据格式       

       最后讲一下图优化前端处理以后产生的顶点和边的数据格式,这是写程序时特别关注的,也是众多优化包如g2o的数据格式。

       顶点vertex2: id,pose.x,pose.y,pose.theta 其中id表示位姿的序号,后面三个是位姿参数

       边EDGE2: idFrom idTo mean.x mean.y mean.theta inf.xx inf.xy inf.xt inf.yy inf.yt  inf.tt 其中idfrom,idTo表示连接边的两个位姿顶点序号,mean.xytheta表示测量的位姿变换矩阵。inf表示边的信息矩阵即权重。


       到目前为止,基本已经了解了graph based slam是咋回事。在实际编程中,弗莱堡大学的牛人们开发了g2o优化包帮大家解决图优化的后端(back-end)问题。因此写写图优化前端的程序(match,icp求位姿矩阵,loop closure),构建图,然后再用g2o库就能够完成一个图优化SLAM程序,是不是感觉思路很清晰,很容易。关于g2o库的使用,Grisetti写了一个教程,g2o也自带很多例子,但是不自己动动手,永远给人感觉像是雾里看花,水中望月,不踏实。在下一篇博客中,我们将拿一个数据集练练手,操练操练g2o库的使用,祝好运。



(转载请注明作者和出处:http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途)


reference:

1. Grisetti的课程,有课件下载,点这里

2. Grisetti. 《A Tutorial on Graph-Based SLAM》

3. Grisetti. 课件 《SLAM Back-end》(可以直接搜到)





已标记关键词 清除标记
一、课程简介 随着技术的飞速发展,经过多年的数据积累,各互联网公司已保存了海量的原始数据和各种业务数据,所以数据仓库技术是各大公司目前都需要着重发展投入的技术领域。数据仓库是面向分析的集成化数据环境,为企业所有决策制定过程,提供系统数据支持的战略集合。通过对数据仓库中数据的分析,可以帮助企业改进业务流程、控制成本、提高产品质量等。 二、课程内容 本次精心打造的数仓项目的课程,从项目架构的搭建,到数据采集模块的设计、数仓架构的设计、实战需求实现、即席查询的实现,我们针对国内目前广泛使用的Apache原生框架和CDH版本框架进行了分别介绍,Apache原生框架介绍中涉及到的技术框架包括Flume、Kafka、Sqoop、MySql、HDFS、Hive、Tez、Spark、Presto、Druid等,CDH版本框架讲解包括CM的安装部署、Hadoop、Zookeeper、Hive、Flume、Kafka、Oozie、Impala、HUE、Kudu、Spark的安装配置,透彻了解不同版本框架的区别联系,将大数据全生态系统前沿技术一网打尽。在过程中对大数据生态体系进行了系统的讲解,对实际企业数仓项目中可能涉及到的技术点都进行了深入的讲解和探讨。同时穿插了大量数仓基础理论知识,让你在掌握实战经验的同时能够打下坚实的理论基础。 三、课程目标 本课程以国内电商巨头实际业务应用场景为依托,对电商数仓的常见实战指标以及难点实战指标进行了详尽讲解,具体指标包括:每日、周、月活跃设备明细,留存用户比例,沉默用户、回流用户、流失用户统计,最近连续3周活跃用户统计,最近7天内连续3天活跃用户统计,GMV成交总额分析,转化率及漏斗分析,品牌复购率分析、订单表拉链表的设计等,让学生拥有更直观全面的实战经验。通过对本课程的学习,对数仓项目可以建立起清晰明确的概念,系统全面的掌握各项数仓项目技术,轻松应对各种数仓难题。 四、课程亮点 本课程结合国内多家企业实际项目经验,特别加入了项目架构模块,从集群规模的确定到框架版本选型以及服务器选型,手把手教你从零开始搭建大数据集群。并且总结大量项目实战中会遇到的问题,针对各个技术框架,均有调优实战经验,具体包括:常用Linux运维命令、Hadoop集群调优、Flume组件选型及性能优化、Kafka集群规模确认及关键参数调优。通过这部分学习,助学生迅速成长,获取前沿技术经验,从容解决实战问题。
课程目标 从零开始掌握Premiere影视剪辑的基础知识,学会视频剪辑+影视特效+视频调色+字幕制作 适用人群 PR零基础小白,在校大学生,职场新人,想成为影视剪辑、影视后期、短视频制作、自媒体等高手的朋友。 课程简介 课程以新版PR 2020讲解,可使用PR任意版本学习,绝大多数功能兼容旧版,但强烈建议使用新版本。 【Adobe认证专家讲师精耕细作精品教程,非学院派照本宣科软件操作教程,以任务为导向,面向实际应用场景,每一章都能学会实打实的高手技能,讲解细致,小白也能轻松入门成大神!】 课程好不好,看过就知道,前面的免费章节欢迎试看。 本课程学习不需要任何PR基础,只需要电脑操作基础即可。兼容Windows和Mac操作系统,同时讲解两种系统下的快捷键操作,不用担心操作上的障碍问题。 课程特色 1、以实际PR影视编辑与特效的流程为导向,绝大多数内容都是为了完成某个具体任务,而不是为了讲解某个软件操作而凑数。 2、不同于国内多数教程和书籍,每个知识点务求讲精、讲透,帮助你掌握PR的精髓,而非软件操作上的皮毛,让你真正学到PR的本质,一次学习,终身受用,少走弯路,节约生命。 3、课程会随PR新版本的推出持续更新,不必担心有新功能却不知道怎么用。 4、充足的练习题和作业题,让你在不断的练习和挑战中提升PR技能。
©️2020 CSDN 皮肤主题: 游动-白 设计师:上身试试 返回首页