精华内容
下载资源
问答
  • 用于kitti数据集的LeGO-LOAM 该存储库包含LeGO-LOAM的修改后的代码,可通过kitti数据集运行和评估。 运行代码时,您将以KITTI地面格式获取LeGO-LOAM的轨迹结果,并且可以通过EVO-eval套件使用KITTI地面真实结果直接...
  • A-LOAM.zip

    2020-10-17 22:50:00
    将秦通的aloam稍作修改,使对于输入的每一帧激光数据,都输出一个建图后的pose(非实时),原版本的aloam在建图过程为保证实时性,会丢掉一部分数据。 修改的部分: 1 发布与订阅缓冲区的大小 2 建图频率 3 取消为保证...
  • loam_velodyne_kitti_ros 该软件包是LOAM算法的ROS Indigo版本的修改副本,可与KITTI数据集一起使用: 在以下方面进行了主要更改: scanRegistration.cpp重命名为scanRegistrationKittiROS.cpp 通过读取.bin...
  • SC-Lego-LOAM:LiDAR SLAM:扫描上下文+ LeGO-LOAM
  • 在使用ROS做SLAM时,会用到开源算法loam,此文件为loam官方激光雷达点云测试数据包,利用此数据包可以对loam进行测试和建图,测试loam以及其他依赖安装是否合适
  • 这篇文章提出了一个名为LeGO-LOAM的轻量级6自由度的激光SLAM方法. LeGO-LOAM主要分为点云分隔, 特征提取, 里程计和建图四个方面, 在特征提取方面它借鉴了LOAM算法, 并且和其他的激光SLAM方法相比, LeGO-LOAM的亮点是...
  • 可以在A-LOAM/analysis/ntuviral_aloam文件夹中找到A-LOAM/analysis/ntuviral_aloam 。 只需运行MATLAB脚本checkall_parallel.m ,结果将被打印出来。 如果您在工作中使用NTU VIRAL数据集,请使用数据集处的BibTex...
  • 安装过程见:添加链接描述 ...等到pcl安装完毕,安装loam,在用catkin编译loam之后,出现如下图所示的报错 错误提示: – +++ processing catkin package: ‘loam_velodyne’ – ==> add_subdirectory(loam_ve
  • Lego-loam安装编译-附件资源
  • [学习SLAM]loam_velodyne的安装与使用1-附件资源
  • loam_velodyne-master.zip

    2019-09-01 08:13:30
    slam建图,三维点云地图,适合大场景,使用一个三维空间中运动的两轴激光雷达来构建实时激光里程计。
  • floam:快速LOAM:用于室内室外定位的快速和优化的Lidar里程表和制图(Lidar SLAM)
  •  LOAM是KITTI测试中排名第一的状态估计和激光建图方法,知名度很高,在它的基础上衍生出了很多改进版本,例如ALOAM、LEGO-LOAM、Inertial-LOAM等。  本文通过对论文和代码的细节进行分析,试图弄明白这个方法的...
  • 基于高度图的ORB特征点法激光里程计——orb_loam首先,采用ORB特征点检测(移植了ORB-SLAM2的四叉树管理部分,提高特征点提取效率);然后,采用ORB特征点匹配,获得匹配关系;最后,通过2D-ICP计算帧间运动估计。
  • SLAM学习笔记,欢迎大家交流有关问题。。。。。。。。。。。。。。。。。。。。
  • eigen3.2.8_for_loam.zip

    2020-11-15 16:04:59
    这是loam等开源代码运行程序所必须的依赖库。
  • loam_velodyne使用速腾聚创16线激光雷达建图和计算激光雷达里程计-附件资源
  • M-LOAM 具有在线外部校准功能的多LiDAR系统的稳健里程表和制图 M-LOAM是用于多LiDAR外在校准,实时测距和制图的强大系统。 在没有人工干预的情况下,我们的系统可以从几个外部未校准的LiDAR开始,自动校准它们的外部...
  • matlab精度检验代码LeGO-LOAM的评估:轻量级和地面优化的激光雷达里程表和制图 这是Team 11针对NAVARCH / EECS 568,ROB 530:Mobile Robotics的最终项目git存储库。 我们项目的标题是“乐高-LOAM的评估:轻量级和...
  • 速腾16线激光雷达运行LOAM、A-LOAM和LeGO-LOAM

    千次阅读 多人点赞 2021-02-15 23:29:07
    本人使用turtlebot搭载速腾16线激光雷达(只使用激光雷达,没有用到IMU),运行并对比LOAM、A-LOAM和LeGO-LOAM异同。 环境: 1、速腾16线激光雷达rslidar; 2、工控机; 3、Ubuntu 16.04; 4、Turtlebot。 参考博客:...

    二维激光雷达SLAM已经相当成熟,前一段时间开始接触三维激光雷达,发现资料很少,遇到了很多问题,也有一些收获,趁着假期整理一下。本人使用turtlebot搭载速腾16线激光雷达(只使用激光雷达,没有用到IMU),运行并对比LOAM、A-LOAM和LeGO-LOAM异同。

    环境:

    1、速腾16线激光雷达rslidar;
    2、工控机;
    3、Ubuntu 16.04;
    4、Turtlebot。

    参考博客:
    1.记录robosense16多线雷达配置过程(同门的博客)https://blog.csdn.net/Anubissz/article/details/107838647
    2. 3D-SLAM 入门教程-多线雷达(RSlidar 16)loam_velodyne 3D 建图https://www.ncnynl.com/archives/201702/1382.html
    3.保存并查看 Loam 的三维点云地图https://blog.csdn.net/qq_36396941/article/details/83048415
    4. A-LOAM的github官方安装方法
    https://github.com/HKUST-Aerial-Robotics/A-LOAM
    5. Ceres安装报错“找不到要求版本3.3的Eigen3”解决方法https://www.cnblogs.com/didada/p/12305066.html
    6. ceres报错:Eigen3版本和ceres版本冲突问题
    https://zhuanlan.zhihu.com/p/149775218?from_voters_page=true
    7. LOAM的编译安装运行(安装PCL1.8)
    https://blog.csdn.net/weixin_43211438/article/details/87818526
    8. LeGO-LOAM的github官方安装方法
    https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
    9. LeGO-LOAM初探:原理,安装和测试https://blog.csdn.net/learning_tortosie/article/details/86527542
    10. GTSAM和LeGO-LOAM安装调试错误与解决方案
    https://zhuanlan.zhihu.com/p/123446216
    11. 【激光雷达】3D激光雷达传感器建图:速腾聚创、velodyne建图过程总结https://blog.csdn.net/xingdou520/article/details/85098314
    12. LeGO-LOAM和LOAM的区别与联系
    https://zhuanlan.zhihu.com/p/115986186

    1. robosense16多线雷达配置

    1.1 安装驱动源码

    $ mkdir –p ~/catkin_rslidar/src
    $ cd catkin_rslidar/src
    $ git clone https://github.com/RoboSense-LiDAR/ros_rslidar
    

    1.2 安装 libpcap-dev

    $ sudo apt-get install libpcap-dev
    

    1.3 更改源码属性

    $ cd ros_rslidar/rslidar_drvier
    $ chmod 777 cfg/*
    $ cd ..
    $ cd rslidar_pointcloud
    $ chmod 777 cfg/*
    

    1.4 catkin make

    1.5 将自己的工控机与雷达连接,修改 ip

    通过ifconfig 查看网口名字,enp4s0 是本人的工控机的网口名字。
    将本地 ip 地址改为 192.168.1.102,子网掩码设置为 255.255.255.0:

    $ sudo ifconfig enp4s0 192.168.1.102 netmask 255.255.255.0
    

    1.6 测试

    $ cd ~/catkin_rslidar
    $ source devel/setup.bash
    $ roslaunch rslidar_pointcloud rs_lidar_16.launch
    

    看到多线雷达的画面:
    在这里插入图片描述

    2. LOAM 安装和测试

    2.1 安装 loam_velodyne

    $ cd catkin_rslidar/src
    $ git clone https://github.com/laboshinl/loam_velodyne.git
    $ cd ..
    $ catkin_make
    

    其中,我遇到了问题catkin_make 运行出错:
    在这里插入图片描述
    查到解决方案如下:
    在这里插入图片描述

    2.2 rslidar 跑 loam 的准备

    进入 launch 目录

    $ roscd loam_velodyne/launch/
    

    增加 loam_rslidar.launch(内容见附录),由于未使用imu,去掉了两句 imu 数据的映射。

    <launch>
      <node pkg="tf" type="static_transform_publisher" name="rslidarlink_broadcaster" args="0.0 0.0 0.42 0.0 0.0 0.0 base_link laser 50"/>
    
      <arg name="rviz" default="false" />
      <arg name="scanPeriod" default="0.1" />
      <arg name="lidartype" default="VLP-16" /> <!-- options: VLP-16  HDL-32  HDL-64E RS-32 RS-16-->
    
      <node pkg="loam_velodyne" type="multiScanRegistration" name="multiScanRegistration" output="screen">
        <param name="lidar" value="$(arg lidartype)" /> <!-- options: VLP-16  HDL-32  HDL-64E RS-32 RS-16 -->
        <param name="scanPeriod" value="$(arg scanPeriod)" />
        <remap from="/multi_scan_points" to="/rslidar_points" />
        <!--remap from="/imu/data"  to="/mobile_base/sensors/imu_data" /-->
      </node>
    
      <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
        <param name="scanPeriod" value="$(arg scanPeriod)" />
      </node>
    
      <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
        <param name="scanPeriod" value="$(arg scanPeriod)" />
        <!--remap from="/imu/data"  to="/mobile_base/sensors/imu_data" /-->
      </node>
    
      <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
      </node>
    
      <group if="$(arg rviz)">
        <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
      </group>
    </launch>
    

    2.3 LOAM实际运行过程

    loam_velodyne 实时建图:
    修改雷达 IP:

    $ sudo ifconfig enp4s0 192.168.1.102 netmask 255.255.255.0
    

    启动 rslidar 雷达

    $ roslaunch rslidar_pointcloud rs_lidar_16.launch
    

    新终端,启动 loam_velodyne 建图和 Rviz

    $ roslaunch loam_velodyne loam_rslidar.launch
    

    在这里插入图片描述启动底盘

    $ roslaunch turtlebot_bringup minimal.launch
    

    启动键盘控制建图

    $ roslaunch turtlebot_teleop keyboard_teleop.launch
    

    建图效果如下:
    在这里插入图片描述

    2.4 保存并查看 loam 的三维点云地图

    在建图过程中执行, 来录制 loam 后生成的地图。点云话题是 laser_cloud_surround,
    录好之后 ctrl+c 结束, mybag.bag 是录好之后的包的名字。

    $ rosbag record -o mybag.bag out /laser_cloud_surround
    

    将 bag 文件转换成 pcd 文件:

    $ rosrun pcl_ros bag_to_pcd mybag.bag /laser_cloud_surround mypcd
    

    mypcd 是转换成 pcd 的文件夹名字。获取的 pcd 文件是很多个,每一个 pcd 是一
    帧数据。可以通过 pcl_viewer 命令查看 pcd 格式的最后一个文件:

    $ pcl_viewer yourname.pcd
    

    在这里插入图片描述
    loam是三维slam中最基础的算法。实际运行过程中,小车转弯时,构建的地图容易翻转,效果不是很好。

    3. A-LOAM 安装和测试

    3.1 Ceres Solver

    github上安装A-LOAM步骤:(These instructions are for Ubuntu 18.04 and newer. On Ubuntu 16.04 you need to manually get a more recent version of Eigen, such as 3.3.7.
    https://github.com/HKUST-Aerial-Robotics/A-LOAM
    注释中强调他的步骤是针对Ubuntu 18.04及更新版本,由于我的Ubuntu是16.04。按照此步骤进行,安装Eigen3.3报错:Ceres安装报错“找不到要求版本3.3的Eigen3”,查找解决方案,可直接安装Eigen3.3.7:(或遇到此错误按照此方法卸载重装3.3.7)
    https://www.cnblogs.com/didada/p/12305066.html
    按步骤安装了ceres2.0.0,ceres报错:Eigen3版本和ceres版本冲突问题,查找解决方案,直接安装ceres1.14.0(或遇到此错误按照此方法卸载重装1.14.0)
    https://zhuanlan.zhihu.com/p/149775218?from_voters_page=true

    3.2 PCL

    github步骤中pcl点击失效,如若PCL1.7不能运行,可卸载PCL1.7,安装PCL1.8。(我用第二台工控机安装的时候,直接就可以运行)
    安装PCL1.8可参考:https://blog.csdn.net/weixin_43211438/article/details/87818526

    3.3 A-LOAM实际运行

    $ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
    

    运行此指令便可看到效果,但是我的却报错,如遇到此情况
    在这里插入图片描述
    找不到两个包,用直接运行cpp方式运行其余两个节点:

    $ cd turbot_ws/devel/lib/aloam_velodyne
    $ ./alaserMapping
    $ ./alaserOdometry
    

    在这里插入图片描述
    保存并查看 A-LOAM 的三维点云地图,点云相对稠密:
    在这里插入图片描述
    A-LOAM可以生成robot运动轨迹:

    $ rostopic echo /aft_mapped_path
    

    将位姿输出成aloam_pose.txt:

    $ rostopic echo /aft_mapped_path > aloam_pose.txt
    

    在这里插入图片描述
    轨迹在一个平面上。

    4. LeGO-LOAM 安装和测试

    4.1 安装gtsam并编译

    $ git clone https://bitbucket.org/gtborg/gtsam.git
    

    编译

    $ cd ~/gtsam
    $ mkdir build
    $ cd build
    $ cmake ..
    $ sudo make install(一定要加上sudo)
    

    4.2 下载并编译LeGO-LOAM

    $ mkdir -p ~/lego_loam_test/src
    $ cd ~/lego_loam_test/src
    $ git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
    $ cd ..
    $ catkin_make -j1
    

    当第一次编译代码时,需要在“catkin_make”后面添加“-j1”以生成一些消息类型。将来的编译不需要“-j1”。报错:
    在这里插入图片描述
    解决方案:
    将/usr/local/lib/cmake/GTSAM/GTSAMConfig.cmake:17 行的find_dependency改成find_package。具体操作:

    $ cd /usr/local/lib/cmake/GTSAM/
    $ sudo chmod a+w GTSAMConfig.cmake
    $ gedit GTSAMConfig.cmake
    

    重新运行

    $ cd ~/lego_loam_test
    $ catkin_make -j1
    

    4.3 数据集运行

    运行launch文件:(这个rviz界面确实是黑的,不要慌张)

    $ roslaunch lego_loam run.launch
    

    播放bag文件(数据集)

    $ rosbag play *.bag --clock --topic /velodyne_points
    

    4.4 LeGO-LOAM实际运行

    修改雷达发布的话题,convert.cc中output_points_topic由rslidar_points改为velodyne_points(rslidar16默认发布的 topic 为/rslidar_points,而LeGO-LOAM需要订阅的 topic 为/velodyne_points,这个问题耽误进度很久)。再改“utility.h”中的参数,即可。开启雷达后,运行lego-loam:

    $ roslaunch lego_loam run.launch
    

    在这里插入图片描述
    生成4个pcd文件:finalCloud.pcd,cornerMap.pcd,surfaceMap.pcd,trajectory.pcd。最终点云地图,点云相对少。
    在这里插入图片描述
    录制自己的数据包:

    $ rosbag record -O mybag.bag /velodyne_points
    

    在终端执行以下命令,可以打印pose:

    $ rostopic echo /aft_mapped_to_init
    

    在终端执行以下命令,可以将pose保存到pose.txt中:

    $ rostopic echo /aft_mapped_to_init > pose.txt
    

    5. 总结、对比:

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    图片均来自博客:https://zhuanlan.zhihu.com/p/115986186。

    LOAM进行了特征点提取和匹配,边缘点和平面点。
    缺点:不能处理大规模的旋转变换。

    A-LOAM是LOAM简化版本。
    A-LOAM没有提供IMU信息修正的接口,缺少对提取到的特征点的筛选;
    LOAM使用LM优化方法,A-LOAM则是使用了Ceres库完成;
    LOAM中解析地求出了雅可比的表达式,A-LOAM使用了Ceres提供的自动求导工具。

    LeGO-LOAM特点:轻量级,点云分割去除噪声,带有地面优化的实时6自由度估计,回环检测。
    增加了更多预处理步骤,包括地面的提取,和点云分割。使用更多筛选之后的点云再提取特征点;
    在提取特征点时,将点云分成小块,分别提取特征点,以保证特征点的均匀分布;
    一个双步骤的LM优化,先使用平面点优化高度,同时利用地面的方向优化两个角度信息;再使用边角点优化剩下的三个变量;
    不用的地图点存储方式。LOAM中将所有历史的点云存储到同一张点云图中。LeGO-LOAM则是分别存储每一帧的特征点信息和每一帧的位姿数据;
    适合室外场景。
    原理和代码还得认真学习和钻研,加油!如有错误,欢迎纠正。

    展开全文
  • Sfusion_loam 语义融合壤土。
  • 一、featureAssociation相关推导 1)帧间匹配雅可比矩阵推导 首先明确LEGO-LOAM中,运动坐标系(符合右手系)的设置为: 因此对于平面运动来说,影响最大的三个分量为 t x , t z , r y t_x,t_z,r_y tx​,tz​,ry​,...

    一、featureAssociation相关推导

    1)帧间匹配雅可比矩阵推导

    首先明确LEGO-LOAM中,运动坐标系(符合右手系)的设置为:
    在这里插入图片描述
    因此对于平面运动来说,影响最大的三个分量为 t x , t z , r y t_x,t_z,r_y tx,tz,ry,因此仅考虑这三个分量对雅可比矩阵,也就是对非线性优化问题的贡献。首先让我们考虑整个非线性优化过程,首先将本帧点 p p p转换至上一帧坐标系中,得到 p ′ p' p,即

    p ′ = T p p' = Tp p=Tp

    通过计算 p ′ p' p点和其近邻线(以corner为例,存在于上一帧点云系中)的点线间距离,得到残差 f f f,即

    f = d i s t ( p ′ , l i n e ) = d i s t ( T p , l i n e ) f = dist(p', line) = dist(Tp, line) f=dist(p,line)=dist(Tp,line)

    于是雅可比矩阵可以写为

    J = ∂ f ∂ T = ∂ ( d i s t ( T p , l i n e ) ) ∂ T J =\frac{{\partial f}}{{\partial T}} = \frac{{\partial (dist(Tp,line))}}{{\partial T}} J=Tf=T(dist(Tp,line))

    由于我们只关心三个分量,且每一帧包含多个点线配对,可以计算许多个残差,则雅可比矩阵写开以后是这个类型: J = J= J=
    在这里插入图片描述 f f f对这三个位姿量的求导,使用链式法则来计算,首先将残差使用这个位姿量转换过去的点 p ′ = ( x , y , z ) p'=(x,y,z) p=(x,y,z)进行求导(将本帧点转换至上一帧坐标系中的点),然后再使用被转换过去的点 p ′ = ( x , y , z ) p'=(x,y,z) p=(x,y,z)位姿量求导。也就是:

    ∂ f k ∂ t x = ∂ f k ∂ x ∂ x ∂ t x + ∂ f k ∂ y ∂ y ∂ t x + ∂ f k ∂ z ∂ z ∂ t x \frac{{\partial {f_k}}}{{\partial {t_x}}} = \frac{{\partial {f_k}}}{{\partial x}}\frac{{\partial x}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial y}}\frac{{\partial y}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial z}}\frac{{\partial z}}{{\partial {t_x}}} txfk=xfktxx+yfktxy+zfktxz

    每一项链式法则的求导分为两部分,在程序中分别在findCorrespondingCornerFeatures函数和calculateTransformationCorner函数中完成。之后我们再细说,在这里先推导 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} xfk ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} txx的具体形式,首先推导前半部分 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} xfk,根据公式(这里的公式复制自知乎某回答),点线距离为:
    在这里插入图片描述

    在这里插入图片描述
    其中,
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    令分母为:
    在这里插入图片描述
    接下来求 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} xfk,也就是这里的 ∂ d ε ∂ x \frac{{\partial {d_\varepsilon }}}{{\partial x}} xdε x x x即为公式里面的 x 0 x_0 x0

    ∂ d ε ∂ x 0 = 1 2 ( m 11 2 + m 22 2 + m 33 2 ) ( 2 m 11 ∂ m 11 ∂ x 0 + 2 m 22 ∂ m 22 ∂ x 0 ) l 12 = m 11 ∂ m 11 ∂ x 0 + m 22 ∂ m 22 ∂ x 0 l 12 m 11 2 + m 22 2 + m 33 2 \frac{{\partial {d_\varepsilon }}}{{\partial {x_0}}}{\rm{ = }}\frac{{\frac{1}{2}(\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} )(2{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + 2{m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}})}}{{{l_{12}}}}{\rm{ = }}\frac{{{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + {m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}}}}{{{l_{12}}\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} }} x0dε=l1221(m112+m222+m332 )(2m11x0m11+2m22x0m22)=l12m112+m222+m332 m11x0m11+m22x0m22

    对另两个位姿分量的就省略了,是同理的,这一块对应源码中的:

                    tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                    tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];
    
                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    float x1 = tripod1.x;
                    float y1 = tripod1.y;
                    float z1 = tripod1.z;
                    float x2 = tripod2.x;
                    float y2 = tripod2.y;
                    float z2 = tripod2.z;
    
                    float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
                    float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
                    float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));
    
                    float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);
    
                    float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
    
                    float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;
    
                    float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;
    
                    float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;
    
                    float ld2 = a012 / l12;
    

    接下来求链式法则的后面那部分,也就是 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} txx x x x指代的是经过变换矩阵变换,将本帧点转换至上一帧坐标系中的点的坐标。因此,首先需要明确旋转变换关系。LEGO-LOAM(LOAM)中的帧间坐标变换关系如下:
    在这里插入图片描述

    其中, p ′ = ( x , y , z ) p'=(x,y,z) p=(x,y,z)表示被转换到上一帧坐标系中的点, p = ( x c , y c , z c ) p=(x_c,y_c,z_c) p=(xc,yc,zc)表示本帧这个点。 R R R t x , t y , t z t_x,t_y,t_z tx,ty,tz分别代表旋转和平移变换关系,角度为本帧坐标系转了多少欧拉角能转到上一帧坐标系,象征着所有欧拉角加符号带入公式才能获得本帧坐标系点转换到上一帧坐标系中。至于LOAM为什么这样设定不清楚(有点拧巴)。

    在这里插入图片描述

    要求 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} txx,必须要明确R是什么形式。根据wiki上给出的公式(上图右边红色框),欧拉角(-rx,-ry,-rz)转换为旋转矩阵的公式为:

    这里之所以加负号,与欧拉角设定相关?

    因此易求 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} txx,仅看第一行第一列乘出来的东西就行,即:
    在这里插入图片描述
    化简可以得到

    2)高频率里程计位姿累积函数:integrateTransformation

        void integrateTransformation(){
            float rx, ry, rz, tx, ty, tz;
            AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                               -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);
    
            float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) 
                     - sin(rz) * (transformCur[4] - imuShiftFromStartY);
            float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) 
                     + cos(rz) * (transformCur[4] - imuShiftFromStartY);
            float z1 = transformCur[5] - imuShiftFromStartZ;
    
            float x2 = x1;
            float y2 = cos(rx) * y1 - sin(rx) * z1;
            float z2 = sin(rx) * y1 + cos(rx) * z1;
    
            tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
            ty = transformSum[4] - y2;
            tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
    
            PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                              imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
    
            transformSum[0] = rx;
            transformSum[1] = ry;
            transformSum[2] = rz;
            transformSum[3] = tx;
            transformSum[4] = ty;
            transformSum[5] = tz;
        }
    

    (1)AccumulateRotation函数:

    输入:

    • 上一帧粗配准的激光里程计的累积位姿(全局),仅取旋转部分欧拉角:transformSum[0], transformSum[1], transformSum[2]
    • 上一帧本帧间的位姿变化量(增量),仅取旋转部分欧拉角: -transformCur[0], -transformCur[1], -transformCur[2]。由于transformCur处于本帧坐标系,取负号表示从上一帧变换到本帧的欧拉角变化量。
    • 本帧粗配准的激光里程计的累积位姿,仅取旋转部分欧拉角,由以上两组量求得:rx, ry, rz

    在这里插入图片描述

    首先从位姿变换基础公式入手,根据wiki上给出的公式(上图画黄色框处),这里欧拉角(x,y,z)转换为旋转矩阵的公式为:
    在这里插入图片描述
    设上一帧全局位姿的旋转部分为 R 1 = ( x 1 , y 1 , z 1 ) R_1=(x_1, y_1, z_1) R1=(x1,y1,z1),本帧全局位姿的旋转部分为 R 2 = ( x 2 , y 2 , z 2 ) R_2=(x_2, y_2, z_2) R2=(x2,y2,z2),上一帧到本帧的旋转变化量 δ R = ( δ x , δ y , δ z ) \delta R=(\delta x, \delta y, \delta z) δR=(δx,δy,δz),于是根据姿态变换公式:

    R 2 = R 1 ⋅ δ R R_2=R_1·\delta R R2=R1δR

    根据上面欧拉角转旋转矩阵的通式,与待求欧拉角相关的 R 2 R_2 R2为:
    在这里插入图片描述
    观察到,第二行第三列直接与 x 2 x_2 x2相关,因此考虑先求 x 2 x_2 x2,即先求 R 2 ( 2 , 3 ) R_2^{(2,3)} R2(2,3),也就是 ( R 1 R_1 R1 δ R \delta R δR也按照上面通式转为旋转矩阵) R 1 R_1 R1的第二行乘以 δ R \delta R δR的第三列,即:
    在这里插入图片描述

    展开就能用来求 − s i n x 2 -sinx_2 sinx2,于是就有了AccumulateRotation函数内部的:

    float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
    ox = -asin(srx);
    

    接下来求 y 2 y_2 y2,可以看到:

    a r c t a n ( R 2 ( 1 , 3 ) / R 2 ( 2 , 3 ) ) = y 2 arctan(R_2^{(1,3)} / R_2^{(2,3)})=y_2 arctan(R2(1,3)/R2(2,3))=y2,同理,按照上述几行几列的思路写出公式,可得函数内部的:

     float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
                  + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
     float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
                  - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
     oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
    

    接下来求 z 2 z_2 z2,可以看到:
    a r c t a n ( R 2 ( 2 , 1 ) / R 2 ( 2 , 2 ) ) = z 2 arctan(R_2^{(2,1)} / R_2^{(2,2)})=z_2 arctan(R2(2,1)/R2(2,2))=z2,同理,按照上述几行几列的思路写出公式,可得函数内部的:

    float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
                 + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
    float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
                 - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
    oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
    

    接下来观察旋转部分,这里我们忽略IMU的使用,即imuShiftFromStartZ = imuShiftFromStartY = 0 (LEGO-LOAM的IMU听说也很鸡肋)。就有如下代码:

            float x1 = cos(rz) * (transformCur[3]) 
                     - sin(rz) * (transformCur[4]);
            float y1 = sin(rz) * (transformCur[3]) 
                     + cos(rz) * (transformCur[4]);
            float z1 = transformCur[5];
    
            float x2 = x1;
            float y2 = cos(rx) * y1 - sin(rx) * z1;
            float z2 = sin(rx) * y1 + cos(rx) * z1;
    
            tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
            ty = transformSum[4] - y2;
            tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
    

    可以这么理解,先使用rx,ry,rz相关的转换矩阵将参考系与世界坐标系同轴(平行),然后transformSum是位于世界坐标系的,因此可以直接做加法。这个原因主要是因为transformCur是存在于本帧坐标系的,推导公式也能推导出来上面式子。

    展开全文
  • LOAM, ALOAM, LegoLOAM, hdl graph slam比较

    千次阅读 多人点赞 2019-09-26 16:37:18
    A-LOAM LOAM: • LOAM use a new defined feature system (corner and flat point), for the detail see its article. • LOAM suppose linear motion within the scan swap (VLOAM further uses visual odometry to...

    LOAM

    LOAM:

    • LOAM使用了作者定义的特征点提取和匹配方法,主要去边角点和平面点。LOAM use a new defined feature system (corner and flat point), for the detail see its article.
    • LOAM假设每一次激光扫描过程中是匀速运动,并且用这个假设修正激光雷达数据的运动扭曲问题。在VLOAM中则是更进一步,使用视觉的里程计估计每一个扫描数据的运动。LOAM suppose linear motion within the scan swap (VLOAM further uses visual odometry to estimate it), and undistort the lidar points.
    • LOAM也有一个低频率调用的全局优化线程。

    A-LOAM

    ALOAM github page

    另外,下面的算法都使用hdl_graph_slam给到的室外数据集做了结果的测试,建模的图像如下所示。由于没有找到轨迹的真实值,没有对轨迹误差做比较分析。

    LOAM和ALOAM的区别(Difference LOAM vs A-LOAM):

    • LOAM中提供了使用IMU信息修正的接口, ALOAM中省略了这一块。LOAM has IMU refinement.
    • ALOAM中缺少了对提取到的特征点的筛选过程,具体可以参见LOAM的代码部分(对一些不好的边角点做了筛选)。Lack feature filter in A-LOAM.
    • LOAM中的优化LM方法是作者自己编写的,ALOAM则是使用了Ceres库完成这一部分。LOAM implies the LM solver itself. A-LOAM uses Ceres solver.
    • 补充上面的一点,LOAM中作者解析地求出了雅各比的表达式(其中使用了一些小技巧统一了不同特征点的残差函数的导数表达),ALOAM则是简单地使用了ceres提供的自动求导工具(这样可以节省开发的时间,得到了也是准确的解,但是运算时间会稍稍长些,具体可以参见ceres的官方文档)。LOAM use analytical derivatives for Jacobians, but A-LOAM uses the automatic derivatives offered by Ceres (which is exact solution but a little bit slower).
    • 相比于原本的LOAM, ALOAM的可读性更高,非常适合学习。尤其是雅各比的部分,LOAM原本的雅各比推导非常的难理解。ALOAM直接用自动求导,整个SLAM最复杂的运算就不需要推导了!

    Performance:
    • A-LOAM seems good,less redundant points.
    • but has more error in far edges.
    • LOAM method has no assumption of a consistent “floor”, that is better for our case.
    • A-LOAM has the same logical with LOAM, but its performance is much worse. ALOAM result
    下面是对ALOAM的三个线程的运行时间的分析(以下的运行测试都是再i5 9300 cpu上进行的),分别是scan registration线程,odometry线程,和mapping线程。其中mapping是进行全局优化的线程,另外两个线程则和实时性息息相关。
    在这里插入图片描述

    • ALOAM - scan refistration
      the maximum time is : 0.034434
      the mean of time is : 0.0148146394612
    • ALOAM - odometry
      the maximum time is : 0.027296
      the mean of time is : 0.0157431030928
    • ALOAM - mapping
      the maximum time is : 0.326849
      the mean of time is : 0.257764385093

    For one input scan, it takes 0.0305s in average. 对于一个新的扫描帧,需要大概0.03秒的处理时间,另外低频优化每次的耗时大概0.25秒(并不影响实时性)。

    LEGO LOAM

    lego LOAM

    vs LOAM:

    • Lego LOAM针对处理运算量做了优化,它的运算速度增加,同时并没有牺牲精度。Faster and similar accuracy as LOAM, and has a better global map visual effect.

    Difference LOAM:

    • LogoLOAM 增加了更多预处理的步骤,其中主要包括一个地面的提取(并没有假设地面是平面),和一个点云的分割。使用更多筛选之后的点云再提取特征点,效率会更高。Add segmentation before processing (gound extraction and image-based segmentation)
    • 在提取特征点时,将点云分成小块,分别提取特征点,以保证特征点的均匀分布。Sub-divide the range image before feature extraction → more evenly distributed features.
    • 再特征点匹配的时候,使用预处理得到的segmenation标签筛选,又提高了效率。Label match
    • 一个双步骤的LM优化,先使用平面点优化高度,同时利用地面的方向优化两个角度信息;再使用边角点优化剩下的三个变量。以这种方式分别优化,效率提升40%,但是并没有造成精度的损失(根据原文章所述)。Two step LM. Seperate the optimization based on different property of edge and planar points. Becomes faster while similar accuracy.
    • 不用的地图点存储方式。LOAM中将所有历史的点云存储到同一张点云图中,并做了grid sampling。Lego LOAM则是分别存储每一帧的特征点信息和每一帧的位姿数据。这样提供了两种全局优化方式,一种是仿照LOAM的方法;另一种是可以使用图优化理论。Difference map storage method, can use pose graph optimization and use loop closure.

    另外LeGO LOAM需要对激光设备的标定。应该是在预处理中,使用了将激光数据转化为range image的步骤中需要这些数据,只有正确的设定才能正确执行算法。
    在源代码中对Velodyne的一些设备都有完整的参数设置,但是对速腾(rslidar)并没有设置好,需要自己完成这方面的工作,才能使用速腾的设备运行算法。虽然话是这么说。。我之前也问题这个遇到过问题。但是我发现就用源代码提供的velodyne的参数就可以顺利运行了。

    对我来说,在这个室外场景的数据,我觉得LeGO LOAM是本文列举的几个算法中最好的。 它可以高速运行,有较高的精度,同时相比于LOAM可以加入回环优化。

    但是,在室内运行发现,LeGO LOAM很容易丢失误匹配,但是LOAM就好很多,即使有相对大的运动也能够正确的处理。

    Performance:
    • lego slam has the best result, error is small.
    • Flat plane has good look, achieve a dense map, while keep its consistence.

    lego loam result
    下面是LeGO LOAM的三个主要线程的处理时间的分布曲线。分别是image projection线程(将扫描帧投影到二维矩阵,并做一系列的预处理),feature association线程(与LOAM大概一致,特征点的提取和匹配),以及map optimization线程(低频激发的全局优化线程)。
    在这里插入图片描述

    • LEGO LOAM - image projection
      the maximum time is : 0.029819
      the mean of time is : 0.0123906096595
    • LEGO LOAM - feature association
      the maximum time is : 1.226773
      the mean of time is : 0.0126770831335
    • LEGO LOAM - map optimization
      the maximum time is : 0.427468
      the mean of time is : 0.30585172524

    For one input scan, it takes 0.0249s in average. It is about 25% faster than ALOAM. 对于每一个输入的扫描帧,处理时间大约是0.025秒,相对于ALOAM快了25%左右。另外由于在LeGO LOAM中使用了另外的地图结构和优化方式(也加入了回环优化),所以map optimization的处理时间稍长一些,大概是0.3秒一次。

    HDL GRAPH SLAM

    hdl graph slam

    hdl_graph_slam:

    • 它是一个简单的图优化模型。 It is basically a graph optimization algorithm.
    • 它提供了ICP为基础的和NDT为基础的一系列点云标配方法。Use ICP-based or NDT-based methods to register new point cloud, and match candidates of loop closure.
    • 它假设有一个共享的地面,但是假设了地面是一个平面,这个假设可能过于强了,限制了算法的鲁棒性。
    • 在全局图优化的步骤,只是使用了相邻两帧的相对位姿和每一帧检测到的地面信息。其实使用的信息很少,图优化的结构也十分简单。For the graph optimization part, it use the most sample edge for consecutive frames, along with the floor observation edge.
    • 它提供了回环优化的模块,但是回环比较粗糙。简单来说,就是在当前帧地附近搜索历史地每一帧,分别标配得到可能的回环,再进行优化。
    • 总的来说,它提出了使用图优化来处理激光SLAM,这是很好的想法。In summary, it uses the most basic algorithms, however it has a complete structure.

    Performance:
    • not that much error for the far points, as it has loop closure
    • lots of redundant points as it has no optimization on point cloud, floors and walls are very thick in the global map.
    在这里插入图片描述
    HDL graph slam有四个主要线程,对于点云的预处理降采样prefiltering线程,floor detection线程(检测一个共有的平面作为地面),odometry线程(在测试中使用的是使用openmp加速的NDT算法),和graph optimization线程(优化包括:相邻帧的约束,回环约束,和每一帧检测到的地面约束)。
    在这里插入图片描述

    • HDL - prefiltering
      the maximum time is : 0.395365
      the mean of time is : 0.00943357786885
    • HDL - floor detection
      the maximum time is : 0.856617
      the mean of time is : 0.0456638586777
    • HDL - odometry
      the maximum time is : 0.309964
      the mean of time is : 0.0742533234078
    • HDL - graph slam
      the maximum time is : 2.140327
      the mean of time is : 0.13695704878

    Its processing time is much more than the other two methods, as it use NDT, while the other use feature points. 相比于上面的LOAM为基础的特征点标配方法,使用NDT(openmp加速过的NDT)仍然慢了很多,NDT大概需要0.07秒/帧。在全局优化的线程中,由于约束只是简单的相对位姿(帧与帧的相对位姿和帧与地面的相对位姿)所以graph optimization线程速度很快,只需要LOAM衍生算法的一半左右的时间。

    展开全文
  • 作者丨yc zhang@知乎来源丨https://zhuanlan.zhihu.com/p/111388877编辑丨3D视觉工坊LOAM作为比较古老的激光匹配slam方法,一直以来都霸占着...

    作者丨yc zhang@知乎

    来源丨https://zhuanlan.zhihu.com/p/111388877

    编辑丨3D视觉工坊

    LOAM作为比较古老的激光匹配slam方法,一直以来都霸占着KITTI的前列,近些年来,依靠LOAM框架也产出了很多文章,理解了LOAM,就可以很好的理解LOAM系列的其他文章。
    因此,我决定将重新整理一下LOAM的论文和代码,方便初入门的同学更好的理解LOAM算法。
    论文:https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf

    代码:

    https://github.com/HKUST-Aerial-Robotics/A-LOAM
    文章较长,书写不易,如果觉得对您有帮助的话,希望可以点赞收藏支持一下哈~
    1、论文概览

    LOAM这篇论文是发表于2014年RSS的文章,全称为:LOAM: Lidar Odometry and Mapping in Real-time . LOAM是基于激光雷达而搭建的在ROS平台下的SLAM系统,一共分为四个部分:

    本文的核心主要在于两个部分:特征提取(Lidar Registration)和里程计解算(Odometry and Mapping)。
    当提取出特征后,通过两个高频率的里程计(Odometry)实现粗定位和低频率的里程计(Mapping)实现精定位。下面,我们将结合论文和代码,进行LOAM框架的详细讲解。
    2、符号设定

    一个sweep代表了一次扫描周期,记作 

    一个扫描周期内获取到的所有点云记作  代表了第k个扫描周期的点云

    雷达坐标系设定为  代表了第k个周期时的雷达观测坐标系,  在  中可以被表示为 

    全局坐标系设定为  代表了第k个周期时的全局坐标系,  在  中可以被表示为 

    因此,我们就可以将整个问题转化为:
    已知一段点云序列  ,计算在前  个时期内的雷达位姿以及构建全局地图。
    3、Lidar Registration
    为了计算雷达的运动位姿,我们需要得到的是相邻帧间的姿态变换关系,这样才能继续往后走下去。为了获取到相邻帧的姿态变换,使用全部点云处理是不可靠的,为了减少计算的时间消耗,一般需要使用特征点来代替完整的数据帧。
    常见的特征点提取方法:特征向量、直方图、旋转图片等。
    这些方法虽然能很精准的涵盖一帧数据的大部分信息,但是由于计算量大,很难在激光slam的相邻帧的匹配中使用。因此,需要想一些更好的方法。
    本文作者根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标。

    这种提取方法就是通过计算一个集合  中的所有点之间的关系来判断到底点  是属于平面点还是边缘点。具体到代码的实现就是使用计算当前点和前后五个点,来得到对应的平滑度数据,计算量减少了很多。

    再得到平滑度这一指标后,可以将特征点划分为两大类:平面点和边缘点。

    平面点:在三维空间中处于平滑平面上的点,其和周围点的大小差距不大,曲率较低,平滑度较低。

    边缘点:在三维空间中处于尖锐边缘上的点,其和周围点的大小差距较大,曲率较高,平滑度较高。

    我们对集合内的点进行排序,找到最小的点c作为平面点,最大的点c作为边缘点。
    这样就可以在一帧中得到有效的点数了。而在论文中是对整个扫描进行化段,分四段,每段各取两个边缘点和4个平面点。而在A-LOAM的代码中则是进行了一下的实现:

    同时,对所取的点也有一定的限制:

    该点的周围的点尽量不要被再被取到,这样可以使整体的特征点分布更加的平均

    该点不能与雷达扫描束过于平行,这样也会出现问题

    具体样例如下图所示:

    因此,选点就有了三要素:

    1、不能超过设定的size,每个集合平面点4个,边缘点2个;

    2、已选取的点周围不能有点,使得点可以分布的更加均匀;

    3、选取的平面点不能与激光扫描束平行。

    以上的对应关系,同样再代码中有着体现。
    判断该点是否是之前选取的点的周围的点以及不能超过size:

    通过上面的循环,就实现了边缘点(Corner)和平面点(Planar)的获取了。这样就可以得到一帧数据对应的特征信息了。
    提取后的数据如图所示:

    这样就可以在整个三维空间内,将平面点和边缘点作为特征点提取出来来代替整个数据了。
    4、Lidar Odometry
    再提取了特征点之后,我们需要做的就是特征匹配了。这里使用的使scan-to-scan的方法来实现帧与帧之间的特征匹配。
    已知第  次扫描的点云为  ,而提取的边缘点集合记为:  ,提取的平面点记为  。
    已知第  次扫描的点云为  ,而提取的边缘点集合记为:  ,提取的平面点记为  。
    我们想要得到的是  和  之间的变换关系,也就是  和  以及  和  之间的关系。
    由于雷达自身在  和 时刻过程中是运动的,所以,我们每个点对应的姿态变换矩阵都应该得到一定的修正。为了方便处理,我们将所有的点重投影到每一帧的初始时刻,这样在这一帧中的所有点都可以得到对应的姿态变换信息。
    我们将重投影到每一帧初始时刻的平面点和边缘点记为:  和  。
    这样的话就可以进行后续的优化了。
    我们知道平面和边缘线是两种不同的特征,那么在LOAM的计算中,也是分开进行计算的。
    4.1、边缘点匹配
    已知信息:  和  。
    我们知道,边缘点就是三维结构中线所构成的点,这样的话,就是求点到线的最近距离。需要在  中找到一条线来求解最近距离:

    雷达扫描束

    三个点在两帧之间的直观展示

    构建的几何约束关系

    从  中选取一个点  ,在  中选取与  最近的点  ,以及在  中选取和点  相邻扫描线中最近的点  ,这样的目的是防止  三点共线而无法构成三角形。
    选取最近点的算法使用的是kd-tree的最近邻搜索,如果对kd-tree感兴趣的同学,可以阅读这篇文章:

    https://zhuanlan.zhihu.com/p/112246942

    因此,选取了三个点: {  },坐标分别记为:  ,  和  。
    这样,就将姿态变换转换为了,点  到线  的最短距离了。
    因此,就变为了:

    边缘点约束公式

    我们知道,分子叉乘球出来的是  和  构成的平行四边形的面积,而分母则是  构成的底,这样的话,就可以通过基础的数学知识,得到高  。
    因此,我们就构建了边缘点的优化方程。
    4.2、平面点匹配
    已知信息  和  。
    平面点的匹配起始和边缘点的匹配类似,同样的是寻找两帧之间的对应关系,我们知道平面点的话,就是要求点到平面的距离,这样的话,就需要在  中找到一个对应的平面。

    雷达扫描束

    四个点在两帧之间的直观展示

    构建的几何约束关系

    从  中寻找一个点  ,从  中找寻与点  最近的点  ,并找到与点  相同激光扫描束的最近点  ,然后,找寻相邻帧中与点  最相近的点  ,这样的话,就可以找到一个不共线的,能构成一个平面的三个点。
    因此,选取了四个点:{  },坐标分别记为:  和  。
    这样的话,就变成了点  到平面  之间的最近距离了。
    因此,就变为了:

    平面点约束公式

    故,分子为构成的三维物体的体积,分母为地面构成的平行四边形的面积,则高  。
    因此,我们就得到了平面点的优化方程。
    4.3、姿态解算
    当获取到了  和  之后,我们需要做的就是求解公式(2)和(3)中的右边的部分,最小化右边部分,就得到了最小化的 和 ,这样就可以使用非线性优化的方法来进行求解 了。
    我们首先列出已知的信息:

    由于考虑了雷达的自身运动是匀速运动,所以,我们获取了每个点的时间戳信息,并使用线性方程,得到每个时刻对应的姿态变换矩阵,这一步主要是进行了运动补偿。
    我们首先求解第  帧中第个点的姿态变换信息:

    这样的话,就可以使用每个点对应的姿态变换矩阵放入进去进行后续的求解了。
    但是,这里论文使用的是6-DoF的表示,也就是  。
    所以,我们需要想办法从6-DoF得到对应的姿态变换矩阵,进行迭代优化求解。
    我们设定旋转矩阵  和平移矩阵  。
    这样的话,就可以构建公式:

    这样,就可以将  帧上的点投到k帧所在的集合中了。
    这里可能不是很好理解。
    这里就涉及到论文中所涉及到的几个符号的表示,建议可以对照论文当中来看。

    核心思想就是要找到一个姿态变换使得可以将公式(2)和(3)中的右边部分进行化简计算,而我们知道{  },所以,需要将  转化到同样的帧中,统一坐标系,进行后面的解算。

    回归正文,我们获得了得到了公式之后,就需要求解  和  了。
    然而,我们使用的是6-DoF的表示,就需要想办法进行欧拉角到旋转矩阵的变换,使用罗德里格斯公式进行变换:

    欧拉角转旋转矩阵

    PS:这里使用的是旋转向量来表示,而不是欧拉角。但是在原始的loam-velodyne版本中,我们使用的是欧拉角来进行后续的计算。而在后面的A-LOAM版本使用的是四元数来求解。也算是避开了原版的一个坑吧。
    这样,就可以将欧拉角转换为旋转矩阵了,其中  。
    而,平移矩阵就是6-DoF的前三位,也就是说  。
    这样,就可以实现6-DoF和旋转矩阵的变动了。
    那么我们可以知道在公式(2)和公式(3)中,只有  和  是未知的,其他都是已知的。那么就可以得到一个约束公式:

    因此,可以统一为一个公式:

    因此,我们就需要求解优化这个非线性优化问题就可以了。
    那么使用常规的列文伯格-马夸特法(LM)来进行求解:
    如果对最小二乘的求法感兴趣的话,我在这里总结了一些SLAM中最小二乘的求解方法:https://zhuanlan.zhihu.com/p/113946848

    和高斯牛顿法不一样的是,我们加入了信赖区域,  为半径,  为系数矩阵,最小二乘的函数也不相同:

    构建拉格朗日函数,  是系数因子:

    这样的话,化简后求导就可以得到:

    我们化简后得到:

    而 
    故,我们可以得到导数为:

    我们可以看到,和论文中略有不同,这个和初始雅可比矩阵  以及系数矩阵  有关,实际使用中,系数矩阵D,通常是用  来表示的,这样的话,我们就得到了其微分量  。
    代入梯度下降的公式为:


    和论文中基本一致,就是表现的方式有一点点的区别。
    不断求解上面这个式子,直到收敛即可。
    代码使用的是ceres solver来进行求解。
    首先设定损失函数:

    然后设定代价函数:

    最后是求解:

    然后对球出来的数据进行迭代更新:

    这样就可以实现优化求解了。
    A-LOAM的代码相比于LOAM-Velodyne是使用了现有的优化求解库,这样可以使代码更加简介和简单,只需要输入边缘点对应关系的{  }和平面点的{  },这样的话,就可以优化求解了。
    然而,需要注意的是:
    这里求解的还是局部雷达观测坐标系下的结果,是为了求解相邻帧之间的变换,也就是  ,而为了定位和建图,需要求解的是全局坐标系下的变换,也就是  。因此,需要进入到下一个章节Lidar Mapping中。
    5、Lidar Mapping
    当我们获取了若干相邻帧的姿态变换信息后,我们需要做的就是将其和全局地图进行匹配,并将其加入到全局地图之中。

    mapping匹配示意图

    这里设定:

    第  帧之前的扫描点云在全局坐标系下的投影为  ;

    第  次扫描的末位,也就是  帧的起始时的姿态变换信息  。

    利用Odometry的输出  ,将  从  时刻的起始推演到  时刻的起始,得到姿态矩阵  。

    通过  ,将之前第  帧的点云投影到全局坐标系下,记为  。

    这样,其实就很明晰了,我们需要做的就是优化求解  。
    这里的已知信息为:{  },不精准的  。想要优化得到精准的  。
    同样是两个点云,求精准的姿态变换,这里和之前的Lidar Odometry部分很接近,所以,使用的算法基本相同。
    不过,我们这里是map-to-map的算法,所以,使用的  是10帧Odometry输出的数据,  是之前的地图数据。
    如果使用全部的地图数据,在计算效率上会大打折扣,所以,这里使用的是一个边长为10m的立方体,用以代替全局地图,优化得到最终的姿态变换矩阵  。
    选取特征点的方法是一样的,不过具体实现的方式不尽相同。
    将  和  中相交的存入到KD-tree中,这里的相交部分,也就是判定是否处于这个cube中。相交的部分属于了两个map之间的重合部分,可以用来作为点云匹配的依据。
    在这里,首先选取相邻点集合  ,针对平面点和边缘点又有两种处理方法:

    1、平面点:  只保留平面特征点;

    2、边缘点:  只保留边缘特征点。

    计算  的协方差矩阵,记为  ,  的特征值记为  ,特征向量记为  。
    如果  分布在一条线段上,那么  中一个特征值就会明显比其他两个大,  中与较大特征值相对应的特征向量代表边缘线的方向。(一大两小,大方向)
    如果  分布在一块平面上,那么  中一个特征值就会明显比其他两个小, 中与较小特征值相对应的特征向量代表平面片的方向。(一小两大,小方向)
    边缘线或平面块的位置通过穿过  的几何中心来确定。
    经过和评论区的同学讨论,发现这个地方没有描述的很清楚,这里可以看一个动图(感谢CSDN博主robinvista同意我使用他的图片):

    特征向量表示整个平面

    可以清楚地看到,特征向量的长度反应了点的分布,也就是说我们根据特征值和特征向量就能计算出直线的方向。平面也是同理,我们可以根据两个较长的特征向量计算平面的法向量,三个向量相交于几何中心,这样平面就确定了。
    通过这种方法就可以快速的确定对应的边缘线和平面了。
    这样就可以快速的找到  中的一个点  ,和  中的边缘点{  }以及平面点{  }。
    这样就可以使用公式(2)和(3),利用LM法来求解  了。
    这里需要注意的是除了由于实时性的缘故,找对应的特征点更换了方法,Lidar Mapping其他的算法步骤和Lidar Odometry 的基本一致。
    这里需要注意的是,Lidar Odometry中使用过运动补偿了,这里的点云就都被设置为对应帧的时间戳,就不用再考虑运动补偿的事情了。
    后续可以通过VoxelFilter来进行降噪,减少点云的数量。
    整个的计算流程可以表示如下:

    Lidar Mapping算法更新流程

    通过不断的处理,就可以得到对应的了。
    对应的代码解析如下:
    对cube里的数据进行处理:

    接受Lidar Odometry的数据,并得到初始的姿态变换矩阵:

    经过计算后,得到的新的姿态信息:

    在这里,需要注意的是,我们最终传递的并不是q_wmap_odom和t_wmapodom。最终传递的是优化计算后得到的t_w_curr和q_w_curr:

    Lidar Mapping节点publish的message

    6、Transform Integration
    这一部主要是将Lidar Odometry中得到的姿态信息和Lidar Mapping中得到的信息全部都放入到rviz中,方便观看和处理。如果是为了使用LOAM作为前端的话,到Lidar Mapping就完全够用了。
    7、总结:
    LOAM作为常见霸占KITTI榜的激光SLAM算法,是有着自己的独特优势的。其优缺点如下:
    优点:

    新颖的特征提取方式(边缘点和平面点)

    运动补偿(时间戳)

    融合了scan-to-scan(odometry)和map-to-map(mapping)的思想

    缺点:

    没有后端优化(年代)

    不能处理大规模的旋转变换(旋转向量的求解)

    LOAM的整理到这里就结束了。LOAM由于发表时间较早,并没有进行后端优化。不过,他的后继者:LeGO-LOAM就很好的解决了这个问题。

    https://zhuanlan.zhihu.com/p/115986186

    在LeGO-LOAM这篇文章中,我详细的阐述了LOAM和LeGO-LOAM的区别与联系,大家可以根据自己的实际需求选择对应的算法来实现想要的功能。

    一路看下来,想必也不容易,如果觉得本文对您有帮助的话,希望可以点赞收藏关注支持一下哈~

    本文仅做学术分享,如有侵权,请联系删文。

    下载1

    在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

    下载2

    在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

    下载3

    在「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

    重磅!3DCVer-学术论文写作投稿 交流群已成立

    扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

    同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

    一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

    ▲长按加微信群或投稿

    ▲长按关注公众号

    3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列三维点云系列结构光系列手眼标定相机标定orb-slam3等视频课程)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近2000星球成员为创造更好的AI世界共同进步,知识星球入口:

    学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

     圈里有高质量教程资料、可答疑解惑、助你高效解决问题

    觉得有用,麻烦给个赞和在看~  

    展开全文
  • rslidar LOAM系列建图

    2021-10-08 20:16:29
    1 LOAM建图 1.1 工作空间建立 mkdir -p catkin_jt/src cd catkin_jt/src catkin_init_workspace 1.2 源码下载及编译 cd ~/catkin_jt/src # catkin_jt自己的ROS工作空间。 git clone ...
  • loam 介绍 固态雷达和机械雷达的区别 筛掉不好的点 位姿的迭代估计 外点和动态点的过滤 回环检测 参考链接 loam: lego-loam: cartographer 3D: lio-sam: lvi-sam: livox-loam: PPT: 从2D到3D——Cartographer ...
  • loam论文的理解.pptx

    2019-10-18 16:25:22
    对于论文loam的理解,作者提出了使用六自由度的两轴激光雷达来构建实时激光里程计。
  • A-LOAM-devel.zip

    2020-07-26 09:12:07
    基于Velodyne激光雷达扫描得到的点云数据,进行SLAM,使用Eigen和Cero库简化计算,使用平台基于ROS
  • 学习LOAM笔记——特征点提取与匹配学习LOAM笔记——特征点提取与匹配1. 特征点提取1.1 对激光点按线束分类1.2 计算激光点曲率1.3 根据曲率提取特征点2. 特征点匹配2.1 scan-to-scan中的特征点匹配2.2 scan-to-map中...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 3,712
精华内容 1,484
关键字:

LOAM

友情链接: ucos3.rar