使用json配置realsense的参数(非rosparam)

今天使用realsense时，发现有个参数需要修改

这个参数可以修正因反光而导致的错误，也就是把稍微凸起的地面当做障碍物，适当减小这个参数可以实现。

这个界面在realsense viewer中，但问题是如何在ROS中做到，它在rosparam中没有对应的参数。查来查去，在github的issue里发现了解决方法。realsense已经提供了一种visual preset文件，其实是个json文件，可以对很多参数设置，而且realsense的launch文件中已经定义了json文件的路径，比如rs_camera.launch:

1

<arg name="json_file_path"      default=""/>

realsense提供的所有json文件在这里，我们需要的参数在DefaultPreset_D435.json，也就是第一个参数："aux-param-autoexposure-setpoint": "400"，修改它的值后，把json文件的路径填到上面launch参数那里，启动launch就可以实现了。不过不能动态调整参数。

配置点云的滤波器

rs_camera.launch的参数是比较全的，demo_pointcloud.launch比较少。

现在介绍filters这个参数，在launch中没有赋值:<arg name="filters" default=""/>

pointcloud: 点云话题是/camera/depth/color/points. 点云的texture可以在rqt_reconfigure里修改，或者用参数pointcloud_texture_stream和pointcloud_texture_index修改

The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting allow_no_texture_points to true.

可以配置的滤波器如下:

• disparity, convert depth to disparity before applying other filters and back.
• spatial，filter the depth image spatially.
• temporal，filter the depth image temporally.
• hole_filling，apply hole-filling filter.
• decimation，reduces depth scene complexity.

所有滤波器的说明在这里

给参数filters赋值，必须有pointcloud，再增加滤波器，用逗号隔开。比如 disparity,spatial,pointcloud

滤波器的相关的源码在base_realsense_node.cpp

• BaseRealSenseNode::setupFilters(), 读取参数filters，把所有滤波器名称都插入容器_filters

• BaseRealSenseNode::publishPointCloud(), 发布滤波后的点云

参考:
realsense-ros 滤波器
CSDN Realsense D435 Post-processing filters

ROS版本不兼容产生的TransformListenerWrapper问题

AmclNode类中有这样一段：

1
2
3
4
5

// Use a child class to get access to tf2::Buffer class inside of tf_
struct TransformListenerWrapper : public tf::TransformListener
{
   inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}
};

这是Kinetic的版本，但是在Melodic里没有这一段了，这是ROS版本兼容问题，在Kinetic里用不了这个结构体了。搜索发现getBuffer只有在AmclNode::runFromBag里调用，因为平时不用bag仿真时测试AMCL，所以这句话可以注释掉。

“map” passed to lookupTransform argument target_frame does not exist

解决方法： 先加上listener.waitForTransform，再lookupTransform

The listener’s buffer, which carries all information about recent transformation, is literally empty. Therefore, any transform which looks-up the buffer does not find the frames it needs. It is good practice to wait for some time after the listener has been created so that the buffer can fill up. But instead of just sleeping, tf use waitForTransform

参考:

机器人SLAM问题中涉及到平移和旋转，平移是在欧氏空间中，连续的平移变换只需要向量相加。 可是旋转是在非欧空间了，连续的旋转变换就是矩阵连续右乘

右手坐标系：x向前(roll)，y向左(pitch)，z向上(yaw)

用矩阵表示绕某轴旋转

旋转矩阵

坐标系A变换到B，从坐标系B到坐标系A的旋转矩阵的每一列，都是{B}的坐标轴单位向量在{A}中的表示

旋转矩阵R是一个行列式为1的正交矩阵，而且行向量和列向量的长度都为1，所以它的转置就是它的逆，而逆矩阵就可以将坐标系旋转回来。
R的每一行就是{A}的坐标轴单位向量在{B}中的表示。
SO(n)是特殊正交群，这个集合由n维空间的旋转矩阵组成。 SO(3)指三维空间的旋转。

变换矩阵

左上角为旋转矩阵，右侧为平移向量，这种矩阵又称为特殊欧氏群

逆矩阵的形式 $\left[ \begin{matrix} R & t \\ 0 & 1 \\ \end{matrix} \right]^{-1} \ = \left[ \begin{matrix} R^T & -R^Tt \\ 0 & 1 \\ \end{matrix} \right]$

如果一个点的坐标是 $P(x,y,z)$，那么点作T变换后的坐标就写作 $R*P + t$，按齐次矩阵和坐标形式计算就可得到。

Censi 公式

我习惯用Censi论文的公式，还可以用Grisetti派系的

$\quad\quad\quad\quad\quad\quad\quad\Downarrow$

其实这里就是 $Rp + t$ ，只是在二维平面，R就是绕Z轴旋转的旋转矩阵形式。

左乘和右乘

左乘是行变换，右乘是列变换。这是因为习惯上来说，空间中的向量用列向量表示，用矩阵左乘列向量，就是把它在空间当中变换。

若绕静坐标系（世界坐标系）旋转，则左乘，也是变换矩阵 x 坐标矩阵。 在固定坐标系下，一个点 $p$ 经过一个旋转矩阵$R_1$，再经过一个旋转 $R_2$ ，总共旋转 $R_2 * R_1 * p$，这是左乘。 向量旋转中的右乘本没必要，甚至会引起人误解。

若是绕动态坐标系旋转（自身建立一个坐标系），则右乘，也就是坐标矩阵 x 变换矩阵。

即左乘是相对于坐标值所在的坐标系（世界坐标系）下的三个坐标轴进行旋转变换，右乘则是以当前点为旋转中心，进行旋转变换。

具体去看文章： 欧拉角，旋转矩阵，旋转向量，四元数

图优化里的相对位姿是由右乘获得的，即节点 $X_i$ 和 $X_j$ 之间的相对位姿是

常用的公式 $P_c=R_c∗P_w+t_c$ 是把点P从$w$坐标系转换到 $c$ 坐标系，由于旋转时，两个坐标系原点还是重合的，我们说经过了平移后，平移向量 $t_c$ 是从坐标系 $w$坐标系指向 $c$

参考：
cartographer中公式的推导

这个就是最常用的advertise函数的最后一个参数，原型： advertise (const std::string &topic, uint32_t queue_size, bool latch=false)

是否锁存。某些话题并不是会以某个频率发布，比如/map这个topic，只有在初次订阅或者地图更新这两种情况下，才会向/map发布消息，这里就用到了锁存。

锁存的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。

查看gmapping中的函数SlamGMapping::startLiveSlam()，发现三个话题都是 latched：

1
2
3

entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

latch=true，发布消息一次或几次，然后设法阻塞程序(常常用spin()，这里只是阻塞，跟订阅无关)，订阅者仍然能收到阻塞之前发布的消息。

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

ros::init(argc,argv, "Pub");
ros::NodeHandle nh;
ros::Publisher pub_latch_true = nh.advertise<std_msgs::Int8>("topic_latch_true", 50, true);
ros::Publisher pub_latch_false = nh.advertise<std_msgs::Int8>("topic_latch_false", 50, false);
ros::Rate rate(1);

std_msgs::Int8 msg;
msg.data = 2;
pub_latch_true.publish(msg);
pub_latch_false.publish(msg);
while(ros::ok())
{
  ros::spinOnce();
  rate.sleep();
}
return 0;

运行程序后等一会，运行rostopic echo topic_latch_false，什么也没有。运行rostopic echo topic_latch_true，会收到data=2的消息。 即使先运行两个echo，再运行发布者，结果也是如此

bool Publisher::isLatched () const 用于判断是否latch

协方差可以表示两个变量的协同关系， 变化趋势是否一致。 相关性就是用X、Y的协方差除以X的标准差和Y的标准差。

我们机器人用到的向量是(x,y,z, roll,pitch,yaw)，协方差就是一个6x6的矩阵，对角线的元素为各个随机变量的方差，非对角线的元素就是两两随机变量之间的协方差。AMCL中所用的协方差矩阵，由于z,pitch,roll都是0，矩阵的第1,8和最后的元素，分别是x,y,yaw的方差。另外还有x和y的协方差

如果协方差矩阵是单位矩阵，那么就是标准的多元高斯分布。由于协方差矩阵是对称矩阵，所以可以做特征分解

其实可以变成

因此任意一个协方差矩阵可以看做三维变换的结果，概率密度函数是由协方差矩阵的特征向量控制旋转，特征值控制尺度。 协方差矩阵就是根据这两项对标准高斯分布进行变换。

AMCL中的pf_ran_gaussian函数是以对角线元素为标准差，生成一个高斯分布。

如果点云分布非常规整，是某个特定物体的点云模型，则应该使用Octree，因为很容易求解凸包并且点与点之间相对距离无需再次比对父节点和子节点，更加明晰，典型的例子是斯坦福的兔子。

lego_loam建立的点云太稀疏，只能用于定位而不能导航。

用RGBD建立3D稠密点云图，并使用octomap进行压缩滤除地面信息。然后通过2D投影生成占据栅格地图最后利用costmap进行全局和局部路径规划导航实时避障，这又变成2D导航了

点云地图也有一些基于ICP或直接用八叉树地图的三维导航的方法，可以用于无人机在三维空间的导航。至于稀疏特征点地图，容易受光照和场景影响

点云地图: 无序,因为它的点都是无序的,无法坐标索引查询

八叉树地图特点

• 稀疏: 不需要对空间进行稠密切分
• 结构化: 方块排列固定,切分为八份
• 非直接索引查询

八叉树地图基于OctoTree，可以是多分辨率的地图

Voxel Grid就像3D的占用地图，分辨率是固定的

要想在地图中添加障碍，用virtual_layer和costmap_prohibition_layer都可以，效果一样，区别就是前者的yaml里需要加上朝向，其实是没必要的。下面以virtual_layer为例向代价地图加入新的障碍。

进入costmap_2d这个包，查看costmap_2d\costmap_plugins.xml文件，可以看到目前所拥有的层，基类都是costmap_2d::Layer。

配置代价地图

launch里加入

1
2

<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="local_costmap" />

最后是设置禁止区域坐标参数，首先需要在参数配置文件夹（global_costmap_params.yaml所在的文件夹）中创建新的文档，命名为prohibition_areas.yaml，然后输入:

1
2
3
4
5
6
7

costmap_prohibition_layer:
 prohibition_areas:
  - [[-1.3, -2.98],[-1.3, -16.77]]
  - [[13.31, -26.77],[2.88, -3.45]]
  - [[-4.96, -37.78],[-32.67, -37.90]]
  - [[-23.39, -33.43],[-11.89, -33.20]]
  - [[0.0, 6.89],[0.0, 38.05]]

1
2
3
4
5
6

virtual_layer:
    zone_topics: ["/zone"]
    obstacle_topics: ["/obstacle"]
    static_forms: [[[-18.46,-12.18,0.0],[-13.21,-12.18,0.0],[-13.21,-13.4,0.0],[-18.46,-13.4,0.0]],
                   [[-13.99,-0.17,0.0],[-11.87,-0.14,0.0]],
                   [[-13.18,-11.15,0.0],[-11.81,-11.15,0.0]]]

查来查去，怎么也找不到原因，最后是在两个代价地图的yaml里，把- {name: virtual_layer, type: "virtual_costmap_layer::VirtualLayer"} 放到膨胀层之前 ，问题解决，看来膨胀层的源码不完善。

注意事项：

1. 一定要严格按照上述格式来设置坐标，格式错误导致不能识别禁止区域坐标情形有: (1)坐标前的短横线没对齐 (2) 定义禁止区域或者禁止线，两坐标之间缺少了逗号
2. 可以同时定义多个点障碍／线障碍／障碍区域。对于3个及以上的点，形成的是封闭的区域障碍。
3. 禁止区域的坐标是map坐标系的
4. 由于Bresenham算法，形成的线可能看起来不够直
   

costmap_prohibition_layer

源码在costmap_prohibition_layer

CMakeLists里可以看到add_library(costmap_prohibition_layer src/costmap_prohibition_layer.cpp)， 结果在lib/libcostmap_prohibition_layer.so

看文件package.xml，最后有一句：

1
2
3

<export>
      <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
</export>

costmap_plugins.xml如下：

1
2
3
4
5

<library path="lib/libcostmap_prohibition_layer">
  <class type="costmap_prohibition_layer_namespace::CostmapProhibitionLayer" base_class_type="costmap_2d::Layer">
    <description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
  </class>
</library>

以上几项缺一不可，放到工作空间编译后，在终端中输入rospack plugins —attrib=plugin costmap_2d ，这个命令是显示当前costmap_2d能接受的所有插件，如果出现

1

costmap_prohibition_layer /home/user/plan_ws/src/costmap_prohibition_layer/costmap_plugins.xml

参考：
在Costmap_2d里面插入Prohibition_layer

以前我没注意这个问题，看了一个程序才想起来研究。

打开rviz，发现它发布的话题如下：

1
2
3
4
5

/clicked_point
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg

我们在rviz按下后两个按钮，就可以用echo获得对应点的位姿，注意当前的固定坐标系。如果有了地图，那就可以在地图上选起始点和终点了。

至于订阅的话题，就看在显示栏的添加的项目了。

IMU Tracker

1. imu_tracker_: 存放由IMU数据得到的信息，用一帧一帧IMU来推断当前时刻的机器人位姿
2. odometry_imu_tracker_: 存放由里程计得到的信息，表示AddPose到AddOdometryData之间（即 最新优化的位姿时刻 到 最新里程计数据时刻 之间的时间段）的姿态变化。 在AddOdometryData用于辅助计算线速度
3. extrapolation_imu_tracker_: 存放经过数据融合后的结果，表示AddPose到ExtrapolatePose之间（即 最新优化的位姿时刻 到 最新估计位姿时刻 之间的时间段）的姿态变化。 在ExtrapolatePose用于前端辅助实现激光运动畸变矫正。

重点看IMU Tracker。

一般IMU要安装在被测物体的重心上。imu的角速度测量精度往往比线速度好的多，因此在工程上，经常采用imu的角速度，角速度与时间积分计算角度，得到的角度变化量与初始角度相加，就得到目标角度，其中积分时间 $\Delta t$ 越小输出的角度越精确。但积分的累积误差会随着时间的而增加，所以陀螺仪只能工作在相对较短的时间内。

所以要得到较为真实的姿态角，就要利用一阶低通滤波扬长避短，在短时间内增加陀螺仪的权值，在更长的时间增加加速度的权值，这样系统输出角度就接近真实值了。

ImuTracker是使用角速度和线加速度(就是来自IMU数据) 跟踪 朝向和重力向量。 平均化后的线加速度(假设移动很慢)， 是直接测量为g倍数的，roll/pitch不会漂移，但是重力加速度会漂移，也就是说用IMU测量得到的话题/IMU/linear_acceleration/z不是固定的9.8

更新姿态 void Advance(common::Time time)

将姿态更新到时刻time。 通过计算time和上次的时间差，imu的角速度乘时间差得到角位移，原来的姿态右乘角位移得到新的姿态；同时更新时间和重力向量gravity_vector_

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

void ImuTracker::Advance(const common::Time time) 
{
   // 要求指定的时间要比当前时间晚或相等
  CHECK_LE(time_, time);
  // 求当前时间对指定时间的时间差
  const double delta_t = common::ToSeconds(time - time_);
  //假设为匀角速度运动的计算， 求得角度， 换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 在原朝向基础上继续旋转， 更新方位角
  // orientation_ 初始为 Eigen::Quaterniond::Identity()
  orientation_ = (orientation_ * rotation).normalized();

  /*  车体的旋转方向，跟重力向量的旋转方向是反向的，比如车体向上仰a角度，
  重力向量是往反方向即俯方向旋转a角度，
  conjugate 返回当前四元数的共轭四元数，就是逆旋转 */
  // gravity_vector_ 初始为 Eigen::Vector3d::UnitZ()  向量(0,0,1)
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

// 因为imu直接测得角速度，所以直接更新
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity)
{
  imu_angular_velocity_ = imu_angular_velocity;
}

更新重力向量

使用线加速度不断更新重力向量gravity_vector_，再根据重力向量更新 旋转矩阵和方位角。后面还会对齐点云方向

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

// 使用线加速度更新重力向量，使用了一阶低通滤波， 防止加速度跳动剧烈
void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) 
{
  // 当前时刻离上一次时刻的差， 初值是inf
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;
  // 计算权重, alpha为滤波系数, delta_t越大, 当前的权重越大. delta为inf时，权重为1
  // imu_gravity_time_constant_为滤波器常数， 默认为10
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  // 一阶低通滤波， gravity_vector_ 为滤波输出值， imu_linear_acceleration为采样值
  // 初始的输出值是 imu_linear_acceleration，如果没有IMU就是 (0,0,1)
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

  // 根据新的重力向量，更新对应的旋转矩阵, FromTwoVectors返回一个表示从向量a旋转到向量b的四元数
      // rotation 看做线加速度对上次线加速度的校正
    // orientation_.conjugate() * Eigen::Vector3d::UnitZ()就是把这个四元数转换成向量的形式
    // 在这里我们只关注重力方向上的值，即z轴上的值
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ() );
  // 更新方位角
  orientation_ = (orientation_ * rotation).normalized();
  // 如果之前的计算正确，二者的乘积应当是（0,0,1）
  CHECK_GT((orientation_ * gravity_vector_).z(),  0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(),  0.99);
}

这里的X就是线加速度，Y是重力向量，滤波系数$\alpha$ 通常远小于1，d为滤波器的常数10

从公式可以看出，时间差越大，$\alpha$ (当前线加速度的权重 )越大，当前线加速度的权重越大，上一次的重力向量的权重越小，应该是考虑到长时间内的重力向量数据不可靠

使用我的参数和IMU，发现alpha经常保持很精确的数值0.0009995，原因当然是delta_t

没看出这里如何避免传统一阶滤波的缺点，常说的动态调整滤波系数的要求：当数据快速变化时，滤波结果能及时跟进（灵敏度优先）；当数据趋于稳定，在一个固定的点上下振荡时，滤波结果能趋于平稳（平稳度优先）

其实还可以用卡尔曼滤波，效果更好，估计是因为运算负荷问题而没有使用

AdvanceImuTracker

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

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                       ImuTracker* const imu_tracker)  const 
{
  CHECK_GE(time, imu_tracker->time());
  // 当没有IMU数据时，输入一个假的imu数据
  if (imu_data_.empty() || time < imu_data_.front().time) 
  {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    imu_tracker->Advance(time);  // 根据时间进行imu数据插值
    // 给定一个理论上的重力向量
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    //优先选择里程计得出的角速度给imu
    //当里程计的数据小于2个时，选择从pose；否则选择里程计。因为里程计的精度更高
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_ );
    return;   // 没有IMU可以return了
  }
  //有imu时，此处调用了imu数据队列，ImuTracker维护的时间早于IMU数据队列最早的时间
  if (imu_tracker->time() < imu_data_.front().time)
  {
    // 先把ImuTracker更新到IMU数据来临的那一刻
    imu_tracker->Advance(imu_data_.front().time);
  }
  // 依次取出IMU数据队列中的数据，更新ImuTracker，直到IMU数据的时间比指定时间time要晚
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });
  // 用imu数据队列进行imu-tracker的数据更新.
  // imu_tracker主要负责对角速度进行插值, 并且估计重力向量
  while (it != imu_data_.end() && it->time < time)
  {
      imu_tracker->Advance(it->time);
      imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
      imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
      ++it;
  }
  imu_tracker->Advance(time);
}

参考：cartographer-位姿推测部分

位姿估计器就是用一段时间的位姿数据估计线速度和角速度，进而预测运动。

位姿估计器在Node中有一个（一条轨迹一个），在LocalTrajectoryBuilder也有一个。 Node中的估计器只有在Node::PublishTrajectoryStates有用，也就只有执行发布数据的时候才会将local_slam_pose添加到其中。所以重点看后者。

看LocalTrajectoryBuilder::AddRangeData函数，发现估计器会在两种情况下初始化：如果没在lua里启用IMU，就会在LocalTrajectoryBuilder2D里直接初始化；如果在lua里启用了，会在收到IMU数据后再初始化，否则一直等待，此时也不会处理里程计数据。

从Cartographer流程图可以看出，PoseExtrapolator输入为scan match反馈的位姿、 IMU、里程计三个数据源，估计出下一时刻的位姿，用来作为下次scan match的初值。

scan match可以获取较高精度的位姿，但是两次扫描匹配之间的运动只有里程计数据，如果里程计误差严重，将影响定位。

InitializeExtrapolator

初始化需要一个pose_queue_duration_(1ms) 和 重力加速度时间常数(10s)。
初始化之后立刻添加一个位姿为0的位姿。添加位姿的过程:实例化imu_tracker_，更新速度信息；处理imu和 odom 数据；初始化odometry_imu_tracker_和extrapolator_imu_tracker_都指向 imu_tracker_

1
2
3
4
5
6
7
8
9
10
11

if(extrapolator_ != nullptr)
    return;
CHECK(!options_.pose_extrapolator_options().use_imu_based());
// 创建一个PoseExtrapolator类型的位姿推理器
// pose queue 用于速度估计，pose_queue_duration是估计所用的时间
extrapolator_ = absl::make_unique<PoseExtrapolator>(
    ::cartographer::common::FromSeconds(
    options_.pose_extrapolator_options().constant_velocity().pose_queue_duration() ),
    options_.pose_extrapolator_options().constant_velocity().imu_gravity_time_constant()  );
// 添加一个初始位姿，Rigid3d::Identity()对变换矩阵赋值，位移为0，旋转为1
extrapolator_->AddPose(time, transform::Rigid3d::Identity() );

PoseExtrapolator 的三条线

1. imu_data_： 保存imu的双向队列 std::deque
2. odometry_data_： 保存里程计数据的双向队列 std::deque
3. timed_pose_queue_: 保存估计的位姿的双向队列 std::deque。只保存时间在 pose_queue_duration_范围内的位姿，旧的数据要删除掉。 位姿包括从IMU测量得到的Pose，以及从Scan Match输出的PoseObservation。

AddImuData 和 AddOdometryData

PoseExtrapolator管理里程计数据和经过ImuTracker的IMU数据，这两个函数的调用的最终源头是LocalTrajectoryBuilder2D::AddOdometryData和LocalTrajectoryBuilder2D::AddImuData

AddImuData比较简单，主要是把IMU数据加入imu_data_队列，清除旧的imu数据，保证队列中第2个数据的时间大于最后一个位姿的时间。AddOdometryData除了添加里程计数据，删除比最新位姿旧的odom数据，还根据首尾的odometry数据，计算tracking 坐标系下的线速度和角速度。

AddPose, ExtrapolatePose, EstimateGravityOrientation，这三个函数在LocalTrajectoryBuilder2D::AddRangeData中调用

ExtrapolatePose

目的是预测time时刻tracking坐标系在local坐标系下的位姿。假设传感器数据之间，机器人以恒定线速度和恒定角速度移动( local坐标系 )

估计的线速度有2种方式组成，优先级参见ExtrapolateTranslation：

1. 由于里程计的更新速度更快，优先使用里程计获得线速度
2. 采用两次scan-match位置进行微分获得

估计的角速度有3种方式，按优先级(参见AdvanceImuTracker开始部分)：

1. 采优先使用IMU数据获取角速度
2. 采用里程计进行微分
3. 采用两次scan-match的朝向进行微分获得，频率最低

以上估计线速度和角速度所用的两次scan mathch的位姿的处理都在UpdateVelocitiesFromPoses。如果出现了报警： Queue too short for velocity estimation，可以尝试降低odometry_sampling_ratio 和 imu_sampling_ratio，先从1降到0.5

最后针对线速度和角速度进行实时积分，输出当前时间的融合的估计位姿

AddPose

存入的是前端一帧一帧激光匹配的位姿，就是两个node之间的位姿。由于pose是由激光匹配产生的，而激光的频率是比IMU和odom要低的，所以这里pose的传入频率比IMU、odom低，比如传入5帧IMU，才插入一个pose。

位姿估计器十分相信scan match得到的位姿，如果遇到长走廊等环境相似的区域，这个位姿就会有问题

只在Node::PublishTrajectoryStates和LocalTrajectoryBuilder2D::AddAccumulatedRangeData调用。重点是后者，这个pose是Scan Match反馈给估计器以修正估计值, 也就是添加到timed_pose_queue_队列

函数大致流程:

• 确保ImuTracker实例化：实例化时需要用到重力加速度常数，初始时间。重力常数主要用来做距离的修正。

• 将该位姿pose加入到timed_pose_queue_中，删除其中比加入时间早 pose_queue_duration_的位姿。

• 根据timed_pose_queue_中的位姿估计速度：找出队列中最久和最新的位姿和时间， 通过除以时间得到线速度(linear_velocity_from_poses_)和角速度(angular_velocity_from_poses_)。 计算角速度的时候是使用四元数old_q.inverse() * new_q 得到old_q 到 new_q 的角位移，然后除以时间得到的。队列中的位姿时间要求如下：
  
  一般选择1ms以内的姿态来估计，而且旧的数据要删除掉。

• 进一步处理ImuTracker：如果到该时间之前没有imu数据，那么使用从poses中推断的角速度和一个假的重力来保障2d的稳定。如果有IMU，使用IMU数据进行姿态估计（imu_tracker_），并将此时估计出的姿态用于计算 里程计线速度 和最新估计姿态 的初值（odometry_imu_tracker_ extrapolation_imu_tracker_）

GetLastPoseTime

1
2
3
4
5
6

common::Time PoseExtrapolator::GetLastPoseTime() const {
  if (timed_pose_queue_.empty()) {
    return common::Time::min();
  }
  return timed_pose_queue_.back().time;
}

就是返回队列最后一个pose对应的时间

在位姿融合中,因为IMU和里程计的采集速率比scan match快很多，因而在一定时间段内，可能没有scan match估计的位姿数据，这时需要通过 IMU 和里程计数据进行推断，但因为这两个传感器存在相对激光扫描较大的累积误差，所以需要对两者进行一些更新和初始化消除累积误差

最后以里程计数据为例，补上整个的处理流程

为什么每次新建地图的角度不同：每次开始的方向不同，从第一帧雷达数据到来时，前端子图的坐标原点来自ExtrapolatePose，此时机器人发生旋转，ExtrapolatePose输出的位姿朝向会不同；另外还有IMU的数据问题。