Each source gives a pose estimate and a covariance. The sources operate at different rates and with different latencies. A source can appear and disappear over time, and the node will automatically detect and use the available sensors

给滤波器节点提供信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，每个传感器发出来的绝对位姿不能直接对比。

robot_pose_ekf使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器，用松耦合方式融合不同传感器信息实现位姿估计。

在位姿本身上发布协方差是没有用的，而是传感器源发布协方差如何随时间变化，即速度的协方差。使用对世界的观测（例如，测量到已知墙壁的距离）将减少机器人位姿的不确定性；但是这是定位而不是里程计。

准备工作

编译前先安装orocos-bfl

校正好IMU,因为融合后的效果就是要看imu的校准度

注释掉StatusPublisher.cpp中的sendTransform(tf::StampedTransform(transform, current_time.fromSec(base_time_), "odom", "base_footprint")); 因为ekf包会为我们处理好这部分tf，所以不需要我们发布odom变换了。但是保留odom话题的发布

修改 robot_pose_ekf.launch

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<launch>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
      <param name="output_frame" value="odom_ekf"/>
      <param name="base_footprint_frame" value="base_footprint"/>
      <param name="freq" value="30.0"/>
      <param name="sensor_timeout" value="1.0"/>
      <param name="odom_used" value="true"/>
      <param name="odom_data" value="odom"/>
      <param name="imu_used" value="true"/>
      <param name="imu_data" value="imu"/>
      <param name="vo_used" value="false"/>

      <remap from="/robot_pose_ekf/odom_combined" to="/odom_ekf" />
    </node>
 </launch>

• freq：滤波器更新和发布频率。注意频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高位姿估计的精度。

• sensor_timeout：当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。

• odom_used, imu_used, vo_used：确认是否输入。

启动之后,发布一下tf树,看看各坐标系名称是否正确,而且注意odom_ekf是不是robot_pose_ekf发布的，它提供的tf变换是： odom_ekf --> base_footprint。 我们最终要实现的目标TF关系是： odom_ekf --> base_footprint --> base_link

启动launch: roslaunch robot_pose_ekf robot_pose_ekf.launch

为了查看ekf包是否正常工作，可以用下面代码：

1

rosservice call /robot_pose_efk/get_status

协方差问题

robot_pose_ekf不输出协方差信息，但要求输入协方差，协方差是on velocity level。接收的odometry数据格式错误的问题，一般是由于底盘发布的odometry数据的协方差矩阵默认为0矩阵。解决的方法由两种：一种是底盘对协方差矩阵进行初始化；另一种方法在robot_pose_ekf中添加判断，如果接收到的odometry信息的协方差矩阵没有进行初始化，则进行初始化。

turtlebot_node对odom的协方差矩阵设置为

1
2
3
4
5
6
7
8
9
10
11
12
13

ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                        0, 1e-3, 0, 0, 0, 0,
                        0, 0, 1e6, 0, 0, 0,
                        0, 0, 0, 1e6, 0, 0,
                        0, 0, 0, 0, 1e6, 0,
                        0, 0, 0, 0, 0, 1e3]

ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                         0, 1e-3, 0, 0, 0, 0,
                         0, 0, 1e6, 0, 0, 0,
                         0, 0, 0, 1e6, 0, 0,
                         0, 0, 0, 0, 1e6, 0,
                         0, 0, 0, 0, 0, 1e3]

注意imu信息的协方差矩阵中代表机器人航向角的分量方差为1e-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3，两个值相差很大。在进行EKF融合时，会更“相信”imu提供的姿态信息，因为其方差更小。比如机器人在转动过程中轮子发生了打滑，用编码器推算出的姿态一直在旋转，而实际姿态（主要由IMU测量得到）却没发生太大变化，这种情况就需要使用信息融合方法来减小误差。 协方差矩阵中的参数设置非常重要，要根据传感器手册或者实际使用测量来确定。

这里的协方差矩阵就是<<概率机器人>>154页的Vt*Mt*VtT, 根据之前速度积分的模型,我们已知的是ωt,Δt,θ

只有a1~a4还未知,它们的说明在《概率机器人》103页， 似乎不对： 这里就涉及到粒子滤波和AMCL了,其实它们就是AMCL的四个参数,即里程计的四个噪声分量. 我们在使用AMCL定位前就要调节这四个参数, 有了这四个参数,就能算出里程计的协方差矩阵了.

随着机器人的移动，其位姿的不确定性越来越大。随着时间的流逝，协方差将无限增长。robot_pose_ekf会在每个传感器更新数据前重置协方差， 所以发布的协方差是the increase in covariance over the past time interval. 这个时间间隔不是不变的，它取决于传感器的测量速度和什么时候完成．

try adjusting the process noise vs. the measurement covariance for the state variables you’re fusing. Lower covariance in the measurement and higher process noise will mean that the filter trusts your sensors more. Otherwise, the filter will prefer to stick with its predictions

里程计消息的协方差平时可以没有,但是如果要用到卡尔曼滤波做融合,就必须有,否则到OdomEstimation::addMeasurement会报错，那里是判断协方差矩阵的对角线元素

1

[ERROR] [1519539033.600801081]: Covariance specified for measurement on topic wheelodom is zero

1

CarOdom.pose.covariance =  boost::assign::list_of(var_len)(0)(0)(0)(0)(0)(0)(var_len)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(var_angle);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

for (unsigned int i=0; i<6; i++)
  for (unsigned int j=0; j<6; j++)
    odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
 
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0)  // 或者不用 if
{
  SymmetricMatrix measNoiseOdom_Cov(6);
  measNoiseOdom_Cov = 0;
  measNoiseOdom_Cov(1,1) = pow(0.01221,2);  // = 0.01221 meters / sec
  measNoiseOdom_Cov(2,2) = pow(0.01221,2);  // = 0.01221 meters / sec
  measNoiseOdom_Cov(3,3) = pow(0.01221,2);  // = 0.01221 meters / sec
  measNoiseOdom_Cov(4,4) = pow(0.007175,2);  // = 0.41 degrees / sec
  measNoiseOdom_Cov(5,5) = pow(0.007175,2);  // = 0.41 degrees / sec
  measNoiseOdom_Cov(6,6) = pow(0.007175,2);  // = 0.41 degrees / sec
}
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);

如果协方差太大，那么说明机器人不太依靠里程计．协方差矩阵具体值可以考虑设置为精度的二次方。工程中确保odom的协方差矩阵对角线元素不均为0，则robot_pose_ekf即可工作。

源码的坐标系错误

使用rviz观看滤波后的行走轨迹，结果报错：

它居然要求的是odom坐标系，但是查看tf树，坐标关系是正确的odom_combined --> base_footprint，没有odom了。

使用echo查看/robot_pose_ekf/odom_combined话题，发现header里的frame_id: “odom”，看来是哪个地方发布错了。 从robot_pose_ekf源码里查找，发现OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)中，有这样一句estimate.header.frame_id = "odom";，按说新的坐标系是odom_combined，不知为什么这里还是odom，只能认为是源码写错了。

参考:
turtlebot所用的里程计和IMU协方差是手动设置的
ROS answer的回答
XiaoXiaoTao博客

异常是运行期出现的情况，编译不会报错。如果出现异常，它后面的代码不会执行，一般会显示 The program has unexpectedly finished. </font>。如果能处理好异常，就可以让后面的代码继续运行

throw就是抛出异常，后面可以接任何语句表示异常。比如throw 123;, throw "exception";

try里面的第一个语句必须包含throw，可以是个函数。之后的语句不再进行，直接进catch了

catch的参数是和throw一致的，比如下面的const char*，如果要catch任何类型，小括号内换成...

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

double division(int a, int b)
{
   if( b == 0 )
      throw "Division by zero condition!";
   return (a/b);
}


try {
    cout << division(1,0) <<endl;
    cout << "本句不执行" <<endl;
}
catch(const char* msg)
{
  cerr << msg << endl;	// 输出错误用cerr
}

C++ 提供了一系列标准的异常，定义在，我们可以在程序中使用这些标准的异常。它们是以父子类层次结构组织起来的。可以使用catch(std::exception e)

一些第三方库也提供了异常，使用时要注意。比如log4cpp:

1
2
3
4
5
6
7
8

try
{
    log4cpp::PropertyConfigurator::configure(config_base_path+"setting.conf");
}
catch (log4cpp::ConfigureFailure& f)
{
    std::cout << "Configure Problem: " << f.what() << std::endl;
}

ELF文件有三种：可执行文件，so共享库，o目标文件

打印文件校验和

二进制文件传输过程中有没有被损坏或者是否是同一个版本，看看校验和以及程序块计数:

1
2

# md5sum liblidar.so
615f8ede92bb7cca3d559a46397474b6  liblidar.so

打印ELF文件中的可打印字符串 strings

例如你在代码中存储了一个版本号信息，那么即使编译成elf文件后，仍然可以通过strings搜索其中的字符串甚至可以搜索某个.c文件是否编译在其中：

1

strings elfFile| grep "someString"

nm命令查看函数或者全局变量是否存在于elf文件

nm命令用于查看elf文件的符号信息。文件编译出来之后，我们可能不知道新增加的函数或者全局变量是否已经成功编译进去。这时候，我们可以使用nm命令来查看。当然也可以用来查看函数，比strings命令更精确

查看文件段大小 size

可以通过size命令查看各段大小：

1
2
3

# size cmdTest
text       data      bss      dec      hex  filename
1319      560        8     1887      75f   cmdTest

text段：正文段字节数大小
data段:包含静态变量和已经初始化的全局变量的数据段字节数大小
bss段：存放程序中未初始化的全局变量的字节数大小
当我们知道各个段的大小之后，如果有减小程序大小的需求，就可以有针对性的对elf文件进行优化处理。

为elf文件瘦身 strip

strip用于去掉elf文件中所有的符号信息：

1
2
3
4
5

# ls -al cmdTest
-rwxr-xr-x 1 hyb root 9792 Sep 25 20:30 cmdTest #总大小为9792字节
strip cmdTest
ls -al cmdTest
-rwxr-xr-x 1 hyb root 6248 Sep 25 20:35 cmdTest#strip之后大小为6248字节

查看elf文件信息 readelf

readelf用于查看elf文件信息，它可以查看各段信息，符号信息等，readelf -h cmdTest是查看elf文件头信息：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

Magic:   7f 45 4c 46 02 01 01 00 00 00 00 00 00 00 00 00  #elf文件魔数字
Class:                             ELF64  #64位 elf文件
Data:                              2's complement, little endian#字节序为小端序
Version:                           1 (current)
OS/ABI:                            UNIX - System V #
ABI Version:                       0
Type:                              EXEC (Executable file)#目标文件类型
Machine:                           Advanced Micro Devices X86-64 #目标处理器体系
Version:                           0x1
Entry point address:               0x400440  #入口地址
Start of program headers:          64 (bytes into file)
Start of section headers:          4456 (bytes into file)
Flags:                             0x0
Size of this header:               64 (bytes)
Size of program headers:           56 (bytes)
Number of program headers:         9
Size of section headers:           64 (bytes)
Number of section headers:         28
Section header string table index: 27

参考：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

class Foo
{
public:
    void   operator() ()
    {
        cout <<"Foo operator"<<endl;
    }
    int operator() (int val)
    {
        return val*10;
    }
};

Foo f;
f();    // Foo operator
cout<< f(5) <<endl;  // 50

Foo是定义了调用操作符()的类,它的对象就相当于函数名,因此operator()取名叫函数对象

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

struct Data {
    double dist;
    double confidence;

    bool operator <= ( double d ) const
    {
        return(dist <  d);
    }
    bool operator <= ( Data& d ) const
    {
        return(dist <=  d.dist);
    }
};


Data a, b;
a.dist = 1;
a.confidence = 1;

b.dist = 1.3;
b.confidence = 1;
if(a <= 1.0)
       cout << " 00000 " << endl;
if(a <= b)
       cout << " 11111 " << endl;

代码在这里，网上的示例是这个，但是我发现现在的QtMqtt已经变换很大，之前的API很多已经不能使用了

值得注意的是，客户端不能在构造函数里订阅话题，因为客户端在connected信号对应的槽函数里才连接成功，所以订阅也应该换到槽函数或者自己做的按钮里

MQTT的通信可以在wireshark里读取，可以直接解析MQTT的数据包：

安装服务器

配置安装看ubuntu安装MQTT服务器 + windows安装MQTT客户端，主要是三个安装命令：

1
2
3
4
5
6

#安装服务端
sudo apt-get install mosquitto
#安装客户端 （订阅sub  与 发布pub）
sudo apt-get install mosqutitto-client
#安装调试器
sudo apt-get install mosquitto-dbg 

• 订阅话题test: mosquitto_sub -t test
• 发布消息到话题test: mosquitto_pub -t test -m “hello world”

编译Qt库QtMqtt

Qt现在支持MQTT了， 但是很坑的是还需要自己编译，我用的是Qt5.14，从网上找了好几个版本都编译出问题，看了不知道多少博客，最后终于编译成功了。

从Github官方仓库下载5.14.0，解压到F:\MyCpp\qtmqtt-5.14.0，打开后编译，结果会报错，这是因为头文件的问题。在qtmqtt-5.14.0\src\mqtt\mqtt.pro中添加一行 INCLUDEPATH += F:\MyCpp\qtmqtt-5.14.0 ，然后qmake, 编译。应该就成功了。

新建的测试程序MQTT_Client，然后将qtmqtt-5.14.0\src\mqtt文件夹中的所有头文件放到一个文件夹QtMqtt，然后放到程序的根目录。到编译生成的文件夹，将lib和include文件夹也放到MQTT_Client根目录，其中的文件Qt5Mqtt.dll.debug可以删除。

编辑MQTT_Client.pro，添加下面内容：

1
2
3
4
5

INCLUDEPATH += $$PWD
DEPENDPATH += $$PWD/include

win32:CONFIG(release, debug|release): LIBS += -L$$PWD/lib/ -lQt5Mqtt
else:win32:CONFIG(debug, debug|release): LIBS += -L$$PWD/lib/ -lQt5Mqtt

现在程序就可以用QtMqtt了，但是最好能让mqtt跟Qt自身库一样使用，也就是在pro文件中添加QT += mqtt即可，这样才叫优雅。参考如何让第三方库和Qt自身库一样使用

参考：
搭建MQTT服务器及测试
Qt官方MQTT库的配置

大多数雷达都是TOF测量法，只有深度信息，没有相机那样的纹理信息，也就没有视觉SLAM的运算负荷。

目前常见的激光雷达都是旋转扫描式的，内部长期处于旋转中的机械结构会给系统带来不稳定性，在颠簸震动时影响尤其明显。固态激光雷达的逐步成熟可能会为激光SLAM扳回这项劣势。

激光雷达的使用寿命问题已经被解决，能够保证长时间使用不会出现故障。比如在连续工作情况下，RPLIDAR-A2的设计使用寿命可长达5年以上。

雷达的光线遇到大雾、烟尘会受到遮挡，影响性能。

测试材质

选用雷达需要判断雷达是否适用于自己的场合，所以根据需要在以下场景进行测试

• 大理石瓷砖。 应用场景参考：酒店大堂、走廊、室外墙壁

• 玻璃。 应用场景参考：玻璃门窗，办公大厅、玻璃柜台。 激光会穿过透明玻璃，从而造成一定概率的漏检。可以增加一些辅助反射手段，比如粘贴磨砂贴纸，或配合其他非光学的传感器作为补充。 雷达有时会穿透玻璃，有时不会，所以临时在玻璃上贴磨砂纸。

• 不锈钢板。 应用场景参考：电梯、生产车间、港口码头

• 反光条。 应用场景参考：医院、生产车间、酒店大堂

误差源

• 发送和接收激光束的精确耗时误差，也就是计时设备的精度问题

• 目标材质的反射值特性，比如全黑的材料吸收了光的大部分能量，使得反射量极低；或者像镜子一样的材料会将大部分光反射到其它地方

• 运动畸变：由于激光雷达在跟随自动驾驶车辆前进的同时，对周围环境进行扫描建模，也就是说车辆相对于周围的环境是运动的，导致对环境测量的实际位置与真实位置存在偏差。但是扫描频率高，速度低时，可以不必考虑。

测距范围

指雷达能够测量的距离范围。如果实际障碍距离超出最大值，那么雷达数据会标记为无效点（不是距离为0）。市面上2D雷达最近距离也至少几厘米

实际情况下，雷达测距的最大值有可能因为工作环境而产生变化。雷达要想测距，需要接收到反射的激光。所以官方在测距范围这一项上添加了备注：“基于白色70%反射率物体”。

如果是吸收激光比较厉害的物体，例如黑色的表面又几乎不反光的物体，会导致反射光强度很弱，那么距离稍微远点，可能就测距失败了，这时候，该物体即使在标出的12米范围内同样无法测出。针对类似这样的物体，相当于实际测距的最大值变小了。透明的玻璃也是同样原因。

不过因为不同的物体和环境差异太大了，所以厂商也不太可能将全部情况测试一遍，更多的时候需要靠自己来实验，看是否能够适用实际的工作环境。

扫描角度

思岚雷达是360°扫描的。有些雷达例如SICK的一些雷达，扫描角度只有220°。 实际使用，通常也不需要完全的360°，特别是雷达放在结构的中间层，因为有结构固定装置的存在，必然会有遮挡。

测距分辨率

分辨率和精度是两个不同的概念，按照上述参数的意思，更准确来说应该指的是测距精度。

RPLIDAR的精度并不是恒定的一个百分比，简单的解释是，距离越远，反射光受到的干扰越大，自然精度下降了。实际上，不同批次的雷达精度之间也有一定的差异。正因为这些不确定性，官方文档给的是较保守的值。

1.5m范围内小于0.5mm的精度还是可以的，1.5米处约为万分之三点三。
当在最大距离12米的时候，如果精度下降到最差的1%，则误差为0.12m，也能接受。

扫描频率

衡量雷达一秒钟能转多少圈，直接改叫雷达转速也是可以的。

转速实际上跟雷达数据更新周期是挂钩的，比如说典型的10Hz，那就是说转一圈的时间大概是100ms，那么雷达数据差不多也是100ms一帧。 要跟scan话题的发布频率区分开，后者跟计算机性能有关。

LMS1xx系列的扫描频率是25～50Hz，角度分辨率为0.25°～0.50°
LMS5xx系列的扫描频率是25～100Hz，角度分辨率为0.1667°～1°

雷达自身的旋转是有方向的，大部分雷达都是逆时针旋转，与ROS中规定的一样，也有少部分雷达是顺时针旋转的，只不过使用起来有点不方便。

角度分辨率

正常来说，雷达转一圈，这一圈得到的测量点是均匀分布的，每个点之间间隔的角度就是所谓的角度分辨率了。

角度分辨率越小说明雷达转一圈得到的点数越多。例如，角度分辨率是0.45，则一圈是800个点，角度分辨率是0.9，则一圈是400个点。

不过，实际的角度分辨率其实不一定是固定的，即两个点之间的间距不一定是相同的，不过都在给出的分辨率范围内。在ROS中，雷达数据的标准格式认为角度分辨率是固定的，为了符合ROS标准，雷达的ROS驱动实际上做了角度补偿，将输出点修正为均匀分布的。

数据的强度

激光雷达的激光点是有能量的，不同品牌激光点的能量也不同。当能量太小时，远距离情况下可能存在返回不了数据的情况。

可以等阳光或者使用光束照射到墙面上，激光雷达再去看被光照射到的墙面，对比这时的点云效果。可以用照度仪测量此时的光强度。倍加福雷达的点云效果在高强度情况下非常好，不愧是用于反光板的雷达。

数据的精度

这是最重要的一个指标，表示激光雷达的数据跳动情况。现在一般厂商的雷达的精度都是2%。也就是100m的情况下，点的跳动幅度为2cm。但是，实际感觉能达到这个精度的雷达不是很多。

multi-echo

multi-echo可以分析每个测量光束的两个回波信号，这样在雨雪天可以提供可靠的测量结果。一般激光打到玻璃上会有部分穿透，导致测量不准，multi-echo使激光从玻璃上返回来，还能从玻璃后面的墙上返回来。

有的雷达具备这种特性，比如SICK-LMS111

参考：
从零开始搭二维激光SLAM —- 激光雷达数据效果对比
LakiBeam1雷达

查找速度

对序列式容器，如果元素已经排好序，那么查找速度可以达到logN的时间复杂度；如果是无序，只能是N

对关联容器，底层是红黑树，总能达到logN

有在任意位置插入元素的需求； 大量添加新元素的需求

最好用list，不要使用vector, deque

元素的排序

遍历元素的时候，序列容器输出的顺序和插入的顺序是一致的，关联容器就不一定了

sort()函数是快速排序的分段递归版本

关联容器的插入删除效率一般比用其他序列容器高(list除外)，因为不需要做内存拷贝和内存移动

STL 标准库提供了 4 种关联式容器，分别为 map、set、multimap、multiset

set

set的元素有序不重复，而且能根据元素的值自动进行排序。set中的键值不能直接修改，只能先删除再插入。底层采用红黑树。

set不支持随机访问，只能使用迭代器去访问。由于set放入一个元素就会调整这个元素的位置，把它放到合适的位置，所以set中只有一个insert插入操作。

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

set<int> s;
s.insert(5);
s.insert(2);
s.insert(3);
s.insert(1);
s.insert(2);
s.insert(4);
cout<<"set 的 size："<<  s.size() <<endl;  // 5
cout<<"set 中的第一个元素是 ："<<  *s.begin()<<endl;  // 1
cout<<"set 中的最后一个元素是:"<<  *s.end()<<endl;   // 5
set<int>::iterator it;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}    // 1  2  3  4  5
//    s.clear();
s.erase(++s.begin());
cout << "       after erase begin"<<endl;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}	 // 1  3  4  5
cout<<"lower bound: "<<*s.lower_bound(5)<<endl;  // 5
cout<<"upper bound: "<<*s.upper_bound(5)<<endl;  // 4
if(s.empty())
    cout << "set is empty !" <<endl;

multiset底层也是红黑树，但允许有重复数据

map 和 unordered_map

map适合存储一个数据字典，并要求方便地根据key找value。Map节点有一个Key和Value两个元素，Key不重复，Value可以重复。map可以通过key改变value的值

底层也是红黑树，所有元素都是有序的，红黑树的每一个节点都代表着map的一个元素，因此它的插入删除查找的时间复杂度为O(logN)

map支持随机访问(at函数和[])，这是set没有的。

1
2
3
4
5
6
7
8
9

map<int, string> m;
m.insert(pair<int, string>(1, "a"));
m.insert(pair<int, string>(2, "b"));
m.insert(pair<int, string>(3, "c"));
m.insert(pair<int, string>(4, "d"));
m.insert(pair<int, string>(5, "e"));

cout << m.upper_bound(3)->second <<endl;   // 大于
cout << m.lower_bound(3)->second <<endl;   // 不小于

1
2

d
c

缺点： 空间占用率高，因为map内部实现了红黑树，虽然提高了运行效率，但是因为每一个节点都需要额外保存父节点，孩子节点以及红/黑性质，使得每一个节点都占用大量的空间

unordered_map内部实现了一个哈希表，因此其元素的排列顺序是杂乱的，无序的，但查找速度非常的快。 缺点：哈希表的建立比较耗费时间。

Cartographer是基于图优化的SLAM，采用误差累计少，计算成本低的Scan to Map匹配方式，而不是造成误差快速累积和计算成本高的scan to scan匹配。整个算法分为局部SLAM和全局SLAM，两部分都对雷达观测的位姿进行了优化。

虽然cartographer适用于2D和3D两种情况，但3D情况下，cartographer用的不如LOAM多

订阅的话题

节点cartographer_node订阅的话题是：scan, tf, tf_static。 cartographer_node的launch需要增加remap，因为它接受的话题名称和我们现有的话题可能不一致：

1
2
3

<remap from="scan"  to="/scan" />
<remap from="odom"  to="/xqserial_server/Odom" />
<remap from="imu"   to="/xqserial_server/IMU" />

cartographer_node 发布的话题

• scan_matched_points2, 类型sensor_msgs/PointCloud2, scan-to-submap匹配的2D点云数据，这个云可以根据配置进行滤波和投影。是scan话题的大部分，不会包括行走的人的轮廓

• submap_list, 类型cartographer_ros_msgs/SubmapList，所有轨迹的所有submaps的列表，成员：

  1 2 3 4 5 6 7 8

  std_msgs/Header header cartographer_ros_msgs/SubmapEntry[] submap # 其中submap的类型如下 int32 trajectory_id int32 submap_index int32 submap_version geometry_msgs/Pose pose

  话题包括一个子图的ID list(包括轨迹ID和子图index)，以及子图的全局位姿。

发布话题的源码流程是 Node::PublishSubmapList —— MapBuilderBridge::GetSubmapList() —— PoseGraph2D::GetAllSubmapPoses() —— PoseGraph2D::GetSubmapDataUnderLock —— PoseGraph2D::ComputeLocalToGlobalTransform

• trajectory_node_list，类型为visualization_msgs/MarkerArray，即轨迹路径node列表，代码是

1
2
3
4
5
6
7
8
9
10

void Node::PublishTrajectoryNodeList(
    const ::ros::WallTimerEvent& unused_timer_event)
{
  if (trajectory_node_list_publisher_.getNumSubscribers() > 0)
  {
    carto::common::MutexLocker lock(&mutex_);
    trajectory_node_list_publisher_.publish(
        map_builder_bridge_.GetTrajectoryNodeList()  );
  }
}

• constraint_list: 类型为visualization_msgs/MarkerArray，约束列表

• landmark_poses_list: 类型为visualization_msgs/MarkerArray，路标点位姿列表

节点 cartographer_occupancy_grid_node 发布话题 map(栅格地图)， 订阅submap_list [cartographer_ros_msgs/SubmapList]。 cartographer_occupancy_grid_node 节点将子图转换为ROS的地图格式

服务

• submap_query (cartographer_ros_msgs/SubmapQuery)： 查询submap的服务，输入的是trajectory_id 和 submap_index
  
  

start_trajectory

类型cartographer_ros_msgs/StartTrajectory，通过将其传感器主题和轨迹选项指定为二进制编码的原型来启动另一个轨迹。返回指定的轨迹ID。

StartTrajectory.srv:

1
2
3
4
5
6
7
8

string   configuration_directory
string   configuration_basename
bool   use_initial_pose
geometry_msgs/Pose   initial_pose
int32   relative_to_trajectory_id
---
cartographer_ros_msgs/StatusResponse   status
int32   trajectory_id

命令稍微长了点:

1
2
3
4
5
6
7

rosservice call /start_trajectory "configuration_directory: '/home/user/carto_ws/src/cartographer_ros/cartographer_ros/configuration_files'
configuration_basename: 'localization.lua'
use_initial_pose: true
initial_pose:
  position: {x: 5.211, y: 5.617, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: 0.689, w: 0.725}
relative_to_trajectory_id: 0"

出错的地方可能会在relative_to_trajectory_id：

1
2
3
4
5
6
7

status:
  code: 5
  message: "Trajectory 6 doesn't exist."

status:
  code: 3
  message: "Trajectory 5 is in 'DELETED' state."

加载 frozen state 意味着不插入no pose graph constraints. 一条frozen trajectory和它的子图不受 optimization影响

finish_trajectory

类型cartographer_ros_msgs/FinishTrajectory: Finish 一个给定ID的轨迹，方式是运行最终的图优化。

1
2
3

int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

命令rosservice call finish_trajectory "trajectory_id: 5"，得到这样的结果:

1
2
3
4

status:
  code: 3
  message: "Topics are already used by another trajectory."
trajectory_id: 0

trajectory_query

• get_trajectory_states (cartographer_ros_msgs/GetTrajectoryStates): 返回所有轨迹的ID和状态， 可以用于从单独的节点观察Cartographer的状态。

  1 2 3 4

  --- cartographer_ros_msgs/StatusResponse status cartographer_ros_msgs/TrajectoryStates trajectory_states

命令rosservice call /get_trajectory_states "{}"，比如得到这样的结果：

1
2
3
4
5
6
7
8
9
10
11
12

status:
  code: 0
  message: ''
trajectory_states:
  header:
    seq: 0
    stamp:
      secs: 1612232712
      nsecs:  41338148
    frame_id: ''
  trajectory_id: [0, 1, 2, 3, 4, 5]
  trajectory_state: [2, 2, 2, 2, 2, 0]

trajectory_state的枚举值也就是 轨迹的四种状态对应: ACTIVE, FINISHED, FROZEN, DELETED

• write_state (cartographer_ros_msgs/WriteState): 将当前内部状态写入磁盘到文件名。用于保存pbstream文件，此文件可用作assets_writer_main的输入，以生成概率网格，X-Rays或PLY文件等资源

read_metrics

类型(cartographer_ros_msgs/ReadMetrics)， 返回Cartographer的所有内部指标的最新值。运行时度量标准的集合(collection of runtime metrics)是可选的，必须使用节点中的-collect_metrics命令行标志激活:

1
2
3
4
5
6
7
8
9

<node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename map.lua
          -collect_metrics true"
      output="screen">
  <remap from="scan" to="/scan_rectified" />
  <remap from="odom" to="/odom" />
</node>

如果没有加-collect_metrics标志，rosservice call /read_metrics "{}"的结果:

1
2
3
4
5
6
7

status:
  code: 14
  message: "Collection of runtime metrics is not activated."
metric_families: []
timestamp:
  secs: 1612333336
  nsecs: 359502516

需要的tf变换

必须提供从所有传入传感器数据帧到配置的tracking_frame和published_frame的转换。通常，这些是由robot_state_publisher或static_transform_publisher定期发布的。

发布的tf变换

提供map_frame和 published_frame之间的转换。如果在Lua中启用了provide_odom_frame，则将提供配置的odom_frame和published_frame之间的连续（不受循环闭包影响）转换。

其他节点

• cartographer_rosbag_validate

可以检测bag中的错误，使用：

1

rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>

1
2
3
4
5
6
7
8
9
10
11
12
13

E0722 22:11:01.138546  3442 rosbag_validate_main.cc:389] IMU data (frame_id: "imu") has a large gap, largest is 0.064731 s, recommended is [0.0005, 0.005]s with no jitter.
I0722 22:11:01.139314  3442 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/IMU_data" (frame_id: "imu"):
Count: 27679  Min: 0.000144  Max: 0.064731  Mean: 0.005027
[0.000144, 0.006602)    					    Count: 27445 (99.154594%)    Total: 27445 (99.154594%)
[0.006602, 0.013061)                            Count: 181 (0.653925%)  Total: 27626 (99.808517%)
[0.013061, 0.019520)                            Count: 42 (0.151740%)   Total: 27668 (99.960258%)
[0.019520, 0.025979)                            Count: 7 (0.025290%)    Total: 27675 (99.985550%)
[0.025979, 0.032437)                            Count: 3 (0.010839%)    Total: 27678 (99.996384%)
[0.032437, 0.038896)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.038896, 0.045355)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.045355, 0.051814)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.051814, 0.058272)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.058272, 0.064731]                            Count: 1 (0.003613%)    Total: 27679 (100.000000%)

• cartographer_pbstream_map_publisher

一个简单的节点，它从序列化的Cartographer状态（pbstream格式）创建静态占用网格。如果实时更新不重要，它是占用网格节点的有效替代方案。 发布话题：map (nav_msgs/OccupancyGrid)，只发布一次