gmapping源码中不完善的部分：
m_angularOdometryReliability和m_linearOdometryReliability用于控制激光评分的增益odo_gain, 实际都是0，所以没有用到，而且无法用参数赋值, 要使用就得修改源码.localScore=odo_gain*score(map, localPose, readings);，localScore在之后的likelihoodAndScore函数里实际赋值为0，根本没有用。所以这里有好几行可以删除在CMakeLists里做如下配置：1
2
3
4
5
6FIND_PACKAGE( OpenMP REQUIRED)
if(OPENMP_FOUND)
message("OPENMP FOUND")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
注意： OpenMP并不适合需要复杂的线程间同步和互斥的场合，这种情况下花的时间可能更长
gmapping使用OpenMP加速的语句: #pragma omp parallel for
for循环的drawFromMotion之前
1 | //#pragma omp parallel for |
invalidateActiveArea之前
1 | //#pragma omp parallel for |
scanMatch函数开头，实际就是对整个scanMatch并行化
1 | inline void scanMatch(double * plainReading) { |
updateMap函数中
1 |
|
对于保留下来的粒子进行更新，在并行化操作里面m_particles.push_back()会报错，因此需要把push_back()提出来，在外面的的for循环进行1
2
3
4
5
6
7
8
9
10
11
for(int i = 0; i<tmp_size;i++)
{
//对保留下来的粒子数据进行更新
//每个粒子的权重都设置为相同的值
temp[i].setWeight(0);
//为每个粒子更新running_scans
//增加了一帧激光数据 因此需要更新地图
m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);
//m_matcher.registerScan(temp[i].lowResolutionMap,temp[i].pose,plainReading);
}
为每个粒子更新地图时，同样可以并行化
先接上篇，不是第一帧激光数据的情况，上来是scanMatch函数，也就是GridSlamProcessor::scanMatch(在文件gridslamprocessor.hxx)。
1 | inline void GridSlamProcessor::scanMatch(const double* plainReading) |
开始的都没意思，直接看ScanMatcher::optimize，在scanmatcher.cpp里，开头又是一个重要函数score，在scanmatcher.h里。它根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来。
代码比较复杂，直接看整个scan函数(包括了optimize和score)的评分算法的理论：
对于栅格地图的环境地图模型，一般采用基于似然场模型（likelihood field range finder mode）的扫描匹配算法来进行激光雷达数据与地图的匹配。 该算法通过评估当前时刻位姿的激光雷达数据中每一个激光点与该时刻栅格地图的匹配程度，并将评估得到的每个激光点得分进行累加，得到当前时刻激光雷达数据与地图匹配的总分数，得分越高说明该时刻激光雷达信息与地图匹配程度越高，即机器人位姿越精确。由于该算法能够根据机器人当前时刻状态、 观测值和地图信息直接得到该时刻位姿的匹配程度，因而该算法常与基于粒子滤波的定位算法结合使用，用于选取各粒子中得分最高的位姿，并计算得分， 从而完成机器人位姿的确定，其原理如图：
黑色的格子代表前一时刻栅格地图中障碍物的位置， 圆形的点表示以里程计运动模型作为提议分布得到的机器人位姿估计为基础，将当前时刻激光雷达数据转换到栅格地图坐标系中的激光点的分布。把激光雷达的坐标转换到世界坐标系: 先旋转到机器人坐标系，然后再转换到世界坐标系。
该位姿下观测信息与地图匹配得分的具体算法步骤如下：对于当前激光雷达数据中任意一个激光点，设其端点在世界坐标系中坐标为 lp ，则其对应栅格地图中坐标为 phit ，对 phit周围八个栅格进行障碍物判定，并计算这些障碍物的平均坐标 pavr， 求出 pavr与 phit 的距离 dist 。 该激光点的评分可由下式表示
ε为基于扫描匹配概率的激光方差，对该时刻所有激光点进行上式的计算，并将评分进行求和，分数越高说明该位姿的可信度越高。 对当前时刻所有粒子位姿进行扫描匹配运算， 并根据得分实现粒子的权重更新，最后求出粒子群的平均评分sumScore/m_particles.size()
如果设置的minimumScore参数过大，一些粒子的匹配会失败，因为要求太高了，gmapping会出现下列信息：
gmapping变成使用里程计进行位姿估计，这其实是不好的，因为从论文可知激光精度比里程计精确得多，但是注意粒子的权重计算还是调用likelihoodAndScore函数。
直接修改gridslamprocessor.hxx的scanMatch函数。 小强的gmapping编译有问题，执行catkin_make --pkg gmapping之后，需要 cp /home/xiaoqiang/Documents/ros/devel/lib/gmapping/slam_gmapping /home/xiaoqiang/Documents/ros/src/gmapping/launch，否则roslaunch找不到节点文件
根据测试，激光评分多数在140~160，有时也会超过160.
1 | /*矫正成功则更新位姿*/ |
resample函数： 该函数在gridslamprocessor.hxx。首先是备份老的粒子的轨迹，即保留叶子的节点。然后是需要重采样还是不需要重采样，如果不需要重采样，则权值不变。只为轨迹创建一个新的节点，每个粒子更新地图。当有效值小于阈值的时候需要重采样，通过resampleIndexes提取到需要删除的粒子。删除粒子后，保留当前的粒子并在保存的粒子的节点里新增一个节点。删除要删除粒子的节点，保留的粒子进行数据更新，将每个粒子的设置同一个权重。最后更新一下地图。
resampleIndexes：该函数在particlefilter.h中，使用轮盘赌算法，决定哪些粒子会保留，保留的粒子会返回下标，里面的下标可能会重复，因为有些粒子会重复采样,而另外的一些粒子会消失掉。
首先计算总的权重，计算平均权重值（interval），根据权值进行采样,target是0-1分布随机选取的一数值，当总权重大于目标权重的，记录该粒子的索引，target在加上一个interval。如果某个粒子的权重比较大的话，就被采样多次。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// 简化声明
int resampleIndexes(const std::vector<Particle>& particles, int nparticles) const
{
Numeric cweight=0;
/*计算总的权重*/
unsigned int n=0;
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it)
{
cweight+=(Numeric)*it;
n++;
}
if (nparticles>0)
n=nparticles;
//compute the interval
Numeric interval=cweight/n;
// drand48 返回服从均匀分布的·[0.0, 1.0) 之间的double随机数
Numeric target=interval*::drand48();
// 如果某个粒子的区间为4*interval。那么它至少被采样3次。
cweight=0; n=0;
std::vector<unsigned int> indexes(n);
unsigned int i=0;
for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i)
{
cweight+=(Numeric)* it;
while(cweight>target)
{
indexes[n++]=i;
target+=interval;
}
}
return indexes;
}
多次测试，发现只能在室内平地建图，在地面不平的停车场运行carto，无论用机器人的odom还是carto的，odom坐标系会严重漂移，原因不明。
2020.12.24 xqserial_no_odom.launch: 将base_link和base_footprint之间的变换的注释去掉
将/home/xiaoqiang/Documents/ros/src/ORB_SLAM2中的CMakeLists大部分内容都注释掉了，目前还用不到它．
注释xqserial_server/CMakeLists.txt的57行，否则出现下面情况：
nav_test不需要特别编译，它全是py文件，没有cpp。但是需要从网上下载rbx1工程，这是一本教程里用到的，只把其中的rbx1_nav放到当前工作空间里，编译通过。nav_squared.py需要它
将startup.launch注释掉一部分，比如摄像头节点等等。
robot_pose_ekf包很奇怪，它应当是在/home/xiaoqiang/Documents/ros/src/navigation，但是修改相应源码，执行launch没有效果。最后发现实际使用的是R50_Release中的robot_pose_ekf。删掉后者就报错找不到了，可见xiaoqiang中的那个没有生效，不像个package，但是rospd又能找到，很奇怪。
odom和IMU话题的频率都是50Hz
小强有时不移动，可能是IMU在重启，因为此时的IMU数据除了四元数的 w 为1，其他全是0
/xqserial_server/Twist疑似无用，只有/motor_driver在一直发布它
xqserial_server/Pose2D的消息是(x,y,theta)，是相对里程计坐标系原点的位姿
里程计模型也是速度积分的二轮差速，使用编码器读数获得极小时间内的位移，以左右轮的编码器增量的均值计算。
要开启红外功能，必须使用命令 rostopic pub -r 12 /barDetectFlag std_msgs/Bool "data: true"，但红外检测距离太短了，目前被摄像头挡住了，实际不生效
control.py就是从键盘获取方向键，发布速度命令到cmd_vel话题，订阅了/xqserial_server/Odom话题，但是实际没用到。
xqserial_server命名空间包括StatusPublisher和DiffDriverController两个类
xiaoqiang_log话题对应的LogRecored.msg如下，只有缺少串口设备时才发布，record是json形式1
2
3string collection_name
time stamp
string record
xiaoqiang_tts/text话题对应语音提示信息，也是在缺少串口设备时才发布
开机启动的launch文件是startup包中的startup.launch1
2
3
4
5
6
7
8
9
10
11
12
13<launch>
<node pkg="xqserial_server" type="xqserial_server" name="motor_driver" respawn="true" respawn_delay="10">
<param name="debug_flag" value="false"/>
<param name="wheel_separation" value="0.360"/>
<param name="wheel_radius" value="0.0625"/>
<param name="port" value="/dev/ttyUSB001"/>
<param name="r_min" value="1.0"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="baselink_broadcaster" args="0 0 0.15 0 0 0 1 base_footprint base_link 50">
</node>
<node pkg="tf" type="static_transform_publisher" name="imulink_broadcaster" args="-0.1 -0.03 0 0 0 0 1 base_link imu 50"/>
</launch>
主要是xqserial_server，然后是静态发布tf变换： base_footprint --> base_link --> imu
尝试在远程机上同时启动xiaoqiang_udrf.launch和rviz，先尝试加载urdf.rviz，这种情况下的tf变换都是正常的
但是常用方式是远程机启动launch，本地机启动rviz，rviz里做好配置后，发现终端会报错：找不到所需要的所有STL文件，这几个文件在/home/xiaoqiang/Documents/ros/src/xiaoqiang_udrf/meshes。这是因为rviz在本机找这些文件，
launch文件里加载了xiaoqiang_udrf.URDF，这个文件又加载STL文件，语句是这样的：1
<mesh filename="package://xiaoqiang_udrf/meshes/base_link.STL" />
也就是说本地机需要一个名为xiaoqiang_udrf的package，自己新建一个。
这时再打开rviz就不会报错了，但还是有问题，整个模型透明发白，这是因为rivz中的全局坐标系“fixed frame”设置的不合适，将map改成base_link后即可正常显示。
结果机器人并不能动，rviz里仍显示几个wheel和odom没有tf变换，但是rosrun tf tf_echo有正常结果，结果发现就是显示的问题，重启rviz就好了
现在urdf中的坐标系叫做lidar_urdf，static_transform_publisher定义的是base_link和laser的关系，在a2.launch里。
可以看到二者的坐标关系有所不同，如果把lidar_urdf也改成laser，那么在rviz可以看到laser坐标系会不断旋转跳动，这是存在两个tf变换造成的。由于urdf只是个显示功能，所以还是用两个坐标系。base_link和laser之间的关系可能不精确，这就需要标定了
无地图状态下，以odom为固定坐标系，移动机器人时，base_link会相对移动。但是用rviz里的Odometry可以表达base_link在odom坐标系下的运动
使用最新版本，一开始总连接不成功，最后发现是公用网络的防火墙造成的，但是不知为什么，防火墙已经将它添加例外了，无法修改，只好把防火墙关了
gmapping建图： roslaunch gmapping a2.launch，对应rplidar_a2_test.rviz
这里的更新地图，只是为了可视化。因为真正的地图存储在粒子里面，这里会拿一个权值最大的粒子的地图发布出来.
得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图，然后把得到的地图发布出去。
每次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 |
|
常用滤波顺序: 直通 —-> 体素 —-> 统计
参考：统计滤波的源码
laser_scan_matcher是一个增量式的scan配准工具. 在连续的sensor_msgs/LaserScan消息之间进行scan match, 把laser的估计位姿以geometry_msgs/Pose2D消息发布，产生tf变换。
激光里程计可以不需要轮式里程计和IMU，也可以使用二者加快配准速度。可接受sensor_msgs/LaserScan或者sensor_msgs/PointCloud2消息
当每次新的scan产生时（每次进入scan的回调函数时），如果为传感器当前的位姿提供初值(使用轮式里程计和IMU)，可以加快PL-ICP的配准速度。如果没有初值，则使用零速度模型。 源码在getPrediction函数，在delta_t时间内的相对运动x, y用轮式里程计的delta_x和delta_y表示。 如果用了IMU，delta_theta用IMU的相对yaw表示，否则还用轮式里程计。
包里的demo.bag是手持一个Hokuyo雷达的移动结果，当然没有里程计。
常数速度模型: 假设机器人基于估计的速度运行，速度估计来自传感器或 derivating and filtering scan match的结果. 需要的话题是vel. 使用Alpha-beta滤波器 tracking scan-matching 的预测值
零速度模型: 不使用预测，假设机器人停在原地。
我们可以使用轮式里程计+IMU，或者 IMU+alpha-beta tracking。 使用的优先级是：IMU > Odometry > Constant Velocity > Zero Velocity. 源码在getPrediction函数
IMU和轮式里程计(尤其IMU)可以显著提高收敛速度， 强烈建议添加IMU
scan matcher性能稳定时， Alpha-beta tracking 也可以加速收敛，但可能在动态变化环境或缺少特征的环境下出现weird behavior， 一般建议开启，出现异常再关闭。
激光里程计一般是frame-to-frame，每一帧scan和上一帧配准. scan中的噪声是难免的，即使机器人不动， incremental 变换也不是零，机器人位姿会慢慢漂移。
可以使用keyframe-based matching. 位姿变换是在当前scan 和 keyframe scan之间计算. keyframe scan只有在机器人移动一定距离后才更新，如果机器人不动，keyframe scan不变，位姿就不会漂移。 参数kf_dist_linear和kf_dist_angular(弧度制)作为阈值。 把两个阈值都改为0，就可以切换回frame-to-frame模式
订阅:
use_imu为true才有效use_odom为true才有效发布:
pose2D (geometry_msgs/Pose2D) The pose of the base frame, in some fixed (world) frame
需要的tf: base_link——laser, the pose of the laser in the base frame.
提供的tf: world——base_link, the pose of the robot base in the world frame. Only provided when publish_tf is enabled. world可以改为自定义的名称，一般为odom
~fixed_frame (string, default: “world”): the fixed frame，可以改为odom~base_frame (string, default: “base_link”): the base frame of the robot
~use_imu (bool, default: true) Whether to use an imu for the theta prediction of the scan registration. Requires input on /imu/data topic.
~use_odom (bool, default: true) Whether to use wheel odometry for the x-, y-, and theta prediction of the scan registration. Requires input on odom topic.~use_vel (bool, default: false) Whether to use constant velocity model for the x-, y-, and theta prediction of the scan registration. Requires input on vel topic.注释StatusPublisher.cpp的部分代码，也就是屏蔽原来的odom：1
2
3
4
5mOdomPub = mNH.advertise<nav_msgs::Odometry>("xqserial_server/Odom", 1, true);
mOdomPub.publish(CarOdom);
br.sendTransform(tf::StampedTransform(transform, current_time.fromSec(base_time_), "odom", "base_footprint"));
注释xqserial.launch下面部分，因为我们的目的是odom-->base_link，目前base_footprint是link的parent了，base_link不能有两个parent:1
<node pkg="tf" type="static_transform_publisher" name="baselink_broadcaster" args="0 0 0.15 0 0 0 1 base_footprint base_link 50">
编辑laser_scan_matcher中的rplidar.launch:1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17<launch>
<include file="$(find rplidar_ros)/launch/a2.launch" />
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
name="laser_scan_matcher_node" output="screen">
<param name="publish_tf" value = "true" />
<param name="fixed_frame" value = "odom"/>
<param name="max_iterations" value="10"/>
<param name="publish_pose_with_covariance_stamped" value="true" />
<param name="publish_pose" value="false" />
<param name="do_compute_covariance" value="false" />
<param name="kf_dist_linear" value="0.08" />
<param name="kf_dist_angular" value="0.14" />
<remap from="/imu/data" to="/xqserial_server/IMU" />
<remap from="pose_with_covariance_stamped" to="plicp_pose" />
</node>
</launch>
启动startup, roslaunch laser_scan_matcher rplidar.launch. 现在原来的轮式坐标系odom坐标系换成了 激光坐标系 odom
现在tf变换是: odom —-> base_link —-> laser
深度图不受阴影、光照、色度的影响。
深度图的各个像素点和实际中的对应坐标值通过标定矩阵匹配的方法获取环境信息的三维点云。每一帧深度图上的y坐标和z坐标代表深度图中的像素单位，不是实际世界坐标。
kinect深度在0.5~8米，随着距离的增加而精度下降，推荐深度范围1-4米，所以深度相机只能用于小场景。
Kinect最显著的缺点就是水平视场角太小。
深度图的z轴正向为相机前方
kinect——深度图点云数据——特征提取——特征匹配——建图
深度图中有黑洞和边缘噪点。这就是测量噪声，而且深度相机的测量噪声随着测量距离的变大而变大。双边贝叶斯滤波
当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用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(); |