最近重读了《Effective C++》，对一些复杂的知识点会专门写文章分析，这里将一些小的知识点总结一下，但不会涵盖书里的所有内容。

条款2

• 良好的用户自定义类型的特征是它尽量能与内置类型兼容

• 宁可用编译器替换预处理器

• 尽量多用const,而不是宏定义.宏属于模块化的设计概念,会破坏封装性

• 常量可能比#define产生较小量的代码.

• 如果类需要用到多个常量,不要用const数组,而是改用枚举

• 对于宏形式的函数,最好用内联函数取代

条款3

• 希望迭代器指向的东西不可改动,需要const_iterator

条款4

• 永远在使用对象前初始化.对于int x;,有些编译器会将x初始化为0,有些却不会,所以无论之后有没有用到x,都要先对其初始化,比如int x=0;

• 成员变量如果是const或引用,它们只能用成员列表初始化的方法进行初始化

条款5

• 编译器自动生成的析构函数不是virtual,除非这个类的基类的析构函数是virtural

• 编译器自动生成的4个函数都是public

• 如果基类的copy构造函数和copy运算符是private,编译器不会为派生类生成它们

• 派生类的copy构造函数可以尽量不定义

条款6

• 定义uncopyable类是很好的禁止使用copy构造函数和copy运算符的方法，它的析构函数可以不是virtual,派生类不必以public继承它,也没有成员变量,也可以用于多重继承

• 类的不可拷贝特性是可以继承的，例如凡是继承自uncopyable的类都不能使用copy构造函数和赋值运算符

条款7

• 如果基类析构函数不声明为virtual，析构时不会调用派生类的析构，可查看原因分析

• 如果一个类不做基类，就不要有virtual析构函数或其他虚函数，因为虚指针会增大类的体积。 反过来，只要一个类做基类，就要有virtual函数

• 当类至少有一个虚函数时，为它声明virtual析构函数，否则编译器有报警
  

• 对于抽象类，可以将析构函数声明为pure virtual析构函数

条款8

• 析构函数里不要抛出异常，否则会导致不确定行为

条款9

• 构造函数和析构函数中都不要调用virtual函数，因为基类构造时，virtual函数不会下降到派生类阶层，或者说此时的virtual函数还不是virtual函数

• 对于存在多个构造函数的情况，为避免代码重复，要把同样的代码放到一个函数里，比如init

条款10

• 赋值运算符(包括+= -= *=)的返回最好是return *this，这符合STL等标准库的风格

条款12

• 自定义copy构造函数或运算符时，需要复制所有的成员变量，如果少复制了，编译器不会报警或报错

• 派生类的copy构造函数或运算符，无法像平时那样对基类的private成员变量赋值，因为无法访问private的变量，只能显式调用基类的operator =， 比如： Base::operator=(obj);

条款15

• 多使用智能指针shared_ptr，它可以返回原始指针，显式方法是get()函数，隐式方法是取指针操作符->

条款16

• new和delete必须配对，[]必须都有或都没有。千万避免new没有[]，delete有[]，这会导致程序不停运行析构函数

条款18

• 有时的形参可以用wrapper类型，而不是内置类型

• 保证接口的一致，比如STL容器都有个size的成员函数

• 可以让一些返回指针的函数返回智能指针，比如工厂函数

条款21

• 函数内返回对象时，不要返回其引用。因为引用指向局部变量，而局部变量在函数退出前销毁，所以会出现无定义行为。

条款22

• 成员变量应该都是private，将它们隐藏在函数接口的背后，如果放到public，直接使用成员变量会降低封装性。如果破坏了成员变量，会破坏太多的客户码

• 成员变量的封装性和成员变量的内容改变时破坏的代码量成反比

• protected变量被消灭时，所有用到它的派生类的代码都要破坏，所以封装性也很差

条款24

• 若函数参数都要进行类型转换，应该使用非成员函数

• 成员函数的反面是非成员函数，不是friend函数，friend应该尽量避免

条款26

• 如果某个类的对象没有用到，就不应该声明，否则运行构造和析构函数都会耗费成本

• 对象初始化用构造函数比=运算符的效率高

• 对变量定义后应马上初始化，然后马上使用，中间不要隔太远

• 变量定义在循环内比外面更好，后者造成作用域更大，对程序维护性不好

条款27

• 代码中尽量避免dynamic_cast，它会降低效率

• 避免连续的cast转型，尤其是dynamic_cast

条款32

• public继承是一种is-a的关系，每一个派生类的对象也是基类的对象

• 程序的错误最好能在编译期检测出来，而不是运行期

条款34

• public继承涉及到函数接口继承和实现的继承

• 纯虚函数实际上是可以提供定义的，这个用法比较罕见，知道即可

• 虚函数使派生类继承了其缺省实现，但这可能造成危险

条款36

• 不要重新定义继承而来的non-virtual函数，虽然没有错误，但违反了is-a原则，这种情况下干脆不要使用public继承

根据 Effective C++，类的成员变量最好 都用成员列表初始化 的方法进行初始化，对内置类型没什么不同，但对类对象会提高效率。 成员初始化顺序是其声明的顺序,不是初始化列表中的顺序,二者最好一致

构造函数内赋值成员变量，先调用默认构造函数，再调用赋值运算符。 如果用成员列表初始化， 只调用拷贝构造函数，提高了效率

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

class Derived : public Base
{
public:
    Derived();
    Derived(Base obj)
    {
        m_b = obj;
        cout<<"derived constructor"<<endl;
    }
    ~Derived();
    Derived(const Derived& obj);
    Derived& operator=(const Derived& obj);

private:
    Base m_b;
};

实际调用:

1
2
3

Base b;
cout<<"----------------"<<endl;
Derived d(b);

运行结果:

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

base constructor
----------------
base copy constructor
base constructor
base constructor
base operator =
derived constructor
base destructor
----------------
derived destructor
base destructor
base destructor
base destructor

但实际中一般不这么用，Derived的成员变量会是Base* m_b，此时的调用是这样的:

1
2
3
4
5

Base *b = new Base();
Derived d(b);

delete b;
b = NULL;

结果是:

1
2
3
4
5
6
7
8

base constructor
----------------
base constructor
derived constructor
base destructor
----------------
derived destructor
base destructor

• 在类成员函数后面加const关键字用于声明const成员函数，它不能改变类的成员变量，也不能调用该类中普通的成员函数，最好不要涉及非const的成员变量

• const成员变量只能通过成员列表初始化进行初始化，任何成员函数均不能改变它的值

• 只能调用const成员函数访问const对象，那是它的唯一对外接口

• 若不希望传入的实参对象被修改，应该将形参定义为const的指针变量或者引用

const成员函数的定义是这样形式：

1

int getConst() const;

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

class Base
{
public:
	void setM(int M)	//setter都不能加const
	{
		m = M;
	}
	int getM() const
	{
		return m;
	}
private:
	int m;
}

const Base b;
b.setM(2);

getM就是常量成员函数，如果给setM加const，那么就会报错，因为它修改m的值。定义的常对象b只能调用getM，调setM会报错。

本问题是Effective C++的条款20

提高代码效率

我们知道函数的实际参数是实参的一个副本，它是由copy构造函数产生的，但这种传const引用的方式可以提高函数调用效率，它不会涉及副本的构造函数、析构函数和copy构造函数

测试代码如下，对基类定义了copy构造函数，但派生类没有定义copy构造函数，信息中会输出this指针的地址

1
2
3
4
5
6
7
8
9

void useDerived(const Derived& obj)
{
    cout<<" use Derived"<<endl;
}

Derived f;
cout<<"-------------"<<endl;
useDerived(f);
cout<<"*************"<<endl;

运行结果：

1
2
3
4
5
6
7

Base constrct  0x75fd20
Derived construct  0x75fd20
-------------
 use Derived
*************
Derived deconstruct  0x75fd20
Base deconstrct  0x75fd20

如果改用值传递的方式，结果就变成下面这样：

1
2
3
4
5
6
7
8
9
10

Base constrct  0x75fd10
Derived construct  0x75fd10
-------------
Base copy constrct  ---0x75fd40
 use Derived
Derived deconstruct  0x75fd40
Base deconstrct  0x75fd40
*************
Derived deconstruct  0x75fd10
Base deconstrct  0x75fd10

避免对象切割问题( object sliced )

如果上面的函数改成这样：

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

void useDerived(Base f)
{
    f.testConst();
    cout<<"value use Derived"<<endl;
}

// testConst声明为虚函数
void Base::testConst() const
{
    cout << "base test const" <<endl;
}
void Derived::testConst() const
{
    cout << "derived test const" <<endl;
}
// 调用
Derived d;
useDerived(d);

结果为：

1
2
3
4
5
6
7
8
9
10

base constructor
derived constructor
# 开始副本
base copy constructor
base test const     // 基类版本
value use Derived
base destructor

derived destructor
base destructor

不过对于内置类型和STL的iterator，函数对象，还是用值传递的方式比较好

根据概率论知识，SLAM工作可以划分为定位和建图两部分，一般是通过观测值$z_{1:t}$和里程计信息$u_{1:t-1}$进行定位得到$x_{1:t}$，然后根据位姿 $x_{1:t}$ 以及观测信息进行建图得到m。

gmapping其实就是优化版的RBPF，先看RBPF的一般过程：

1. 时间t的粒子群 $X^ \left( i \right)_t$ 是 $X^ \left( i \right)_{t-1}$ 从提议分布中采样获得的，提议分布往常是里程计运动模型 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$
2. 粒子权重 $w^ \left( i \right)$ 是目标分布和提议分布的比，用来描述提议分布和目标分布的差别，越接近1说明提议分布越好
3. 重采样：只有部分粒子用于估计目标分布，重采样之后，所有粒子权重相同。 如果能直接从目标分布采样，所有粒子权重都相同，实际上只用1个粒子就行，或者说没有采样的概念了。因此，提议分布越差，粒子群的权重差别越大， $N_{eff}$ (粒子归一化权重的平方和的倒数)就越大。
4. 根据轨迹各个位姿 $X^ \left( i \right)_{1:t}$ 和各个观测 $Z^ \left( i \right)_{1:t}$ 估计地图

• 目标分布： 真实分布，如果考虑机器人的运动，那么机器人在t时刻位置的真实分布就是目标分布，但是这是不能得到的，只是理论上的分布，我们需要只能通过其他分布来得到该分布

• 提议分布: 使用提议分布来得到目标分布的真实情况。一般的，使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性，方差更小，更适合作为提议分布，因此考虑激光和里程计一起作为改进的提议分布

gmappping的优化

优化提议分布

不仅考虑机器人的运动，而且考虑了最近的观测，使得提议分布更接近目标分布，从而减少了所需的粒子数。之前的提议分布是 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$，优化之后是 $P(x{_t}\ |x_{t-1}\ , \ u_{t-1}\ , \ m_{t-1}\ , \ z_t)$

如果只用运动模型做提议分布，粒子权重计算比较简单: $w^ \left( i \right)_t \propto w^ \left( i \right)_{t-1}\ \cdot p(z_t\ | m^ \left( i \right)_{t-1}\ , x^ \left( i \right)_t)$。 但是里程计提供位姿的不确定度比激光大得多，导致置信区间太大，似然又太小，各个粒子的权重差别很大，实际上有意义的只有一小部分，结果就需要很多粒子。

相比之下，雷达观测的置信区间小，似然大，所需粒子就少了很多。所以可以把观测加入到提议分布优化之后，结果粒子权重公式为公式 13

因为观测似然函数的不确定形态，提议分布没有闭式解，所以方法是用采样对提议分布做估计。

减少重采样

RBPF中，粒子要覆盖里程计状态的全部空间，但只有一部分粒子是接近目标分布的，粒子权重的差别大，所以需要频繁重采样来丢弃权重小的粒子，保留权重大的粒子，同时又造成了另一个弊端：粒子退化。

选择性重采样，当 $N_{eff}$ 小于粒子数一半时，说明粒子分布和真实分布差距很大，某些粒子离真实值近，某些远，执行重采样。

GMapping依靠粒子的多样性， 回环时消除累积的误差。起始位置需要选择特征较为丰富的地方， 这样能在回环时提高正确粒子权重。

1. 使用scan-matcher估计似然函数的置信区间
2. 对置信区间采样，对采样点进行评价
3. 对每个粒子计算，计算K个粒子的期望方差，计算时考虑了里程计运动信息，公式15和16

算法复杂度

主要影响因素是计算提议分布，更新地图，计算权重，检测是否需要重采样，重采样。 结合源码，最大的影响参数是particles, 其次是resampleThreshold, map_update_interval, throttle_scans, lskip

$N_{eff}$

1. 机器人经过未知区域时，$N_{eff}$ 下降很慢
2. 机器人经过已知区域时，每个粒子都在各自的地图中保持定位，权重都相差不大
3. 当closing loop时， $N_{eff}$ 会显著下降，有些粒子能保持定位，有些则丢失了定位，权重相差很大，此时执行重采样

缺陷

从论文和源码上看，gmapping算法有以下缺陷:

1. scan matcher 计算的是观测似然函数的置信区间，但函数可能是多峰的，比如closing loop的时候，这就造成了问题，粒子群的权重会有很大波动， $N_{eff}$ 会显著下降，当低于resample_Threshold时，就会执行重采样，然后恢复 $N_{eff}$ 最大值． $N_{eff}$ 之所以大幅下降，就是因为观测似然出现多峰，使得提议分布与目标分布差别太大，导致粒子权重减小．因此不适用于多闭环的环境

2. 遇到长走廊或很空旷的环境，雷达数据没有什么特征，比如测距都是最大值，造成位姿在走廊方向的严重不确定，此时里程计精度就很重要了，有利于粒子群的收敛。如果里程计精度不好，增加粒子数也可以解决这个问题。

3. drawFromMotion函数是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《概率机器人》中提到的速度模型和里程计模型，有很多方面都没有考虑，精度上可能有折扣。

4. 随着场景增大所需的粒子也增加，内存和计算量都会增加，实际中建的地图不到几千平米时就会发生错误

5. 重采样的粒子索引选取用的是轮盘赌算法，有些论文提出了一些更好的算法

6. 机器人的轨迹估计与真实的轨迹有一定差异，轨迹出现较大畸变的地方导致了＂假墙＂．这是粒子多样性降低造成的，需要增加粒子数
   

7. 源码中，用户设置的miniScore参数过大时(超过170)，scan match失败，转而使用里程计进行位姿估计．但这样就更不准了，而且gmapping算法里没有提示

综合来看，Gmapping适合的环境有以下特点: 小场景、closing loop比较少、没有长走廊或很空旷的环境、雷达精度高、机器人主机配置较高。此时相比Cartographer，Gmapping不需要太多的粒子并且没有回环检测，因此计算量小于Cartographer，精度没有差太多。

参考：
各种激光slam: 开源代码的比较
2D激光SLAM算法优劣对比

用于估计的二轮差速里程计模型

二轮差速模型的航迹推演原理图，后方两个驱动轮，前面两个万向轮

• IMU只提供yaw
• 根据航迹推演，两驱车的线速度是两个轮子线速度的平均值，角速度是 差/底盘长度;两个参数作为raw_vel话题发布
• 机器人切线运动模型
  base_controller节点正确读取到底层（比如嵌入式控制板）传回的速度后进行积分，计算出机器人的估计位置和姿态，并将里程计信息和tf变换发布出去。
• 机器人中通过运行底盘控制ROS驱动，来实现读取串口的速度反馈，利用航迹推演算法计算得到里程计并发布到/odom这个主题

之所以说这个是粗略的定位，是因为在实际情况中可能会碰到轮子打滑，地面不平整等因素的干扰，里程计的运动增量带有噪声，对速度积分进行航迹推算得到的里程计累积误差会越来越大。当然上层会通过激光信息来匹配校准。

IMU姿态的协方差矩阵代表了姿态测量的不确定度。

用于规划的速度模型

IMU线加速度噪声积分后根本不能用做线速度

IMU在制造过程中，由于物理因素，导致实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。自校准就是要通过给的补偿值来减小坐标轴的偏差及原始值的固定偏差，也就是所谓的IMU内参标定，是为了获得噪声参数。

如果将IMU安装到机器人或摄像头上后，需要知道IMU与机器人或摄像头的相对位置，这个时候进行的标定就是所谓的IMU外参标定

通过IMU获得的是odom坐标系下的坐标，sensor_msgs/Imu消息：

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

# 保存IMU的数据
# 加速度单位 m/s^2 (not in g's), 角速度单位 rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# 如果协方差矩阵元素全是0，那么认为是未知协方差。
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

Header header
geometry_msgs/Quaternion orientation   # IMU的当前姿态，4元数的形式表示
float64[9] orientation_covariance   # 姿态的协方差

geometry_msgs/Vector3 angular_velocity  	 # IMU的3轴角速度
float64[9] angular_velocity_covariance

geometry_msgs/Vector3 linear_acceleration  	 # IMU的3轴加速度
float64[9] linear_acceleration_covariance

滤波工具

ROS官方提供了滤波工具，可以直接安装：sudo apt-get install ros-kinetic-imu-tools，包括两个package和一个rviz插件：

• imu_filter_madgwick: 将IMU设备读取的角速度、加速度和磁力计融合成方位信息

• imu_complementary_filter: 一种基于强制融合的新方法将IMU设备读取的角速度、加速度和磁力计融合成方位信息。

• rviz_imu_plugin：显示sensor_msgs::Imu的rviz插件

两种滤波器都是订阅IMU节点发布的imu/data_raw话题，经过滤波处理后，再发布imu/data话题。但是我用两种滤波器处理IMU数据后，没有发现明显的改善，也许是设备的原因，本来数据浮动就不大

参考：imu_tools

目前导航功能包只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息类型发布的传感器数据。

sensor_msgs/LaserScan和 sensor_msgs/PointCloud跟其他的消息一样，包括tf帧和与时间相关的信息。字段frame_id存储与数据相关联的tf帧信息。以激光扫描为例，它将是激光数据所在帧。

我们使用laser_geometry包将雷达的scan转换为点云，它只有一个C++类LaserProjection，没有ROS API.

有两个函数可以将sensor_msgs/LaserScan转换为sensor_msgs/PointCloud或者sensor_msgs/PointCloud2。 projectLaser()简单快速，不改变数据的frame; transformLaserScanToPointCloud()速度慢但是精度高，使用tf和sensor_msgs/LaserScan的time_increment转换每束激光，对倾斜的雷达或移动的机器人，推荐这种方法。

sensor_msgs/PointCloud还包括一些额外的通道，例如intensities, distances, timestamps, index 或者thew viewpoint
支持将三维空间中的点的数组以及任何保存在一个信道中的相关数据。例如，一条带有intensity信道的 PointCloud 可以保持点云数据中每一个点的强度。

projectLaser

它执行最简单的激光投射，每束激光投射出去的角度根据的是以下公式：

但最后形成的点云消息，坐标系还是laser，不适用于雷达移动或畸变不能忽略的情况。

transformLaserScanToPointCloud

这种方法需要tf变换，由于我们是扫描过程中一直收集数据，选择的target_frame必须是固定的。因为sensor_msgs/LaserScan的时间戳是第一次测量的时间，这个时间还无法转换到target_frame，得等到最后一个雷达的测量时间完成坐标系转换。

代码

依赖包是laser_geometry, pcl_conversions, pcl_ros
修改package.xml文件，添加

1
2

<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

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

#include "ros/ros.h"
#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "tf/transform_listener.h"
#include <boost/shared_ptr.hpp>

laser_geometry::LaserProjection  projector;
ros::Publisher pub;
boost::shared_ptr<tf::TransformListener> listener_;

void chatterCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    sensor_msgs::PointCloud2 cloud;
    // projector.projectLaser(*msg, cloud);    // 第一种方法

    if(!listener_->waitForTransform(
                "/base_link",
                msg->header.frame_id,
                msg->header.stamp + ros::Duration().fromSec(msg->ranges.size()*msg->time_increment),
                ros::Duration(2.0)) )
    {
        ROS_WARN("no transform");
        return;
    }
    projector.transformLaserScanToPointCloud("/base_link",*msg,
                                             cloud, *listener_);
    pub.publish(cloud);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "laser_toCloud");
    ros::NodeHandle nh;
    listener_ = boost::make_shared<tf::TransformListener>();
    pub = nh.advertise<sensor_msgs::PointCloud2>("cloud",10);
    ros::Subscriber sub = nh.subscribe("/scan", 1000, chatterCallback);   // 先有发布的scan话题

    ros::spin();
    return 0;
}

参考：
激光LeGO-LOAM生成pcd点云地图保存和格式转换
LaserScan转pcl::PointCloud
laser_geometry