第1部分
这里的更新地图，只是为了可视化。因为真正的地图存储在粒子里面，这里会拿一个权值最大的粒子的地图发布出来.
得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图，然后把得到的地图发布出去。
每次addScan()成功了，就会调用这个函数来生成地图，并发布出去。
1 | ROS_DEBUG("Update map"); |
计算位姿的信息熵
1 | double SlamGMapping::computePoseEntropy() |
第2部分
1 | /*地图的中点*/ |
这里的更新地图，只是为了可视化。因为真正的地图存储在粒子里面，这里会拿一个权值最大的粒子的地图发布出来.
得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图，然后把得到的地图发布出去。
每次addScan()成功了，就会调用这个函数来生成地图，并发布出去。
1 | ROS_DEBUG("Update map"); |
1 | double SlamGMapping::computePoseEntropy() |
1 | /*地图的中点*/ |
ROS环境下的安装：1
2
3
4
5
6
7
8
9
10
11
12
13# 也可用与非ROS环境
sudo apt-get install ros-kinetic-octomap
# octomap的查看工具
sudo apt-get install ros-kinetic-octovis
````
`octomap_ros`和`octomap_msgs`提供了消息文件，封装和转换方法。`octomap_server`提供建图和服务
## 编译
在`package.xml`中添加：
```sh
<build_depend>octomap</build_depend>
<run_depend>octomap</run_depend>
因为库提供了CMake的config文件，所以直接这样用：1
2
3find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
target_link_libraries(${OCTOMAP_LIBRARIES})
地图分很多种：稀疏的，稠密的，还有半稀疏的。稀疏的地图放大了看就是一个个离散的空间点，不过我们可以把它变成连续的稠密的网格，这个过程也叫点云的网格化。点云网格化需要对点云进行一系列处理
一般下面这几种情况需要进行点云滤波处理：
点云数据密度不规则需要平滑
因为遮挡等问题造成离群点需要去除
大量数据需要降采样
噪声数据需要去除
package.xml中要添加：1
2<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>
CMake按照第一篇里进行配置
1 |
|
把相邻的点合并成一个点，一方面可以减少图像的数据量，另一方面可以增加点云的稳定度。个人感觉贴近滤波
多线雷达点云大部分的点都集中的中间区域，距离越远点云越稀疏，信息量也越小，应该筛选掉一些较远的点。这就用到了直通滤波，它是最为简单、粗暴的一种滤波方式，就是直接对点云的X、Y、Z轴的点云坐标约束来进行滤波。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
reader.read("16line.pcd", *cloud);
std::cerr << "Cloud before filtering: " << cloud->points.size() << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered3(new pcl::PointCloud<pcl::PointXYZ>);
// Create the filtering object 直通滤波
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("x"); // 设置操作的坐标轴
pass.setFilterLimits(-5.0, 5.0);
pass.filter(*cloud_filtered2);
// filter range Y-axis
// 注意多次滤波需要再次输入点云和调用filter函数
pass.setInputCloud(cloud_filtered2);
pass.setFilterFieldName("y");
pass.setFilterLimits(-5.0, 5.0);
pass.filter(*cloud_filtered3);
std::cerr << "Cloud after filtering: " << cloud_filtered3->points.size() << std::endl;
// 读取pcd点云数据进行操作，最后保存为pcd文件
pcl::PCDWriter writer;
writer.write("16line_filtered.pcd", *cloud_filtered3, false);
return 0;
}pass.setFilterLimitsNegative(true);: 是否保存滤波的限制范围内的点云，默认为false，保存限制范围点云，true时候是相反。
在某些情况下，我们也会需要去除给定区域内部的点，比如激光雷达扫描近处的点(可能就是装载雷达的车)，这些点通常对建图和感知不会有作用。
设定滤波条件进行滤波，点在范围内保留，不在范围内丢弃。
getFilterLimitsNegative 可以对选定范围取反，将其设为 true 时可以进行给定只保留给定范围内的点的功能。
实现降采样，既减少点云的数量，并同时保持点云的形状特征。创建一个三维体素，用体素内所有点的重心近似表示体素中的其他点，这样体素内所有点就用一个重心点最终表示。
使用小程序进行体素滤波，输入input.pcd，输出output.pcd1
pcl_voxel_grid input.pcd output.pcd -leaf 0.03,0.03,0.03
对于体素下采样接口功能函数pcl提供了两种方式 pcl::ApproximateVoxelGrid和 pcl::VoxelGrid。两种区别主要在于第一种是用每个体素栅格的中心点来近似该体素内的点，提升了速度，但是也损失了原始点云的局部形态精细度。 ApproximateVoxelGrid是依据每一个体素的中心点来获取点云的，并不是依赖每个体素里面是否存在点云。所以会导致两种方式的处理结果又偏差，VoxelGrid的方式更加精确但是耗时相对于ApproximateVoxelGrid更高。
对整个输入迭代一次，对于每个点进行半径R的邻域搜索(球体)，如果邻域点的个数低于某一阈值，则该点将被视为噪声点并被移除。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); //待滤波点云
if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
{
PCL_ERROR("\a点云文件不存在！\n");
system("pause");
return -1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //滤波后点云
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror; //创建滤波器对象
ror.setInputCloud(cloud); //设置待滤波点云
ror.setRadiusSearch(0.02); //设置查询点的半径邻域范围
ror.setMinNeighborsInRadius(5); //设置判断是否为离群点的阈值，即半径内至少包括的点数
//ror.setNegative(true); //默认false，保存内点；true，保存滤掉的外点
ror.filter(*cloud_filtered); //执行滤波，保存滤波结果于cloud_filtered
实现：1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17void radiusremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, double radius, int min_pts)
{
pcl::KdTreeFLANN<pcl::PointXYZ> tree;
tree.setInputCloud(cloud);
vector<float> avg;
for (int i = 0; i < cloud->points.size(); ++i)
{
vector<int> id(min_pts);
vector<float> dist(min_pts);
tree.nearestKSearch(cloud->points[i], min_pts, id, dist);
// 是否是平方
if (dist[dist.size() - 1] < radius)
{
cloud_filtered->push_back(cloud->points[i]);
}
}
}
构建一个 KD 树，对每个点进行范围搜索（最近邻搜索 nearestKSearch），最后判断邻近点数（最大距离）是否满足要求即可。
看过源码后得知， 计算均值 时除以的是点云的点数，不是邻域的点数。 对滤波结果有影响的是标准差阈值系数k和邻域的点数。 使用的也是 nearestKSearch
缺点：对较远的激光线也筛除掉了。首先对激光雷达点云数据中的每个点而言，由于每个激光线束本身扫描频率是一致的， 显然越远的扫描线点会越稀疏，因此其平均距离会越远（即便其本身可能不一定是噪点）。因此更合理的方法可能是对每个点周围小范围内（同一条线内）的点进行高斯拟合，然后判断该点是否满足该高斯分布，但是如果需要得到比较准确的高斯分布参数，点数需要达到一定数量，否则和上述基于点云密度进行去噪差别不大。
1 |
|
常用滤波顺序: 直通 —-> 体素 —-> 统计
参考：统计滤波的源码
当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight,resample。首帧则调用invalidActiveArea,computeActiveArea,registerScan。
1 | bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles) |
1 | ScanMatcher::ScanMatcher(): m_laserPose(0,0,0) |
全是参数赋值，但是全写死，不能通过ROS修改
不是执行圆加运算，pnew和pold只是用于产生噪声控制量，然后加到p上1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const
{
double sxy=0.3*srr;
// 计算出pnew 相对于 pold走了多少距离，得到控制量
// 这里的距离表达是相对于车身坐标系来说的
OrientedPoint delta=absoluteDifference(pnew, pold);
/*初始化一个点*/
OrientedPoint noisypoint(delta);
/*走过的三个方向的距离，为控制量添加上噪声项*/
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
/*限制角度的范围为 -π～π */
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
/*把加入了噪声的控制量 加到粒子估计的最优的位置上，得到新的粒子位姿的预估(根据运动模型推算)*/
return absoluteSum(p,noisypoint);
}
这里的MotionModel是一个十分粗糙的运动模型，只是简单的矢量加减运算。 相比于《Probabilistic Robotics》中提到的速度模型和里程计模型而言，有很多方面都没有考虑，精度上可能有折扣。
1 | last_update_time_ = reading.getTime(); |
1 | // 激光的名称必须是"FLASER" |
获得centered_laser_pose_ (laser坐标系的原点, 方向(0,0,angle_center)) 在odom坐标系中的坐标gmap_pose，即scan消息的时间戳时，激光雷达在里程计坐标系中的位姿
调用： getOdomPose(initialPose, scan.header.stamp)1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24bool
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
// Get the pose of the centered laser at the right time
centered_laser_pose_.stamp_ = t;
tf::Stamped<tf::Transform> odom_pose;
// 得到centered_laser_pose_ 在odom坐标系中的坐标 odom_pose
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
ROS_INFO("get centered_laser_pose_'s odom pose: yaw: %f", yaw);
ROS_INFO("centered_laser_pose_'s odom pose:: %f %f %f",odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), odom_pose.getOrigin().z());
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(), yaw);
return true;
}
S是随机数种子，保证每次的高斯分布不同。AMCL源码中是pf_pdf_gaussian_sample函数中调用pf_ran_gaussian1
2
3
4
5
6
7
8
9
10double sampleGaussian(double sigma, unsigned int S)
{
if (S!=0)
{
srand(S);
}
if (sigma==0)
return 0;
return pf_ran_gaussian (sigma);
}
GridFastSLAM初始化主要是用来初始化各个粒子的一些信息。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose)
{
//设置地图大小和分辨率
m_xmin=xmin;
m_ymin=ymin;
m_xmax=xmax;
m_ymax=ymax;
m_delta=delta;
/*设置每个粒子的初始值*/
m_particles.clear(); // vector<Particle>
TNode* node=new TNode(initialPose, 0, 0, 0);
//粒子对应的地图进行初始化 用两个地图来进行初始化 一个高分辨率地图 一个低分辨率地图
//高分辨率地图由自己指定 低分辨率地图固定为0.1m
//定义最终使用的地图数据类型 cell类型为 PointAccumulator 存储数据类型为HierarchicalArray2D<PointAccumulator>
// typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
ScanMatcherMap lowMap(Point(xmin+xmax,ymin+ymax)*0.5,xmax-xmin,ymax-ymin, 0.1);
for (unsigned int i=0; i<size; i++) // size是粒子数
{
m_particles.push_back(Particle(lmap,lowMap));
//m_particles.push_back(Particle(lmap));
m_particles.back().pose = initialPose;
// 开始时，前一帧的粒子位姿也是 initialPose
m_particles.back().previousPose = initialPose;
m_particles.back().setWeight(0);
m_particles.back().previousIndex=0;
// we use the root directly
m_particles.back().node= node;
}
m_neff=(double)size;
m_count=0;
m_readingCount=0;
m_linearDistance=m_angularDistance=0;
}
接initMapper1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29GMapping::OrientedPoint odom_pose;
// addScan这个函数要转到pf的核心代码了，将调用processScan
if(addScan(*scan, odom_pose))
{
ROS_DEBUG("scan processed done !");
// 从粒子集合中挑选最优的粒子， 以其地图坐标为基准， 计算从激光雷达到地图之间的坐标变换
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_INFO("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
ROS_INFO("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
ROS_WARN("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
// 另一处的lock在 SlamGMapping::publishTransform()， 这里是获得map_to_odom_，前者是发布
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
// 如果没有地图则直接更新。 如果有地图了，两次扫描时间差大于参数，才更新地图
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
}
else
ROS_WARN("cannot process scan");
这里如果addScan返回false，就是未向地图添加scan，原因在processScan里，可能是机器人走过的线距离太短或者旋转过角度太少，未达到门槛；或者未达到指定的时间(period_，写死为5秒)，毕竟我们不希望机器人停着不动就更新地图，那太消耗资源了。
updateMap()得到最优的粒子的地图数据，利用占据栅格地图算法，更新地图。
把新的测量值和里程计坐标报告给建图引擎gsp_，让其更新粒子集。 根据激光雷达的安装方式，对角度进行修改。
然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式。设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46// 加入一个激光雷达的数据, 里面会调用 processScan()函数,这个函数被laserCallback()函数调用
// gmap_pose为刚定义的里程计的累积位姿
bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
//得到与激光的时间戳相对应的机器人的里程计的位姿
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
//检测是否所有帧的数据都是相等的，如果不相等就return
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
double* ranges_double = new double[scan.ranges.size()];
// 如果激光是反着装的，这激光的顺序需要反过来，这段代码省略
if (do_reverse_range_)
{
......
}
else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)
{
// mapper那里无法过滤掉
// 排除掉所有激光距离小于range_min的值，它们很可能是噪声数据
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
// 把ROS的激光雷达数据信息 转换为 GMapping算法看得懂的形式
// 激光传感器gsp_laser_ 在initMapper里定义，ranges数据赋值给m_dists，size给m_beams
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// 上面的初始化是进行深拷贝，因此申请的内存可以直接释放。
delete[] ranges_double;
//设置和激光数据的时间戳匹配的机器人的位姿 不是雷达位姿吗???
reading.setPose(gmap_pose);
//调用gmapping算法进行处理
return gsp_->processScan(reading);
}gmap_pose应当是不同时间戳时，雷达扫描扇形的中心(包含角度)在odom坐标系中的坐标。
问题：
reading.setPose(gmap_pose); 解释存疑最后就是核心算法所在的processScan
tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）
tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。
步骤:
tf message_filters::Subscriber 目标坐标系来初始化tf::MessageFilter到gmapping包里，查看CMakeLists可知，gmapping所用的可执行文件主要是slam_gmapping，对应slam_gmapping.cpp。
用到了头文件gridslamprocessor.h，其位于/opt/ros/kinetic/include/gmapping/gridfastslam/
gmapping用到的很重要的动态库有/opt/ros/kinetic/lib/libgridfastslam.so，它的来源是：1
2
3
4
5
6
7
8add_library(gridfastslam
gfsreader.cpp
gridslamprocessor.cpp
gridslamprocessor_tree.cpp
motionmodel.cpp
)
# 又链接库 scanmatcher sensor_range
target_link_libraries(gridfastslam scanmatcher sensor_range)scanmatcher库的来源：add_library(scanmatcher eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)
main函数如下，显然就看SlamGMapping了：1
2
3
4ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();
构造函数只有两行，定义了随机数种子，供之后的高斯采样使用。然后是init函数，它也很简单，先是下面两行，看来主要是类GridSlamProcessor1
2gsp_ = new GMapping::GridSlamProcessor();
tfB_ = new tf::TransformBroadcaster();GridSlamProcessor构造函数其实有用的就一句period_ = 5.0;，其他是参数赋值，但在initMapper里又会重新赋值，其实是多余的。 然后声明两个指针和布尔量，剩下的代码全是读取和配置参数了
1 | //注册entryopy map map_metadata话题 dynamic_map服务 |
mapCallback是service的回调函数，留给使用者定义客户端，这样一来重点是laserCallback，稍后再分析。
线程transform_thread_也很简单：1
2
3
4
5
6
7
8
9if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);
while(ros::ok())
{
publishTransform();
r.sleep();
}
publishTransform如下：1
2
3
4
5map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
// 构造函数里定义的广播， 发布map和odom之间的变换
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();
1 | laser_count_++; |
下面分析函数 bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
1 | laser_frame_ = scan.header.frame_id; |
这个是Gmapping算法接受的点类型1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32template <class T, class A>
struct orientedpoint: public point<T>{
inline orientedpoint() : point<T>(0,0), theta(0) {};
inline orientedpoint(const point<T>& p);
inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
inline void normalize();
inline orientedpoint<T,A> rotate(A alpha)
{
T s=sin(alpha), c=cos(alpha);
A a=alpha+theta;
a=atan2(sin(a),cos(a));
return orientedpoint(
c*this->x-s*this->y,
s*this->x+c*this->y,
a);
}
A theta;
};
template <class T, class A>
void orientedpoint<T,A>::normalize()
{
if (theta >= -M_PI && theta < M_PI)
return;
int multiplier = (int)(theta / (2*M_PI));
theta = theta - multiplier*2*M_PI;
if (theta >= M_PI)
theta -= 2*M_PI;
if (theta < -M_PI)
theta += 2*M_PI;
}
我看到很多篇论文使用了log格式的数据集进行建图算法的仿真，比如这里的，发现这输入carmen框架，已经比较古老
download log file下载数据集MIT data center的数据集页面
数据集2011-04-11-07-34-27.bag所用odom坐标系是odom_combined
数据集hallway_slow_2011-03-04-21-41-33.bag所用的雷达话题是base_laser1_scan
参考：各类数据集