一般是两种方法

1
2
3
4
5

x=[-20:0.01:20]

%y2 = pdf('Normal',x,1,5)
y = gaussmf(x,[1 5]);
plot(x,y2)

深蓝色区域是距平均值小于一个标准差之内的数值范围。在正态分布中，此范围所占比率为全部数值之68%，根据正态分布，两个标准差之内的比率合起来为95%；三个标准差之内的比率合起来为99%。

在AMCL中，里程计是作为状态预测器存在的，通过接受当前的控制信号，从上一帧机器人状态对这一帧机器人状态进行预测，并与当前观测的结果对当前预测进行加权打分。所以通过输入和输出我们知道，里程计在AMCL中的作用就是根据当前控制信号更新上一帧的能表征机器人状态的粒子集合。

给机器人一个初始位姿，粒子才根据初始位姿进行初始化分布，才会进行更新(UpdateAction函数)，否则不会有amcl_pose话题输出，而更新依赖于读取到传感器数据发生变化，如在amcl中里程计的数据变化等，也就是机器人的运动

工具函数和类

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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

/* 两个处理角度的函数，保证角度都在(-π，π] 的范围内 */

// 比如 30°的弧度,代入后基本还是得30°的弧度
// 但是对于210°, 代入得到的是-150°的弧度值
static double normalize(double z)
{
  return atan2(sin(z),cos(z));
}

// 对于240和30的弧度,返回-150的弧度
static double angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}

// 向量，成员为[x,y,θ]
typedef struct
{
  double v[3];
} pf_vector_t;

// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
  // Odometric pose
  public: pf_vector_t pose;

  // Change in odometric pose
  public: pf_vector_t delta;
};

// 单个粒子的采样信息，只有位姿和权重
typedef struct
{
  // Pose represented by this sample
  pf_vector_t pose;

  // Weight for this pose
  double weight;
  
} pf_sample_t;


// 从零均值的高斯分布获得随机值，标准差为sigma
// We use the polar form of the Box-Muller transformation
double pf_ran_gaussian(double sigma)
{
  double x1, x2, w, r;
  do
  {
    do { r = drand48(); } while (r==0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r==0.0);
    x2 = 2.0 * r - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);
 
  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

laserReceived中的准备工作

下面来看回调函数laserReceived中的运动模型准备工作，我们需要的是odata，它的类型就是上面的AMCLOdomData，以delta成员为例，它的赋值顺序是： odata.delta <—- delta <—- pose(当前位姿)和pfodom_pose(上一次位姿)

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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

// 获得雷达数据时刻的odom坐标系中的坐标,赋值给pose
pf_vector_t pose;
if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
              laser_scan->header.stamp, base_frame_id_))
{
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
}

pf_vector_t delta = pf_vector_zero();
// 完成初始化后执行
if(pf_init_)
{
// 位姿相对上一次的变化， pf_odom_pose_就是上一时刻的位姿
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

// 是否更新粒子滤波器，位移需要大于参数update_min_d， 角度需要大于参数update_min_a
bool update = fabs(delta.v[0]) > d_thresh_ ||
              fabs(delta.v[1]) > d_thresh_ ||
              fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;

// Set the laser update flags
if(update)
  for(unsigned int i=0; i < lasers_update_.size(); i++)
    lasers_update_[i] = true;
}

bool force_publication = false;
// 第一次执行laserReceived时执行, 之前的handleMapMessage里还是false
// 如果里程计的数据显示机器人已经发生了明显的位移或者旋转，标记所有的雷达更新为true

// 难道这里只在开始运行一次 ???
if(!pf_init_)
{
    // 上一次滤波器更新的位姿 
    pf_odom_pose_ = pose;

    // 滤波器现在初始化了
    pf_init_ = true;

    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;

    force_publication = true;
    resample_count_ = 0; 	// 重采样次数
}
// 刚启动amcl时不执行,机器人移动后执行, 更新滤波器
else if(pf_init_ && lasers_update_[laser_index])
{
    AMCLOdomData odata;
    odata.pose = pose;
    odata.delta = delta;

    // 上一帧位姿粒子状态pf和当前传感器信息，更新滤波器
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
}
bool resampled = false;

到此就是算法的准备工作了, pf_是粒子滤波器，在handleMapMessage中创建和初始化

UpdateAction

接下来就是运动模型的算法了，对应《概率机器人》103页：

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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67

// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  AMCLOdomData *ndata;
  ndata = (AMCLOdomData*) data;

  // Compute the new sample poses
  pf_sample_set_t *set;
  // 确立当前的粒子集合，我们要产生的是一堆有位姿的粒子
  set = pf->sets + pf->current_set;
  // 上一时刻的位姿
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
  // 与103页的各个delta量对应
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;

    // 相较于于书上，在计算delta _rot1时，多了个判定条件以位移是否小于1cm作为判断条件
    // 如果里程计量得机器人位移小于1cm，delta _rot1就置为0
    if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
            ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
      delta_rot1 = 0.0;
    else
      delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
                              old_pose.v[2]);
    delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                       ndata->delta.v[1]*ndata->delta.v[1] );
    delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);

    // We want to treat backward and forward motion symmetrically for the
    // noise model to be applied below.  The standard model seems to assume forward motion.
    // 取了两次旋转角的锐角
    delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
                                fabs(angle_diff(delta_rot1,M_PI)));
    delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
                                fabs(angle_diff(delta_rot2,M_PI)));

	// 用一定数量的粒子来表征这个区域，然后对这些粒子进行采样来尽可能多的逼近机器人的真实值
	// 下面还是书上的公式，我们需要的是sample
    for (int i = 0; i < set->sample_count; i++)
    {
        // 指针形式, 会给set的samples都赋值
      pf_sample_t* sample = set->samples + i;

      // Sample pose differences，pf_ran_gaussian函数：从零平均高斯分布中随机抽取，带有标准差sigma
      // 将这两个角度带入pf_ran_gaussian()进行高斯采样，也就是书103页的sample()函数。
      // 对每个粒子，不同的信息就是高斯采样产生的，其他部分不变
      delta_rot1_hat = angle_diff(delta_rot1,
                      pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise + this->alpha2*delta_trans*delta_trans)  );

      delta_trans_hat = delta_trans - 
              pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
                              this->alpha4*delta_rot1_noise*delta_rot1_noise +
                              this->alpha4*delta_rot2_noise*delta_rot2_noise);

      delta_rot2_hat = angle_diff(delta_rot2,
                          pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                        this->alpha2*delta_trans*delta_trans) );

      // Apply sampled update to particle pose, 对初始预测的修正
      sample->pose.v[0] += delta_trans_hat * 
              cos(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[1] += delta_trans_hat * 
              sin(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
    }
}

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除外)，因为不需要做内存拷贝和内存移动