karto-slam解析
2016-12-04 09:32
260 查看
karto源代码在好几个月之前就已经看完了,但是一直没有整理,趁着周末来整理一下。
应用层
作为用户需要知道kartoslam是如何使用的。首先需要从github上git以下两个包:
git clone https://github.com/ros-perception/open_karto(开源的karto包,实现底层的kartoslam)
git clone https://github.com/ros-perception/slam_karto(ros层,也就是应用层的kartoslam接口)
将这些包放到工作目录,完成编译,我们就能尝试运行代码啦(当然你会发现,运行会有error)~~~~~~~
因为,在这之前我们需要为这个kartoslam定义一个tf变换关系才能保证代码的正确运行:
在karto_slam.launch中加入/base_link与laser的相对/tf关系(<node pkg="tf" type="static_transform_publisher" name="link_to_laser" args="0 0 0 0 0 0 1 /base_link /laser 10" />)(为什么呢?后面再讲~~)
如果你有一个激光雷达,一台turtlebot,还有一台电脑,那么你就可以利用kartoslam实现slam啦!当然,我们也可以在karto_slam.launch修改一下配置(比方说,我们希望的地图更新频率:map_update_interval,地图的分辨率:resolution等等)。
理论层
机器人在运行slam的过程中,需要知道几个参考坐标系之间的关系:
map_frame:世界坐标系中的固定frame。也就是说,这是一个全局的固定frame,它理论上不会随着机器人的移动或者测量的误差而产生漂移,一般设置系统的初始状态作为坐标原点。
odom_frame:里程计frame。它是移动机器人在移动过程中利用里程计(如车轮编码器,视觉里程计等)对机器人进行位姿估计的坐标系,一般设置机器人初始位置作为坐标原点,总所周知,里程计在长时间的使用中会出现一下累计误差。
baselink_link_frame:机器人frame。它是机器人坐标系,是一个移动的坐标系。
laser_frame:激光frame。它是激光雷达的坐标系,固定在机器人上,与baselink_link_frame往往只存在一个平移变换量。
这4个坐标系是按照树状结构组织起来的,它们的对应关系如下图所示:
从frame图中可以看到,laser_frame和aselink_link_frame的关系是由/link_to_laser得到的(也就是一开始我们在launch中添加的代码);baselink_link_frame和odom_frame之间的关系是由底层的里程计信息topic输出得到的;而odom_fra
4000
me与map_frame之间的变换关系是由karto_slam得到的。karto_slam一直在做的事情就是:不断去修正里程计与固定的全局坐标系map_frame的误差,从而得到机器人相对于固定的世界坐标系更加精准的位姿信息。
代码层
代码从slam_karto.cpp开始,首先需要有接口函数
之后对满足关键帧要求的数据进行匹配
至于匹配的方法,具体就不细讲了。主要用的是map和frame的匹配,采用窗口法寻找最优的变换矩阵,并采用粗匹配(coarseSearch),精匹配(fineSearch)和建立一个lookupTable加快了寻优的速度,提高寻优精度。
之后将当前帧放入图优优化中,增加vertex和edges。
注:图的构造(感觉自己说的好绕口)
1, 只提取关键帧(两帧直接距离大于设定值)
2,当前帧相邻时间的关键帧相连
3,当前帧与runningScan中最近的帧相连 (runningScan可以理解为局部地图)
4,与当前帧相邻但不相连的帧链中最近的帧相连(闭环连接关系)
5,闭环检测:将在一定范围内的不与当前帧相连且不与NearLinkedScans相连的帧连接起来组成闭环(也就是在一定范围内不与当前帧有任何连线的帧连接起来,构成闭环)
然后,尝试闭环检测
参考文献
《Efficient Sparse Pose Adjustment for 2D Mapping》
应用层
作为用户需要知道kartoslam是如何使用的。首先需要从github上git以下两个包:
git clone https://github.com/ros-perception/open_karto(开源的karto包,实现底层的kartoslam)
git clone https://github.com/ros-perception/slam_karto(ros层,也就是应用层的kartoslam接口)
将这些包放到工作目录,完成编译,我们就能尝试运行代码啦(当然你会发现,运行会有error)~~~~~~~
因为,在这之前我们需要为这个kartoslam定义一个tf变换关系才能保证代码的正确运行:
在karto_slam.launch中加入/base_link与laser的相对/tf关系(<node pkg="tf" type="static_transform_publisher" name="link_to_laser" args="0 0 0 0 0 0 1 /base_link /laser 10" />)(为什么呢?后面再讲~~)
如果你有一个激光雷达,一台turtlebot,还有一台电脑,那么你就可以利用kartoslam实现slam啦!当然,我们也可以在karto_slam.launch修改一下配置(比方说,我们希望的地图更新频率:map_update_interval,地图的分辨率:resolution等等)。
理论层
机器人在运行slam的过程中,需要知道几个参考坐标系之间的关系:
map_frame:世界坐标系中的固定frame。也就是说,这是一个全局的固定frame,它理论上不会随着机器人的移动或者测量的误差而产生漂移,一般设置系统的初始状态作为坐标原点。
odom_frame:里程计frame。它是移动机器人在移动过程中利用里程计(如车轮编码器,视觉里程计等)对机器人进行位姿估计的坐标系,一般设置机器人初始位置作为坐标原点,总所周知,里程计在长时间的使用中会出现一下累计误差。
baselink_link_frame:机器人frame。它是机器人坐标系,是一个移动的坐标系。
laser_frame:激光frame。它是激光雷达的坐标系,固定在机器人上,与baselink_link_frame往往只存在一个平移变换量。
这4个坐标系是按照树状结构组织起来的,它们的对应关系如下图所示:
从frame图中可以看到,laser_frame和aselink_link_frame的关系是由/link_to_laser得到的(也就是一开始我们在launch中添加的代码);baselink_link_frame和odom_frame之间的关系是由底层的里程计信息topic输出得到的;而odom_fra
4000
me与map_frame之间的变换关系是由karto_slam得到的。karto_slam一直在做的事情就是:不断去修正里程计与固定的全局坐标系map_frame的误差,从而得到机器人相对于固定的世界坐标系更加精准的位姿信息。
代码层
代码从slam_karto.cpp开始,首先需要有接口函数
SlamKarto::addScan(karto::LaserRangeFinder* laser,const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose)得到里程计和激光信息,并通过
// create localized range scan karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan(laser->GetName(), readings); range_scan->SetOdometricPose(karto_pose); range_scan->SetCorrectedPose(karto_pose);将数据放入kato特定的数据结构当中。
(processed = mapper_->Process(range_scan))Process是karto的入口函数,首先需要初始化:
if (m_Initialized == false) { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); }
之后对满足关键帧要求的数据进行匹配
m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, covariance);得到估计的位姿信息bestPose和对应的协方差矩阵convariance。
至于匹配的方法,具体就不细讲了。主要用的是map和frame的匹配,采用窗口法寻找最优的变换矩阵,并采用粗匹配(coarseSearch),精匹配(fineSearch)和建立一个lookupTable加快了寻优的速度,提高寻优精度。
之后将当前帧放入图优优化中,增加vertex和edges。
注:图的构造(感觉自己说的好绕口)
1, 只提取关键帧(两帧直接距离大于设定值)
2,当前帧相邻时间的关键帧相连
3,当前帧与runningScan中最近的帧相连 (runningScan可以理解为局部地图)
4,与当前帧相邻但不相连的帧链中最近的帧相连(闭环连接关系)
5,闭环检测:将在一定范围内的不与当前帧相连且不与NearLinkedScans相连的帧连接起来组成闭环(也就是在一定范围内不与当前帧有任何连线的帧连接起来,构成闭环)
然后,尝试闭环检测
if (m_pDoLoopClosing->GetValue()) { std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames(); const_forEach(std::vector<Name>, &deviceNames) { m_pGraph->TryCloseLoop(pScan, *iter); } }最后更新corrected_pose,publish map_to_odom的tf关系(也就是里程计的漂移误差)。
参考文献
《Efficient Sparse Pose Adjustment for 2D Mapping》
相关文章推荐
- 数字图像处理的MATLAB实现[2]
- 如何更改Tomcat的默认端口
- 【常用模板】 01背包
- 重振精神,拼命干,5*5=25模式已不符合现在状况
- gensim 教程 -Part1
- 【PAT】4-2. 打印九九口诀表(15)
- word2vec API
- HDU 2561 第二小整数 水题
- Distributed Representations of Words and Phrasesand their Compositionality
- 架构师的成长之路
- Distributed Representations of Words and Phrasesand their Compositionality
- Keras API
- 矩阵输出
- thinkphp中session跨域问题
- Android拍照识别身份证SDK
- 《Reverse Engineering for Beginners》 - 第1章 代码模式 - 笔记(1.5-1.6)
- ButterKnife8.4使用配置
- PDF书籍页码优化显示技巧(基于Acrobat Pro DC 2015版)
- map常用用法
- 20145304 20145315 《信息安全系统设计基础》 实验五 网络通信