技术标签: SLAM 3D激光SLAM源码解析系列
hdl_graph_slam是由日本风桥科技大学的Kenji Koide在github上开源的六自由度三维激光SLAM算法。主要由激光里程计、回环检测以及后端图优化构成,同时融合了IMU、GPS以及地面检测的信息作为图的额外约束。其算法流程图如下所示:
从图中可以看出,算法首先读入激光雷达的点云数据,然后将原始的点云数据进行预滤波,经过滤波后的数据分别给到点云匹配里程计以及地面检测节点,两个节点分别计算连续两帧的相对运动和检测到的地面的参数,并将这两种消息送到hdl_graph_slam节点进行位姿图(pose graph)的更新以及回环检测,并发布地图的点云数据。下图是利用该算法重建的3D环境点云效果图:
从上图可以看出该算法的建图效果很赞,且在github上开源了质量较高的代码,唯一美中不足的是对应的论文还处于review状态,且据笔者了解,目前网络上也还没有关于该算法的详细解读,因此只能通过阅读源码来学习理解该算法。接下来笔者将按照自己的理解分别对该算法的4个主要部分进行较为详细地介绍。
3D激光雷达每一帧都会产生大量的点云数据,以velodyne HDL-32E为例,大约每帧会返回包含7万个点的点云数据,同时激光雷达的测量原理又决定了这部分点云密度并不一致,近处密度过大有一些冗余的点云,而过远处的地方点云又很稀疏,无法用来进行有效地感知。另一方面,当激光雷达在较细小物体上扫描或产生错误测量时,会产生明显的稀疏离群点,这部分稀疏的离群点首先无法进行有效的感知,其次会对点云局部形态的估计例如法向量和曲率产生很大影响,因此这一部分主要是对激光雷达产生的原始点云数据进行下采样和野值点滤除。
从原作者的源程序中可以看出,其预滤波部分的源码主要集中在这三个函数里面:
pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);
filtered = downsample(filtered);
filtered = outlier_removal(filtered);
其中,distance_filter函数用来滤除距离过远的稀疏点云,downsample函数用来对距离滤波后的点云进行下采样,outlier_removal函数用来滤除野值点。
这部分代码比较简单,即:遍历原始点云cloud,若某一点对应的欧式距离distance_near_thresh大于且小于distance_far_thresh,则压入点云filtered,时间复杂度为 O ( N ) O(N) O(N)。
std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points),
[&](const PointT& p) {
double d = p.getVector3fMap().norm();
return d > distance_near_thresh && d < distance_far_thresh;
}
);
这部分代码原作者采用了pcl中下采样的两种方法:VoxelGrid和ApproximateVoxelGrid,并提供了不同的接口,只需要在.launch文件中修改对应的选项,即可在两种方法中切换。
如下代码完成了是根据.launch文件的不同设置生成对应的下采样滤波器:
if(downsample_method == "VOXELGRID") {
std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
downsample_filter = voxelgrid;
} else if(downsample_method == "APPROX_VOXELGRID") {
std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
boost::shared_ptr<pcl::ApproximateVoxelGrid<PointT>> approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
downsample_filter = approx_voxelgrid;
} else {
std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
}
如下是根据选择的下采样滤波器对点云进行下采样滤波:
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
downsample_filter->setInputCloud(cloud);
downsample_filter->filter(*filtered);
由于上面的代码比较简单,因此接下来着重介绍一下pcl中两种下采样方法的基本原理,具体源码可参见此链接。
这一部分主要负责将野值点滤除,原作者同样采用了pcl中的两种野值点滤出方法:StatisticalOutlierRemoval和RadiusOutlierRemoval,并提供了相应的接口,同样在.launch文件中修改对应的属性即可选择不同的滤波方法。
如下代码是根据.launch文件中不同的属性修改滤波器选择:
if(outlier_removal_method == "STATISTICAL") {
int mean_k = private_nh.param<int>("statistical_mean_k", 20);
double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);
std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;
pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());
sor->setMeanK(mean_k);
sor->setStddevMulThresh(stddev_mul_thresh);
outlier_removal_filter = sor;
} else if(outlier_removal_method == "RADIUS") {
double radius = private_nh.param<double>("radius_radius", 0.8);
int min_neighbors = private_nh.param<int>("radus_min_neighbors", 2);
std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;
pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
rad->setRadiusSearch(radius);
rad->setMinNeighborsInRadius(min_neighbors);
} else {
std::cout << "outlier_removal: NONE" << std::endl;
}
如下是根据选择的野值点滤出滤波器对点云进行处理:
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
outlier_removal_filter->setInputCloud(cloud);
outlier_removal_filter->filter(*filtered);
同样,接下来简单介绍一下pcl中两种野值点滤波器的工作原理:
如下图是对原始的输入点云进行prefilter的结果,其中左侧是滤波后的结果,右侧是原始点云,可以发现距离过远的稀疏点云、野值点都别滤出。同时相比于原始点云,滤波后的点云变得更为稀疏。
本节就先更到这里,下节会主要对scan_matching_odometry部分进行解析。
文章浏览阅读3.8k次,点赞9次,收藏28次。直接上一个工作中碰到的问题,另外一个系统开启多线程调用我这边的接口,然后我这边会开启多线程批量查询第三方接口并且返回给调用方。使用的是两三年前别人遗留下来的方法,放到线上后发现确实是可以正常取到结果,但是一旦调用,CPU占用就直接100%(部署环境是win server服务器)。因此查看了下相关的老代码并使用JProfiler查看发现是在某个while循环的时候有问题。具体项目代码就不贴了,类似于下面这段代码。while(flag) {//your code;}这里的flag._main函数使用while(1)循环cpu占用99
文章浏览阅读347次。idea shift f6 快捷键无效_idea shift +f6快捷键不生效
文章浏览阅读135次。Ecmacript 中没有DOM 和 BOM核心模块Node为JavaScript提供了很多服务器级别,这些API绝大多数都被包装到了一个具名和核心模块中了,例如文件操作的 fs 核心模块 ,http服务构建的http 模块 path 路径操作模块 os 操作系统信息模块// 用来获取机器信息的var os = require('os')// 用来操作路径的var path = require('path')// 获取当前机器的 CPU 信息console.log(os.cpus._node模块中有很多核心模块,以下不属于核心模块,使用时需下载的是
文章浏览阅读10w+次,点赞435次,收藏3.4k次。SPSS 22 下载安装过程7.6 方差分析与回归分析的SPSS实现7.6.1 SPSS软件概述1 SPSS版本与安装2 SPSS界面3 SPSS特点4 SPSS数据7.6.2 SPSS与方差分析1 单因素方差分析2 双因素方差分析7.6.3 SPSS与回归分析SPSS回归分析过程牙膏价格问题的回归分析_化工数学模型数据回归软件
文章浏览阅读7.5k次。如何利用hutool工具包实现邮件发送功能呢?1、首先引入hutool依赖<dependency> <groupId>cn.hutool</groupId> <artifactId>hutool-all</artifactId> <version>5.7.19</version></dependency>2、编写邮件发送工具类package com.pc.c..._hutool发送邮件
文章浏览阅读867次,点赞2次,收藏2次。docker安装elasticsearch,elasticsearch-head,kibana,ik分词器安装方式基本有两种,一种是pull的方式,一种是Dockerfile的方式,由于pull的方式pull下来后还需配置许多东西且不便于复用,个人比较喜欢使用Dockerfile的方式所有docker支持的镜像基本都在https://hub.docker.com/docker的官网上能找到合..._docker安装kibana连接elasticsearch并且elasticsearch有密码
文章浏览阅读1.3w次,点赞57次,收藏92次。整理 | 郑丽媛出品 | CSDN(ID:CSDNnews)近年来,随着机器学习的兴起,有一门编程语言逐渐变得火热——Python。得益于其针对机器学习提供了大量开源框架和第三方模块,内置..._beeware
文章浏览阅读7.9k次。//// ViewController.swift// Day_10_Timer//// Created by dongqiangfei on 2018/10/15.// Copyright 2018年 飞飞. All rights reserved.//import UIKitclass ViewController: UIViewController { ..._swift timer 暂停
文章浏览阅读986次,点赞2次,收藏2次。1.硬性等待让当前线程暂停执行,应用场景:代码执行速度太快了,但是UI元素没有立马加载出来,造成两者不同步,这时候就可以让代码等待一下,再去执行找元素的动作线程休眠,强制等待 Thread.sleep(long mills)package com.example.demo;import org.junit.jupiter.api.Test;import org.openqa.selenium.By;import org.openqa.selenium.firefox.Firefox.._元素三大等待
文章浏览阅读3k次,点赞4次,收藏14次。Java软件工程师职位分析_java岗位分析
文章浏览阅读2k次。Java:Unreachable code的解决方法_java unreachable code
文章浏览阅读1w次。1、html中设置标签data-*的值 标题 11111 222222、点击获取当前标签的data-url的值$('dd').on('click', function() { var urlVal = $(this).data('ur_如何根据data-*属性获取对应的标签对象