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中使用

gmapping源码中不完善的部分：

1. drawFromMotion函数是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《概率机器人》中提到的速度模型和里程计模型，有很多方面都没有考虑，精度上可能有折扣。
2. m_angularOdometryReliability和m_linearOdometryReliability用于控制激光评分的增益odo_gain, 实际都是0，所以没有用到，而且无法用参数赋值, 要使用就得修改源码.
3. 紧接上面的代码，localScore=odo_gain*score(map, localPose, readings);，localScore在之后的likelihoodAndScore函数里实际赋值为0，根本没有用。所以这里有好几行可以删除
4. 在论文中，粒子的权重不是用最优位姿的似然值来表示的，而是用所有的似然值的和来表示的
5. 重采样的粒子索引选取用的是轮盘赌算法，有些论文提出了一些更好的算法