Local SLAM的工作是建立一系列的子图。每一个子图是局部一致的，但是可以接受它随着时间会发生漂移。

本文件中，距离的单位： 米， 角度的单位： 度，时间的单位： 秒

基本

1
2
3
4
5
6
7
8
9
10
11

use_imu_data = true,
min_range = 0.,
max_range = 30.,
# 设置雷达数据在z轴上的滤波，只用于多线雷达，单线雷达无需使用
min_z = -0.8,
max_z = 2.,

missing_data_ray_length = 5.,
num_accumulated_range_data = 1,
# 使用voxel滤波时立方体边长的大小，这个值是固定的
voxel_filter_size = 0.025,

• use_imu_data： 进行2D SLAM时，Cartographer的默认配置是使用imu的，如果不想要imu就要TRAJECTORY_BUILDER_2D.use_imu_data = false。在源码LocalTrajectoryBuilder2D::AddRangeData中可以看到，如果设置为false，会直接初始化位姿估计器；如果设置为true，会等收到imu数据后初始化位姿估计器，如果没有启动imu程序，Cartographer会一直等待imu的数据而无法进行。

如果不用imu的话，构图的时候一定要移动的很慢。3D SLAM必须使用imu，因为它提供了重力的精确方向和激光方向的一个初始猜测值，巨大地减少了scan matching的复杂度。 代码在Node::ComputeExpectedSensorIds

1
2
3
4
5
6
7
8

// 2D的IMU可选，3D的IMU必须有
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) 
{
  expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
}

• 从min_range到max_z： 相当于带通滤波，减少传感器的噪声，z用于3D SLAM，在源码的sensor::CropRangeData函数。min_range和max_range用于局部地图下的激光点测量范围。

如果对着空旷的环境建图，会运行到AddAccumulatedRangeData的Dropped empty horizontal range data然后返回，不会有地图

• missing_data_ray_length: 官方的解释是 Points beyond ‘max_range’ will be inserted with this length as empty space.这是用于处理miss集合的，但是我看了源码之后，有所疑惑。源码部分：

1
2

const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();

用range跟min_range 和 max_range比较，在范围内的插入 accumulated_range_data_.returns，超出 max_range的做如下处理
hit_in_local.position = origin_in_local + options_.missing_data_ray_length() / range * delta

也就是相当于把hit_in_local = origin + delta变成了hit_in_local = origin + missing_data_ray_length/delta.norm() * delta，delta多了个因子。没看明白为什么这样计算，暂时让missing_data_ray_length小于max_range，这个参数显然跟雷达相关。

• num_accumulated_range_data： 几帧有效的点云才进行一次扫描匹配。 从现象上看这和减少运动带来的雷达数据的畸变有关。Cartographer把num_accumulated_range_data个帧集合 (accumulate) 成一个大帧作为算法的输入。
  Cartographer认为这每一帧都是独立的，以帧为单位补偿运动导致的激光雷达数据产生的畸变，然后再把这些帧集合到一块去。所以Cartographer接收到的数据帧的频率越高，Cartographer 的补偿效果越好，算法的输入数据质量越高。
  当我设置为10后，雷达的scan更新很慢，跟不上机器人的运动，所以对2D雷达一般都设置为1。如果同时用两个雷达，每个的一帧激光用1个ROS消息，那么设置为2。 在源码中的LocalTrajectoryBuilder2D::AddRangeData
  

• voxel_filter_size: 离雷达较近的表面 (如路面) 经常得到更多采样点, 而远处的物体的采样点比较稀少。为了降低计算量，需要对点云数据进行体素滤波，简单的随机采样仍然会导致低密度区的点更少，而高密度区的点仍然比较多。voxel_filter_size即为立方体的大小。如果立方体较小的话会导致密集的数据，所耗的计算量更大。较大的话可能会导致数据丢失，但是计算速度会更快。一般就用0.025，最大不要超过0.1

自适应的voxel滤波

使用固定的voxel滤波之后，再使用自适应的voxel滤波，voxel_filter用于生成稀疏点云，以进行扫描匹配

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

adaptive_voxel_filter = { 
	max_length = 0.5,       # 尝试确定最佳的立方体边长，边长最大为0.5
	# 如果存在较多点，并且大于min_num_points，则减小体素长度以尝试获得该最小点数
  # 如果point_cloud.size()小于该参数则返回，说明点云已经够稀疏了
	min_num_points = 200,
  # 这个是相对local坐标系原点的
	max_range = 50.,        # 距远离原点超过max_range 的点被移除 
},
# 用于生成稀疏点云 以进行 闭环检测
loop_closure_adaptive_voxel_filter = {
	max_length = 0.9,       
	min_num_points = 100,
	max_range = 50.,
},

在3D中，使用两个自适应的体素滤波器来生成一个更高分辨率和一个低分辨率的点云，其使用方法在Local SLAM.

adaptive_voxel_filter的参数不是没用，我调整max_length=3之后，地图中出现了这样的情况：

改成2就好了，默认的1也行

pose_extrapolator

• imu_based = false     3d用来初始化位姿推断器的方式。相关参数全省略

1
2
3
4

costant_velocity = {
  imu_gravity_time_constant = 10
  pose_queue_duration = 0.001
}

• imu_gravity_time_constant: 移动时通过imu观测10s，以确定重力的平均方向。参数为align重力的时间间隔，也是IMU一阶低通滤波中的滤波器常数，调用在ImuTracker::AddImuLinearAccelerationObservation，平时不必修改，如果要认真修改，最好使用Matlab理解一阶滤波算法

• pose_queue_duration

两个scan matcher

real_time_correlative_scan_matcher

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

use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = 
{
  #  一般情况下，位姿估计器传来的初值与scan matcher找到的初值不会差很多
  #  所以搜索窗口不必很大，0.5已经比较大了
  # 找到最佳扫描匹配的  最小线性搜索窗口，0.1就是2个栅格
  linear_search_window = 0.1,
  # 找到最佳扫描匹配的  最小角度搜索窗口，默认20°是比较大的
  angular_search_window = math.rad(20.),

  # 用于计算各部分score的权重，如果一个平台的瞬间平移很少，
  # 则可以降低平移的权重，旋转也是一样
  # 源码在 ScoreCandidates
  translation_delta_cost_weight = 1e-1,
  rotation_delta_cost_weight = 1e-1,
},

use_online_correlative_scan_matching默认 true。如果这项为false，则直接用 ceres_scan_matcher

使用类似实时的闭环检测的方法进行前端的扫描匹配，将当前scan在一定的搜索范围内搜索，范围为设定的平移距离及角度大小，然后在将scan插入到匹配的最优位置处。设置为true后，建图的效果非常好，即使建图有漂移也能够修正回去，但是计算复杂度非常高，很耗CPU。 在低配置平台上，一般不用

ceres_scan_matcher

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

# 将前一时刻的位姿作为先验，使用odom或者imu的数据提供初值，以确定scan的最优位姿
ceres_scan_matcher = 
{
  occupied_space_weight = 1.,   # 每个Cost因素的 尺度因子
  translation_weight = 10., 
  rotation_weight = 40.,    
  ceres_solver_options = {
    use_nonmonotonic_steps = false,
    max_num_iterations = 20,
    num_threads = 1,
  },
},

CeresScanMatcher可以针对它的输入设置权重，体现了对数据的信任程度。某种数据的权重越大，Cartographer在做scan matching时就更重视这种数据。数据有：occupied space(来自scan中的点), 来自pose extrapolator(或者RealTimeCorrelativeScanMatcher)的平移和旋转

计算位姿估计器提供的预测值和ceres scan matcher返回的位姿之间的差

1

double variety_distance = (pose_estimate_2d->translation() - pose_prediction.translation()).norm();

试验发现前端不经过两个scan matcher，所建的图略有偏差，但尚可接受。推测如果IMU和里程计精度足够，对建图精确性要求不高，前端不用两个scan matcher也行，但是占用CPU下降不多，可见优化的重点还在后端

• max_num_iterations: 在ceres中默认是50，这里改为20是因为SLAM的前端有实时性要求，所以不能太大。可以根据自己的情况继续降低, ALOAM中甚至设置为 4。
  另外源码中使用的求解方式是DENSE_QR，因为如果ceres优化问题不是SLAM的大型后端，不是稀疏问题，可使用DENSE_QR

motion_filter

1
2
3
4
5
6

# 只有当scan的平移、旋转或者时间 超过阈值时，才会被加入到 submap 中，不超过则舍弃
  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

为了避免每个子图插入太多的scan，一旦scan matcher发现两个scan之间有motion，它就会进入motion filter. 只要scan的motion达到一个参数要求，就会插入到当前子图，否则被抛弃。 建图时会发现，如果机器人停着不动，过一段时间图会逐渐形成轮廓，边界变黑变粗，这就是max_time_seconds决定的，可以稍微降低。

代码在LocalTrajectoryBuilder2D::InsertIntoSubmap的开头，只调用一个函数MotionFilter::IsSimilar，常见的日志是Motion filter reduced the number of nodes to 3.8%，可见过滤效果很强

submaps

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

# 平时可能修改的只有 num_range_data
submaps = 
{
  # 每个submap的scan总数为num_range_data的2倍，前一半进行初始化而不匹配，后一半进行匹配
  num_range_data = 90,
  grid_options_2d = {
    grid_type = "PROBABILITY_GRID",
    resolution = 0.05,   # 栅格的尺寸
  },
  range_data_inserter = 
  {
    range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
    probability_grid_range_data_inserter = {
      insert_free_space = true,
      hit_probability = 0.55,   # 占用,  >= 0.5
      miss_probability = 0.49,  # 空闲,  <= 0.5
    },
  },

},

• num_range_data：子图由多少scan构成。这个参数对建图效果有很大影响，可以试试50~90。当Local SLAM收到足够的雷达数据后，子图才算完整

子图必须足够小，使其内部的漂移低于分辨率，以便它们是局部正确的。另一方面，它们应该足够大以使loop closure能够正常工作。 对应的源码在ActiveSubmaps2D::InsertRangeData

这个值与CPU有这样一种关系：值比较小(10)，CPU使用率比较稳定，但整体偏高；值大时，CPU平时使用率低，但会短暂爆增。如果再增大(140)，CPU长时间占用低，偶尔才出现一两次高峰。所以可以认为 num_range_data越小，CPU占用的高峰越多，可能就是插入子图完成的时候增大CPU占用，估计是函数 ApplyLookUpTable 导致

• grid_type：submap存储雷达数据的格式，最常用的是概率栅格。在2D中，另一种是Truncated Signed Distance Fields (TSDF). 源码在ActiveSubmaps2D::CreateGrid

• resolution: 栅格尺寸，常常是0.05。并不是越小越好，我在2000米的环境下设置为0.02，结果建图严重错位，全局优化也没有纠正过来，应该是算力不够了。设置为0.05，对后端参数不必特别设置，建图效果也很好。

• range_data_inserter： 概率栅格把空间剪切成一个2D或者3D的表格，该表格的每一个cell有一个固定的大小，并包含了被构造(obstructed)的可能性。可能性是根据hits(测量范围数据)和misses(传感器和测量点之间的自由空间)来进行更新，hits和misses二者在占据概率计算中可能有不同的权重，该占据概率计算或多或少相信占据或者自由空间的测量。

• insert_free_space对应的源码在CastRays函数，是否处理miss事件，如果为 false，则free的栅格不会改变占用网格中的概率。 后两个参数在ProbabilityGridRangeDataInserter2D的构造函数中使用。

概率栅格地图的Odds are updated according to “hits” (where the range data is measured) and “misses” (the free space between the sensor and the measured points)。可以根据对被占据 occupied 和 free space 的雷达数据的置信度，加减 hits 和 misses 的 weight 值（Both hits and misses can have a different weight in occupancy probability calculations giving more or less trust to occupied or free space measurements）。

在2D中，每个子图只有一个概率栅格地图被存储。在3D中，由于scan matching性能的原因，两个混合的概率栅格被使用，并分别应用了一个adaptive_voxel_filter。对于远距离测量，使用一个低分辨率的混合栅格；对于近距离测量，使用一个高分辨率的混合栅格；scan matching先使用低分辨率的点云来与低分辨率的栅格进行对齐，然后通过高分辨率的近处点云与高分辨率的 hybrid grid 对齐来 refine pose。

RViz插件可以观察子图，可以根据索引选择子图

SSH是不带界面的协议，之前在机器人远程机配置vnc4server，从本地连接到远程机，希望能在远程机直接运行rviz以进行观测，不知道失败了多少次，今天又试了试，这次得到如下结果：

这是因为rviz是基于Qt和OpenGL的，但是vnc4server对OpenGL的支持有问题，所以报错

目前最好的就是x11vnc。这个程序不仅不收费，是开源的，而且还支持opengl程序，rviz之类的程序也可以正常打开了。

1. 安装x11vnc

1

sudo apt-get install -y x11vnc net-tools

1. 设置访问密码

1

sudo x11vnc -storepasswd

按提示设置密码，密码一般放在/home/user/.vnc/passwd

1. 创建服务文件

sudo vim /lib/systemd/system/x11vnc.service，文件内容如下:

1
2
3
4
5
6
7
8
9
10

[Unit] 
Description=Start x11vnc at startup. 
After=multi-user.target 

[Service] 
Type=simple 
ExecStart=/usr/bin/x11vnc -auth guess -forever -loop -noxdamage -repeat -rfbauth /home/USERNAME/.vnc/passwd -rfbport 5900 -shared 

[Install] 
WantedBy=multi-user.target

1. 启动服务:

   1 2 3

   sudo systemctl daemon-reload sudo systemctl enable x11vnc.service sudo systemctl start x11vnc.service

一般来说，经过上面步骤就成功了。但是可能出现下面错误

其实上面一大堆配置都是为了开机启动，在4之前可以先手动运行，直接x11vnc即可，看输出的文本是否正常，端口有可能是5901，可以用netstat命令检查

1. 客户端

下载vnc viewer，输入目标IP，端口5900，之后就可以正常连接了，比如192.168.0.103:5900

如果在不插显示器使用rviz时还是报错， 插上hdmi转vga的转接头（不接显示器，只是转接头） 就可以打开正常使用了。如果此时再外接一个显示器，实际就成了双屏配置，在VNC里会出现长屏幕，对于大显示器，用着更舒服了。

但是如果只插一个HDMI线，可以显示，但拖拽终端会出现重影

可以在设置里面调整分辨率和比例，如果感觉VNC速度慢，可以禁用Compiz

有的网络下，会出现经常掉线重连的情况，此时只需把画面质量改为Medium即可，当然Low更可以。

参考：
ubuntu18.04安装x11vnc

配置vino

1. 安装

1
2
3
4
5
6
7
8
9

sudo apt update

sudo apt install vino

sudo ln -s ../vino-server.service  /usr/lib/systemd/user/graphical-session.target.wants

# 配置VNC server:
gsettings set org.gnome.Vino prompt-enabled false
gsettings set org.gnome.Vino require-encryption false

使用命令 /usr/lib/vino/vino-server开启

1. 每连接到一个新的wifi ,都需要在设置的页面把共享的wifi打开

2. 将网卡加入VINO服务命令

1
2
3
4
5
6
7
8

# 用于显示 NetworkManager（网络管理器）中当前配置的网络连接列表及其详细信息.
nmcli connection show

# 将指定的 UUID（Universally Unique Identifier）添加到 Vino 服务器启用的连接列表中，指定允许远程访问和控制你的计算机的特定连接.
dconf write /org/gnome/settings-daemon/plugins/sharing/vino-server/enabled-connections "['your UUID']"

# export DISPLAY作用是指定 X Window 系统的显示器，通过设置 DISPLAY 环境变量，它们可以知道要将图形显示在哪个显示器上,默认是0.
export DISPLAY=:0

1. 设置开机自启动

1
2
3
4
5
6
7
8
9
10
11

gsettings set org.gnome.Vino enabled true
mkdir -p ~/.config/autostart
vim ~/.config/autostart/vino-server.desktop


# 将下面的内容添加到该文件中，保存并退出。
[Desktop Entry]
Type=Application
Name=Vino VNC server
Exec=/usr/lib/vino/vino-server
NoDisplay=true

1. 安装虚拟显示器

经过以上设置，连接VNC后可能是splash screen或者说花屏，还是不正常。用虚拟显示器解决

1

sudo apt-get install  -y  xserver-xorg-core-hwe-18.04  xserver-xorg-video-dummy-hwe-18.04

sudo vim /usr/share/X11/xorg.conf.d/xorg.conf加入下面内容

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

Section "Device"
  Identifier  "Configured Video Device"
  Driver      "dummy"
EndSection
 
Section "Monitor"
  Identifier  "Configured Monitor"
  HorizSync 31.5-48.5
  VertRefresh 50-70
EndSection
 
Section "Screen"
  Identifier  "Default Screen"
  Monitor     "Configured Monitor"
  Device      "Configured Video Device"
  DefaultDepth 24
  SubSection "Display"
    Depth 24
    Modes "1920x1080"
  EndSubSection
EndSection

参考: vino的配置

launch 和 lua 配置文件都是在install_isolated/share/cartographer_ros/launch/ 和 install_isolated/share/cartographer_ros/configuration_files/ 下，所以如果只修改了cartographer_ros软件包中的launch 和 lua 文件之后是需要重新编译的，编译之后才能将修改的文件安装到install_isolated文件中。

从cartographer启动时输出的信息可知，我们自己的lua加载的其他lua有：map_builder.lua, pose_graph.lua, trajectory_builder.lua, trajectory_builder_2d.lua. 它们的路径都在/usr/local/share/cartographer/configuration_files。如果我们需要改这些基本lua的参数，可以在自己lua里覆盖地修改，直接改原文件不太好，它们的关系如下：

map_builder.lua和trajectory_build.lua起着总领的作用，前者包括的pose_graph.lua为后端优化的具体参数配置； 后者是前端的参数配置，分为2d和3d两个文件。

lua_parameter_dictionary.cc是用于lua参数的，如果这里有报错，基本是lua参数的语法问题

1

[/cartographer_node] [ScopedRosLogSink::send] line_63  F0402 21:57:53.000000 20519 lua_parameter_dictionary.cc:399] Check failed: HasKey(key) Key 'use_nav_sat' not in dictionary:

在lua文件里必须设置use_nav_sat参数，同样报错的参数都要添加

自定义lua的参数

我用的是home.lua， 先看options的内容，这是通用的参数。options块中定义的值定义了cartographer前端应当如何与机器人进行交互。 定义在options段后的值用于调试cartographer的内部信息

开头的map_builder = MAP_BUILDER和trajectory_builder = TRAJECTORY_BUILDER万年不变:

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

map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
publish_tracked_pose = false

use_odometry = true,
use_nav_sat = false,
use_landmarks = false,

num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,

lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,  --频率可能比较高
trajectory_publish_period_sec = 30e-3,

rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,

# 补充
collate_fixed_frame = true  是否将数据放入阻塞队列
collate_landmarks = false  -- 一般都是false

坐标系组

• map_frame: 用于发布submaps的ROS坐标系，也是位姿的父坐标系，通常是map

• tracking_frame: 通常，如果使用IMU，就是imu_link; 如果不用IMU，可以用laser。博物馆的lua写的base_link

有时遇到这样的报错：

1

Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise

• published_frame: 位姿子坐标系的ROS坐标系，例如”odom”坐标系，如果一个odom坐标系由系统的不同部分提供，在这种情况下，map_frame中的odom姿势将被发布。 否则，将其设置为base_link可能是合适的

• provide_odom_frame: true 适用于机器人本身没有odom坐标系的情况，如果启用，将发布局部、非闭环、持续的位姿，也就是odom_frame在map_frame中的坐标

• odom_frame: 在provide_odom_frame为true才启用，坐标系在published_frame和map_frame之间用于发布局部SLAM结果，通常是”odom”
• use_odometry: 如果启用，会订阅关于odom话题(或者叫其他名字)的nav_msgs/Odometry消息。应当提供里程信息，这些信息包含在SLAM里
• use_nav_sat: 是否使用gps数据。如果启用，将订阅sensor_msgs/NavSatFix的话题，比如fix. 应当提供导航数据，将用于全局SLAM

• use_landmarks: 如果启用，订阅cartographer_ros_msgs/LandmarkList的话题，比如landmarks. 应当提供Landmarks信息，这些信息包含在SLAM里

• use_pose_extrapolator: 默认false. 发布tf时是使用 pose_extrapolator推断的位姿还是前端计算出来的位姿

1
2
3
4

stamped_transform.header.stamp =
    node_options_.use_pose_extrapolator
        ? ToRos(now)
        : ToRos(trajectory_data.local_slam_data->time);

旧版本没有这个参数，直接使用了now。这个参数认为位姿估计器很精确，但前提是雷达频率高，时间间隔短。最好是使用前端的时间戳

• publish_frame_projected_to_2d: 默认false. 发布TF变换时，是否将tracking在Local坐标系中的坐标投影到2D平面上，这只是个显示功能，不影响SLAM。
• publish_tracked_pose: 是否发布话题tracked_pose，也就是tracking坐标系在map坐标系的位姿，朝向与imu_link的x轴重合

num组

Cartographer可以订阅的主题有三种，我们一般用scan，还可以用echoes或points2，这三个是互斥的。分别对应lua中的num_laser_scans, num_multi_echo_laser_scans,num_point_clouds. 这三个参数必须都是 >= 0 且三者之和 >=1 代码在ComputeRepeatedTopicNames

lua中将num_laser_scans设置为1，则scan将用作SLAM的输入，如果num_laser_scans大于1，则多个编号的扫描主题（即scan_1，scan_2，scan_3，……直到并包括num_laser_scans）将用作SLAM的输入.

当num_point_clouds为1时，默认订阅points2话题的数据

同理，如果将num_multi_echo_laser_scans设置为１，则echoes做输入，但仅使用第一个回声，如果大于1，则多个编号的回声主题（即echoes_1，echoes_2，echoes_3，……直到并包括num_multi_echo_laser_scans）将用作SLAM的输入。echoes的消息类型为sensor_msgs/MultiEchoLaserScan，不同之处在于sensor_msgs/LaserEcho[] ranges和sensor_msgs/LaserEcho[] intensities

points２也是这样，消息类型sensor_msgs/PointCloud2，num_point_clouds适用于多线雷达，对于单线，设置为0

• num_subdivisions_per_laser_scan: 将雷达一帧的数据拆分成几次发出来，必须>=1，默认10。在SensorBridge::HandleLaserScan中调用, 对点云points细分。比如雷达scan有200束激光，本参数为10，那么转换成点云后，对点云取很多小段处理：(0,20)、(20,40)……(190,200)。 对于普通雷达来说，此处为1即可

period 组

都在Node的构造函数里，是ROS定时器的发布间隔

• lookup_transform_timeout_sec: 使用tf2查找tracking_frame和雷达坐标系变换的超时秒数，在函数TfBridge::LookupToTracking
• submap_publish_period_sec: 发布话题submap_list的间隔
• posepublish_period_sec: 对应`publish_local_trajectory_data_timer和Node::PublishLocalTrajectoryData`. 例如5e-3，对应tf发布频率为200Hz
• trajectory_publish_period_sec: 发布轨迹节点和Landmark的时间间隔，对应PublishTrajectoryNodeList和PublishLandmarkPosesList

ratio 组

5种观测的权重比

• rangefinder_sampling_ratio: Fixed ratio sampling for range finders messages.

• odometry_sampling_ratio: Fixed ratio sampling for odometry messages. 如odom的数据非常不准，可以设置为0.3，以减小odom对整体优化的影响

• fixed_frame_sampling_ratio: Fixed ratio sampling for fixed frame messages.

• imu_sampling_ratio: Fixed ratio sampling for IMU messages.

• landmarks_sampling_ratio: Fixed ratio sampling for landmarks messages.

options结束后，一般有一句：MAP_BUILDER.use_trajectory_builder_2d = true，这是因为map_builder.lua中的这个参数设置为false了，我们在这里可以重新赋值，选择使用2D还是3D

不使用机器人的IMU和Odom

1
2
3
4
5
6
7
8

map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "base_footprint",	
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = false,
TRAJECTORY_BUILDER_2D.use_imu_data = false  --默认true

进行2d slam时，carto的默认配置是使用imu的，所以如果没有imu就要TRAJECTORY_BUILDER_2D.use_imu_data = false，否则会一直等待imu的数据而进行不下去。而3D slam 必须使用imu，所以不必修改这个参数。

• 为什么3D必须有IMU，而2D可以不用？

在2D中，Cartographer支持运行相关的扫描匹配器，用于局部SLAM中寻找循环关闭的约束条件。它的计算代价很大，但通常会使odometry或IMU数据的加入变得不必要。2D也有假设平坦世界的好处，也就是说，上升是隐式定义的。
在三维中，IMU主要用于测量重力。重力在测量中是一个很有用的量，因为它不漂移，而且是一个非常强的信号，而且通常包含了大部分所要测量的加速度。

Node::LaunchSubscribers函数中有一段：

1
2
3
4

if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data() )  )

• 屏蔽机器人的里程计

如果我们不想使用轮式里程计， cartographer的lua无法屏蔽里程计，只能自己修改机器人的程序，不让它发布odom话题和odom--->base_footprint的tf变换。如果不修改，使用时会发现tf变换一会儿由 cartographer发布，一会儿由机器人程序发布，千万不能由两个节点发布同一个tf变换，从rviz看上去，RobotModel和LaserScan摇晃地很厉害。

这里有一个技巧， 观察一个tf变换是否由两个节点发布，用rosrun tf view_frame是看不出来的，应该用rosrun rqt_tf_tree rqt_tf_tree，然后用刷新按钮观察。

使用机器人的odom和imu，lua做如下设置:

1
2
3
4
5
6
7

map_frame = "map",
tracking_frame = "imu",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = true,

一开始，我增加了参数TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10，结果出现了scan一直在转的情况，使用tf_echo map和laser坐标系的关系，发现y位移一直在增大，但x z基本不变。去掉这个参数后，不再一直转了，但是过了几分钟还是有所偏，地图还出现了重影，这说明是机器人的里程计误差问题，需要校准，这个参数应该是放大了这种现象。

如果不想每过一段时间就校准，那么odom到base_link的tf就用cartographer的，因为它有重定位，累计误差比较小，而且可以得到修正

对是否使用odom的两套程序如下：

查看节点cartographer_node是否订阅了IMU和Odom，检查是否用到了它们

问题

• 使用IMU后，有时启动时会报警：
  
  
  出问题的代码在cartographer::mapping::ImuTracker::AddImuLinearAccelerationObservation()。启动一下imu程序，如果rostopic echo /xqserial_server/IMU/linear_acceleration就会发现，imu线加速度开始为0，过了一秒左右才有数据，这应该是节点通信的问题。所以cartographer读取到的线加速度全是0，在程序里就会报错。此时再重启就没事了

• 建图时地图围绕原点转圈，画出来的是一团黑，可能是用到的IMU的加速度有问题，需要校准IMU。还有可能是ceres的平移和旋转的权重没有进行重新配置。

• 如果出现下面的输出，一般是scan话题没有订阅到:

  1	[ WARN][/cartographer_node] [ScopedRosLogSink::send] line_55 W0711 20:05:58.000000 12245 ordered_multi_queue.cc:155] Queue waiting for data: (0, scan)

  正确的是这样：

  1	[ INFO][/cartographer_node] [ScopedRosLogSink::send] line_51 I0711 20:14:56.000000 25541 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '637300664959145564'.

• 常常启动后出现这个报警：
  
  这其实是tf坐标系的问题，cartographer源码里lookupTransform写的不好，不管也可以，详细分析可见

tf报警：Lookup would require extrapolation into the past

• 机器人在地图中的位置不稳定，有轻微摆动，tf_echo如下：

yaw有跳变， 判断是里程计累计误差

• cartographer对雷达频率要求比较高，20Hz以上比较好，当频率达到30hz时，做纯旋转运动时不会产生地图的偏移。因为只有频率越高，2帧间的时间越短，误差才能越小。

• 如果雷达比较差，地图边界会比较粗糙，用robosense雷达明显效果好

map_builder.lua 和 trajectory_builder.lua

内容只有几行:

1
2
3
4
5
6
7
8

include "pose_graph.lua"

MAP_BUILDER = {
  use_trajectory_builder_2d = false,
  use_trajectory_builder_3d = false,
  num_background_threads = 4,
  pose_graph = POSE_GRAPH,
}

1
2
3
4
5
6
7
8

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
  pure_localization = false,
}

trajectory_builder.lua更简单，只有一个pure_localization区分是否使用carto用于机器人纯定位

纯定位时载入的地图在submap中的trajectory为0，启动定位后，系统默认从起点开始建立子图进行匹配，该子图在submap中的trajectory为 1。

一条trajectory可以理解为一次建图过程。如果只让机器人跑一圈，那么trajectory数就是1. 但是，建好图后又需要在图中走，这时候可以增加一条trajectory，把pure_localization设为true，那么机器人再重新跑的过程中就会跟已经建好的图进行匹配，估计机器人在地图中的路径。所以，一次运行就代表了一条trajectory。

cartographer定位时，它的地图是变化的

纯定位和建图不同，local和global SLAM之间的延迟要尽量低. 其次global SLAM通常会在作为地图的frozen trajectory和当前trajectory之间，找到大量的相互约束(inter constraints)。

纯定位模式默认只保存 3 个子图，只有这些子图内的节点参与优化。所以地图大小并不会影响后端优化的速度。而建图结束的时候全图优化应该是所有节点参与。

我们需要显著减小POSE_GRAPH.optimize_every_n_nodes以接收频繁的结果， global SLAM通常会太慢，所以再显著减小global_sampling_ratio和constraint_builder.sampling_ratio以补偿大量的约束。

submaps.resolution应当和运行的.pbstream中的子图resolution一致

建图时，cartographer使用 fast correlative scan matcher找到回环，search window默认为7m×7m×30°. 但纯定位模式中，search window变得非常大，所以纯定位消耗CPU更大

constraint_builder_2d.cc中的ComputeConstraint()函数，在纯定位模式下，会频繁地调用MaybeAddGlobalConstraint()，这个函数会搜索整个子图，造成的开销特别大。建议将global_sampling_ratio调小，这样就可以从一定程度上减少开销。

数据集的纯定位测试

程序运行时只在局部建图（保留固定数量的子图），随着机器人的运动和观测， 新的子图会添加进来，旧的子图会被删除，就是只保留局部的地图数据。我的理解是保留局部数据越多，与原有地图进行回环检测就增多，这样计算量增加的同时也增加了误匹配的概率，使得整个系统不稳定 ，所以这里只进行定位，不用于建图

需要用到数据集b2-2016-04-05-14-44-52.bag和b2-2016-04-27-12-31-41.bag，前者用于建图，后者用于定位

先运行建图：

1

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/path/b2-2016-04-05-14-44-52.bag

这里是先用第一个数据集建图，建完图后用第二个数据集进行定位测试。

1

roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=${BAG}/b2-2016-04-05-14-44-52.bag.pbstream bag_filename:=${BAG}/b2-2016-04-27-12-31-41.bag

在机器人的旧版本纯定位

用于定位时的home_no_odom_localization.launch

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

<launch>
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename home_no_odom_localization.lua
          -load_state_filename /home/xiaoqiang/Documents/ros/maps/home.pbstream"
      output="screen">
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

建图使用no_odom_and_imu.lua，定位使用home_no_odom_localization.lua:

1
2
3
4
5
6
7

include "home.lua"

TRAJECTORY_BUILDER.pure_localization = true
--- 每10个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU增大
POSE_GRAPH.optimize_every_n_nodes = 5
POSE_GRAPH.constraint_builder.sampling_ratio = 0.1
POSE_GRAPH.global_sampling_ratio = 0.002

optimize_every_n_nodes会影响定位时的CPU占用，设置为5时对应50%，但只有定位时的一秒左右，所以无所谓。

添加轨迹实际调用的源码是node.cc中的Node::HandleStartTrajectory——AddTrajectory

新版本的配置

这是因为我的 cartographer版本旧，不支持pure_localization_trimmer，最好还是更新到最新。在18年7月增加了pure_localization_trimmer，删掉了pure_localization，如果是新一点的版本，做对应修改。commit历史在这里

相应的源码在 MapBuilder::AddTrajectoryBuilder——MaybeAddPureLocalizationTrimmer：

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

if (trajectory_options.pure_localization()) 
{
  LOG(WARNING) << "'TrajectoryBuilderOptions::pure_localization' field is deprecated"
         "Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id, 3 /* max_submaps_to_keep */)  );
  return;
}
if (trajectory_options.has_pure_localization_trimmer()) 
{
  // 添加一个PureLocalizationTrimmer类型修饰器
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id,
      trajectory_options.pure_localization_trimmer().max_submaps_to_keep() ) );
}

新版本的配置:

1
2
3
4

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  --- 最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可
  max_submaps_to_keep = 3,
}

如果使用了机器人的odom，但无法定位成功或定位误差太大，可能的原因是odom不够精确，尝试关闭odom，并由cartographer代为提供odom。既然定位误差大，可能在建图时的原始submap之间误差较大，图优化后map的边缘较为模糊

重定位注释掉Node::GrawAndPublish函数的后三行，否则会继续建图。但是建图的时候还得加回来。
由于Cartographer定位是当前帧和子图进行匹配，所以对于机器人周围被新物体遮挡的情况完全没有影响。

唯一影响就是没法重定位了，因为地图上没有这些新物体。但是就算长时间被遮挡，存在累积定位误差，只要机器人离开了遮挡区域，回到了之前的环境，就可以重定位回来。

获得机器人在map坐标系中的坐标

cartographer是使用tf转换来发布位姿的，如果想发布类似amcl_pose那样的话题，只能修改源码或自己写个程序。

修改源码的方法可以参考这里

我还是自己写了个程序，其实就是tf报警：Lookup would require extrapolation into the past中的程序改了坐标系和话题名称，以获得base_footprint在map中的坐标系

问题

某次运行得到这样结果：

追查到node.cc里的Node::FinishTrajectoryUnderLock函数，检查了一下是否可以关掉，指定id是否存在，是否已经被Finished了等情况后，如果一切正常，则停止订阅Topic、清除id及其他与该trajectory相关的量。最后调用map_builder_bridge_中的FinishTrajectory函数。发现code对应cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT

参考：
官方参考
更优雅的设定初始位姿

由于要做Realsense D435i的双目结构光相机标定，其中用到了ROS来录制数据包，但是结构光会影响标定，所以得先关闭IR结构光发射器。

一次性关闭IR光

其实就是动态参数调整

1
2

roslaunch realsense2_camera rs_camera.launch 
rosrun rqt_reconfigure rqt_reconfigure 

通过肉眼看相机发射器已经不再发射IR光了，打开rqt，找到/camera/infra1/image_rect_raw和/camera/infra2/image_rect_raw，发现确实没有白斑了。

永久关闭

其实就是通过realsense-viewer，这个是驱动层面的操作，更加底层

参考：Realsense D435i 关闭IR光

throttle

转播 (relay)话题, 限制最大发布频率或者带宽。使用格式: rosrun topic_tools throttle messages <intopic> <msgs_per_sec> [outtopic]

messages是当前使用的是throttle的messages模式,限制发布频率. intopic是指你想要改变频率的那个topic, msgs_per_sec是指你想要它发布的频率，而outtopic是指改变发布频率后的topic的名称，可以省略，如果省略则自动在原来topic的名字上后缀throttle

另外还有bytes模式, 用以限制带宽: rosrun topic_tools throttle bytes <intopic> <bytes_per_sec> <window> [outtopic]

例如，让雷达的带宽占用降至1KBps，则命令为：

1

rosrun topic_tools throttle bytes base_scan 1024 1.0

改变topic发布频率并不是说原来的topic就没了，或者直接在原来的topic上更改， 而是把其更改后的topic发布出来，原来的topic仍然存在

有三个参数，需要注意的是~lazy，如果它等于True的话，只有当你订阅到throttle发出来的消息，它才会工作，这显然是ros::publish函数的lazy模式了

最大的问题是 频率控制的精度低,我要求4Hz,实际却在3.6Hz左右，所以实际要设置的大一些

transform

订阅一个topic或topic field，并在应用给定的Python表达式后将传入的数据发布到另一个topic。它可以处理任何消息类型，主要用于简单的消息转换，例如计算向量或四元数的范数， 甚至将四元数转换为欧拉角。

transform <input> <output_topic> <output_type> [<expression>] [--import <module> [<module> ...]]

• input：要订阅的传入topic（或topic field）
• output_topic:要发布的输出topic
• expression:转换输入消息的Python表达式，在变量m中给出。默认表达式是m，它将输入（可以是topic field）转发到output_topic
• import :要在表达式中导入和使用的Python模块的列表。默认导入numpy模块

1
2
3
4

# 计算IMU给出的方向四元数的范数
rosrun topic_tools transform /imu/orientation /norm std_msgs/Float64 'numpy.linalg.norm([m.x, m.y, m.z, m.w])'
# 将方向四元数转换为Euler角度
rosrun topic_tools transform /imu/orientation /euler geometry_msgs/Vector3 'tf.transformations.euler_from_quaternion([m.x, m.y, m.z, m.w])' --import tf

• mux: multiplex between multiple topics

• relay: republish data on one topic to another.

• relay_field: allow to republish data in a different message type

• drop: relay a topic, dropping X out of every Y message.

mux

topic_tools-mux
可以用于多地图实时切换，参考MUX的使用

参考:throttle Wiki

Kalibr可以解决以下的标定问题:

• 多相机标定: 一个相机系统的内外参标定，这几个相机没有全局性重叠的视角
• 视觉惯性标定(camera-IMU): IMU关于相机系统的时空间标定
• Rolling Shutter Camera calibration: full intrinsic calibration (projection, distortion and shutter parameters) of rolling shutter cameras

可以下载Kalibr源码编译生成可执行文件，也可以下载其CDE精简版包。这中间有个坑就是CDE精简包是没有办法标定彩色图片的，而D435输出的是彩色图。所以还是按编译源码的方式

使用Kalibr标定相机的内参和多个相机相对位置关系即外参

安装Kalibr

1. 安装依赖项

   1

   sudo apt-get install python-pyx python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

2. 源码放入工作空间进行编译，会花很长时间，所以编译命令要这样:

   1 2	# 视情况取j8 catkin_make -DCMAKE_BUILD_TYPE=Release -j4

编译kalibr可能会出现 fatal error: numpy/arrayobject.h: No such file or directory ，解决方法： sudo apt-get install --reinstall python-numpy

可能出现catkin_make时，下载suitesparse过久甚至失败的问题。解决方法： 修改～/catkin_ws/src/kalibr/suitesparse中的CMakeLists.txt为新CMakeLists.txt, 然后重新catkin_make

标定板

下载标定板

我下载的是Aprilgrid 6x6 0.5x0.5m(unscaled)， 打印在A3纸上

原版的参数是:6X6 tags，6乘6个格子，一个大格子size=5.5cm，一个小格子spacing=1.65cm

A3纸上的缩放:6X6 tags，一个大格子size=3.5cm，一个小格子spacing=1cm。记得打印出来用尺子量一下，以免出现差错。

下载官网提供的yaml格式文件，需要按照设定的尺寸进行修改

1
2
3
4
5

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.035           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

找一个适合的能拍到棋盘格的距离,启动相机: roslaunch realsense2_camera rs_camera.launch

d453i是有红外发射器的，可以发射很多红外小斑点，如果打开你会在rviz看到很多光斑，可能不利于标定，所以标定时关闭这个发射器的。

降低图像话题的频率，录制图像数据包

kalibr在处理标定数据的时候要求图像的频率不可过高，一般为4hz(后面计算过程报错，改为20)。使用如下指令来限制图像频率:

1
2
3

rosrun topic_tools throttle messages /camera/color/image_raw 4 /color
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4 /infra_left
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4 /infra_right

注意这里是采用了新的话题/color去发布,所以下面录制要写/color话题

将标定目标AprilGrid置于相机前方合理距离范围内，准备录制两分钟:

1

rosbag record -O multicameras_calibration /infra_left /infra_right /color

移动的方式可以参考TUM CALIBRATION SEQUENCES的标定方式，点play即可播放

kalibr算法计算各个摄像头的内参和外参

april_6x6_A3.yaml: 标定物的参数，具体是标定目标的尺寸之类，因为我是缩小打印在A3上，所以要对参数进行修改；pinhole-equi – 选择的相机模型，kalibr提供了很多相机模型，可以自己选择; —bag-from-to 可以选择时间段，毕竟录制的时候不能保证整体都录制的很好。标定会花很长时间，最后会输出一个pdf和txt文件，有内外参数据。

只标定主相机:

1

kalibr_calibrate_cameras --target ../yaml/april_6x6_A4.yaml --bag ./bag/0_multicameras_calibration.bag --model pinhole-equi  --topic  /color  --show-extraction --approx-sync 0.04

最后还是标定的多相机：

1

kalibr_calibrate_cameras --target april_6x6_A3.yaml --bag multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color  --show-extraction --approx-sync 0.04 --bag-from-to 10 100

• pinhole-radtan指的是针孔相机模型和畸变模型，每个相机都要指定。还有Pinhole + FOV等等
• --bag-from-to 10 100指的是录制的第26秒到100秒这段时间
• --show-extraction, 在标定过程中可视化角点检测情况是否良好
• --approx-sync 0.04

结果报错： ImportError: No module named igraph
解决方法： sudo apt-get install python2.7-igraph

需要在有界面的情况下标定，因为会弹出几个窗口，所以不能通过SSH进行，类似这样：

可以使用calibration validator进行标定的验证，原理是对重投影误差进行量化分析，同样需要有界面：
kalibr_camera_validator --cam camchain-multicameras_calibration.yaml --target april_6x6_A3.yaml

realsense d435i包含两个红外相机、红外发射器、RGB相机和IMU四个模块，显然四个传感器的空间位置是不同的，我们在处理图像和IMU数据时需要将这些数据都放在统一的坐标系上去。比如我们用d435i运行vins，处理的图像和IMU数据都需要放在同一个坐标系下，因此需要标定IMU相对RGB相机的空间位置（包括旋转和位移）。

另外，相机固有参数比如焦距、畸变参数等以及IMU的零偏和scale系数等都需要提前知道。前者称为外参，后者称为内参，在运行程序前我们需要标定它们，不论程序是否有自标定功能，毕竟好的初始标定值对于自标定来说也是有利的。

标定顺序：IMU标定 —> 相机标定 —> IMU+相机联合标定. 这么设定顺序是因为最后一步的IMU和相机的联合标定需要 IMU和相机的内参

Allan方差

在IMU采集数据时，会产生两种误差：确定性误差和随机性误差，为获得精确的数据，需要对上述两种误差进行标定,加速度计和陀螺仪随机误差的标定通常使用Allan方差法，它是20世纪60年代由美国国家标准局的David Allan提出的基于时域的分析方法。

A ROS package tool to analyze the IMU performance. C++ version of Allan Variance Tool. The figures are drawn by Matlab, in scripts.

Actually, just analyze the Allan Variance for the IMU data. Collect the data while the IMU is Stationary, with a two hours duration.
code_utils标定IMU的噪音密度和随机游走系数。

安装库和依赖项

安装依赖项，不装之后的编译会报错： sudo apt-get -y install libdw-dev，结果可能提示

1
2
3
4

The following packages have unmet dependencies:
 libdw-dev : Depends: libelf-dev but it is not going to be installed
             Depends: libdw1 (= 0.165-3ubuntu1) but it is not going to be installed
E: Unable to correct problems, you have held broken packages.

编译code_utils和imu_utils

全局安装ceres库，因为code_imu依赖ceres。不要同时把imu_utils和code_utils一起放到src下进行编译。因为imu_utils 依赖 code_utils，原作者的CMakeLists写的不好，所以先编译code_utils再编译后者。

在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为#include"code_utils/backward.hpp"，再依次编译两个包

发布D435i的IMU数据

可以直接在rs_camera.launch基础上针对IMU校准做修改。目的是将acc、gyro数据对齐使用同一个topic发布。

1
2
3
4

<!-- 更改前原版本arg name="unite_imu_method"          default=""/-->  
<arg name="unite_imu_method"          default="linear_interpolation"/>
<!--或着将参数改为copy-->
<arg name="unite_imu_method"          default="copy"/>

标定

根据imu_utils文件夹里面的A3.launch改写D435i标定启动文件：d435i_imu_calib.launch注意，记得修改max_time_min对应的参数，默认是120，也就是两个小时，如果ros包里的imu数据长度没有两个小时，等bag播放完了，还是停留在wait for imu data这里，不会生成标定文件。我录了1小时59分多一点，所以还得改成119

d435i_imu_calibration.launch如下:

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

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!--TOPIC名称和上面一致-->
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <!--imu_name 无所谓-->
        <param name="imu_name" type="string" value= "D435i"/>
        <!--标定结果存放路径-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <!--数据录制时间-min-->
        <param name="max_time_min" type="int" value= "120"/>
        <!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为200，也就是rosbag play播放频率-->
        <param name="max_cluster" type="int" value= "200"/>
    </node>
</launch>

标定结果

标定结果是D435i_imu_param.yaml:

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

%YAML:1.0
---
type: IMU
name: D435i
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 3.6673681012835031e-03
      gyr_w: 7.0017785520472972e-05
   x-axis:
      gyr_n: 3.6001489799186333e-03
      gyr_w: 6.2846247607788020e-05
   y-axis:
      gyr_n: 4.7157261366663813e-03
      gyr_w: 7.5207268006344615e-05
   z-axis:
      gyr_n: 2.6862291872654953e-03
      gyr_w: 7.1999840947286307e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.7436489578044256e-02
      acc_w: 1.0915021608117670e-03
   x-axis:
      acc_n: 1.8271976632141730e-02
      acc_w: 5.5394830052109354e-04
   y-axis:
      acc_n: 2.8924134998445018e-02
      acc_w: 1.5674764920646303e-03
   z-axis:
      acc_n: 3.5113357103546017e-02
      acc_w: 1.1530816898495772e-03

终端输出结果:

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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

gyr x  numData 781205
gyr x  start_t 1.59343e+09
gyr x  end_t 1.59344e+09
gyr x dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
gyr x  freq 109.403
gyr x  period 0.00914049
gyr y  numData 781205
gyr y  start_t 1.59343e+09
gyr y  end_t 1.59344e+09
gyr y dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
gyr y  freq 109.403
gyr y  period 0.00914049
gyr z  numData 781205
gyr z  start_t 1.59343e+09
gyr z  end_t 1.59344e+09
gyr z dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
gyr z  freq 109.403
gyr z  period 0.00914049
Gyro X 
C   -6.83161    94.2973   -19.0588      2.983 -0.0404918
 Bias Instability 2.37767e-05 rad/s
 Bias Instability 6.28462e-05 rad/s, at 63.1334 s
 White Noise 12.9453 rad/s
 White Noise 0.00360015 rad/s
  bias -0.363298 degree/s
-------------------
Gyro y 
C   -8.74367    117.584   -15.9277    2.47408 -0.0373467
 Bias Instability 6.41864e-05 rad/s
 Bias Instability 7.52073e-05 rad/s, at 104.256 s
 White Noise 16.8998 rad/s
 White Noise 0.00471573 rad/s
  bias -0.544767 degree/s
-------------------
Gyro z 
C   -4.51808    68.1919   -9.33284    1.95333 -0.0262641
 Bias Instability 8.50869e-05 rad/s
 Bias Instability 7.19998e-05 rad/s, at 63.1334 s
 White Noise 9.43212 rad/s
 White Noise 0.00268623 rad/s
  bias -0.0762471 degree/s
-------------------
==============================================
==============================================
acc x  numData 781205
acc x  start_t 1.59343e+09
acc x  end_t 1.59344e+09
acc x dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
acc x  freq 109.403
acc x  period 0.00914049
acc y  numData 781205
acc y  start_t 1.59343e+09
acc y  end_t 1.59344e+09
acc y dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
acc y  freq 109.403
acc y  period 0.00914049
acc z  numData 781205
acc z  start_t 1.59343e+09
acc z  end_t 1.59344e+09
acc z dt 
-------------7140.59 s
-------------119.01 min
-------------1.9835 h
acc z  freq 109.403
acc z  period 0.00914049
acc X 
C  3.36177e-05   0.00175435 -0.000159698  7.23303e-05 -7.16006e-07
 Bias Instability 0.000553948 m/s^2
 White Noise 0.018272 m/s^2
-------------------
acc y 
C  9.36955e-05   0.00234733   0.00012197  0.000243676 -2.66252e-06
 Bias Instability 0.00156748 m/s^2
 White Noise 0.0289241 m/s^2
-------------------
acc z 
C  5.07832e-05   0.00331104 -0.000381222  0.000199602 -2.43776e-06
 Bias Instability 0.00115308 m/s^2
 White Noise 0.0351134 m/s^2

标定外参的准备

将Acc和Gyr的第一组平均数据拷贝到kalibr对应的imu.yaml文件中

1
2
3
4
5
6
7

rostopic: /camera/imu
update_rate: 200.0  #Hz
 
accelerometer_noise_density: 2.89e-01 #continous
accelerometer_random_walk: 4.55e-04 
gyroscope_noise_density: 3.02e-03 #continous
gyroscope_random_walk: 2.29e-05

　查看默认imu与相机参数

D435i相关的imu_info话题如下:

realsense_camera/IMUInfo with the header.frame_id set to either imu_accel or imu_gyro to distinguish between accel and gyro info. 消息成员如下:

1
2
3
4

string frame_id
float64[12] data
float64[3] noise_variances
float64[3] bias_variances

rostopic echo -n1 /camera/color/camera_info得到结果:

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

  frame_id: "camera_color_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [611.3538208007812, 0.0, 327.437744140625, 0.0, 610.015869140625, 239.99667358398438, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [611.3538208007812, 0.0, 327.437744140625, 0.0, 0.0, 610.015869140625, 239.99667358398438, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

对于/camera/depth/imu_info，结果是:

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

frame_id: "camera_depth_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [380.23321533203125, 0.0, 316.4999084472656, 0.0, 380.23321533203125, 237.40985107421875, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [380.23321533203125, 0.0, 316.4999084472656, 0.0, 0.0, 380.23321533203125, 237.40985107421875, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

话题/camera/infra2/camera_info和/camera/infra2/camera_info的结果完全一样:

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

frame_id: "camera_infra1_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [380.23321533203125, 0.0, 316.4999084472656, 0.0, 380.23321533203125, 237.40985107421875, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [380.23321533203125, 0.0, 316.4999084472656, 0.0, 0.0, 380.23321533203125, 237.40985107421875, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

绘制Allan曲线

imu_utils/scripts中是Matlab写的.m文件，按照draw_allan.m创建文件draw_allan_acc.m和draw_allan_gyr.m

由于Ubuntu下安装Matlab比较麻烦，因此将数据和.m文件都拷贝到Windows系统下绘制，注意文件路径和m文件中要对应，尤其matlab中的当前路径指的是左侧栏的路径，而不是m文件所在的路径

参考:
RealSense D435i Calibration
官方Github
VIO标定IMU随机误差——Allan方差法

• 标定板pattern尽量选择apriltag，尽量不要用纸质的
• 选择合适的相机模型
• 标定手法看看TUM的

realsense D435i终于到手了，打开发现其实很小巧，先做一些配置看看。默认要使用USB3，如果RealSense使用USB2, Output Resolution自动降到480*270 30fbs，而非产品所宣称的1280 x 720 active stereo depth resolution和90fps，且只有Stereo Moudle在工作，无Image Sensor的RGB Moudle菜单项，无法进行3D建模。

D435i采用了即用型USB供电形式，不仅提供深度传感器模组，还配备了一个IMU单元（惯性测量单元，采用的博世BMI055）。凭借内置的IMU单元，结合视觉数据可实现6DoF追踪功能。其中，IMU将各种线性加速度计和陀螺仪数据结合，可检测X，Y，Z三轴的旋转和平移，以及俯仰、横摇等动作。D435i的2000万像素RGB摄像头和3D传感器可以30帧/秒的速度提供分辨率高达1280 × 720，或者以90帧/秒的速度提供848 × 480的较低分辨率。该摄像头具有全局快门，可以处理快速移动物体，室内室外皆可操作。深度距离在0.1 m~10 m之间，视场角度为85 × 58度

可以获得RGB图、左右红外摄像图、深度图、IMU数据，并且将深度图数据和RGB图进行对齐。左右红外相机进行测量深度，中间红外点阵投射器相当于补光灯，不打开也能测深度，只是效果不好；最右边的rgb相机用于采集彩色图片，最终可以将彩色视频流与深度流进行对齐.

两个驱动讲究搭配，否则安装编译都不报错，运行时会报错 realsense hwmon command 0x7d failed 我使用的是realsense-ros-2.2.23和librealsense-2.43.0

安装librealsense

先装realsense的驱动，步骤参考网上的，其实不必完全相同，我的步骤如下：

1. 下载驱动，然后解压到根目录
2. 执行如下命令：

   1 2 3 4 5

   cd ~/librealsense sudo apt-get install libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev libglfw3-dev # 许可脚本 sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/ sudo udevadm control --reload-rules && udevadm trigger

3. 开始编译

   1 2 3 4 5

   cd ~/librealsense mkdir build && cd build cmake ../ -DBUILD_EXAMPLES=true sudo make uninstall && make clean && make -j7 && sudo make install

运行realsense-viewer验证，看到如下画面

配置可以修改为常见的黑白深度图以及分辨率等等

ROS驱动

从这里下载，然后放到某工作空间，编译即可：

1
2

catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install

tf关系如图：

在rqt中打开深度图时出现了报错，又是图像编码问题


在rqt里查看/camera/depth/image_rect_raw/theora，同样没有深度图，但rqt不报警

在rqt查看RGB和红外的图像，只要选择theora，rqt终端都会报警:

1

[ WARN][/rqt_gui_cpp_node_25764] [TheoraSubscriber::internalCallback] line_170  [theora] Packet was not a Theora header

使用对齐的深度话题信息发布RGBD点云

1

roslaunch realsense2_camera rs_rgbd.launch

内参

realsense启动时可以发现信息 [color stream is enabled - width: 640, height: 480, fps: 15, Format: RGB8]

realsense相机出厂的时候一般都标定好了，直接读取他们的内参即可。终端输入： rs-sensor-control，根据提示选择，出现所有视频流的列表时，根据上面的信息选择，最终显示内参:

1
2
3
4

Principal Point         : 327.438, 239.997
Focal Length            : 611.354, 610.016
Distortion Model        : Inverse Brown Conrady
Distortion Coefficients : [0,0,0,0,0]

疑难问题 undefined symbol: _ZN2cv3MatC1Ev

运行rs_camera.launch时报错

1

symbol lookup error: /home/jetson/catkin_ws/devel/lib//librealsense2_camera.so: undefined symbol: _ZN2cv3MatC1Ev   [camera/realsense2_camera_manager-2] process has died 

运行时可以看到相关的环境版本

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

RealSense ROS v2.3.2
[ INFO] [1723892537.013079361]: Built with LibRealSense v2.50.0
[ INFO] [1723892537.013109698]: Running with LibRealSense v2.50.0
[ INFO] [1723892537.311113159]: Device with serial number 014122072296 was found.
[ INFO] [1723892537.311624883]: Device with physical ID 2-1.4-6 was found.
[ INFO] [1723892537.311852281]: Device with name Intel RealSense D435I was found.
[ INFO] [1723892537.313360062]: Device with port number 2-1.4 was found.
[ INFO] [1723892537.313859786]: Device USB type: 3.2

[ INFO] [1723892537.373681930]: JSON file is not provided
[ INFO] [1723892537.373700779]: ROS Node Namespace: camera
[ INFO] [1723892537.373737260]: Device Name: Intel RealSense D435I
[ INFO] [1723892537.373757100]: Device Serial No: 014122072296
[ INFO] [1723892537.373777005]: Device physical port: 2-1.4-6
[ INFO] [1723892537.373792685]: Device FW version: 05.13.00.50

最后发现其实存在两个librealsense2_camera.so

1
2

/home/jetson/catkin_ws/devel/lib/librealsense2_camera.so
/opt/ros/noetic/lib/librealsense2_camera.so

参考：
realsense D435 驱动安装并嵌入ROS中使用