参考：这篇博客，意外的是我配置rplidar时发现标志号码和博客中是一样的，还以为像产品序列号那样。我设置了底盘的USB和思岚雷达的USB

realsense的安装包里面包含了udev的文件，即realsense udev的deb。所以使用过realsense的系统，自己没编辑，也会有一个99-realsense-libusb.rules，不必自己设置

70-snap.core.rules是系统自带的

• rosed: 可以直接编辑某个package当中的文件，格式是rosed package file，还可以用tab补全名称，如果文件名在package里不是唯一的，那么会呈现出一个列表，让你选择编辑哪一个文件。

• roswtf: 检查ROS各节点联系是否有错；检查环节变量等系统设置

rostopic

• rostopic echo -b BAGFILE -a, 显示bag文件的所有消息。但是所有消息一次显示，没什么意义。

• rostopic echo -n1， 只echo一段消息

• rostopic echo -p --nostr --noarr /topic_name， 不显示话题中的string和array部分

• rostopic echo /odom/pose/pose/position， 直接显示odom话题中的position部分

• rostopic bw topic_name： 用于显示主题所使用的带宽。

• rostopic delay topic_name : 显示ROS节点通信延迟

以下命令可以组合使用：

• rostopic list -p, 只显示发布的话题

• rostopic list -s, 只显示订阅的话题

• rostopic list -v, 显示话题细节，有几个发布者和订阅者

• rostopic list --host, 根据host名分组

有一次使用rostopic pub一个std_msgs/Int8的消息，结果报错 Inbound TCP/IP connection failed: field data must be an integer type 。 确实echo没有结果，另开一个终端，source 工作空间后成功。

rosmsg rosnode

• rosmsg package [package_name], 显示所有在这个包里面的消息

• rosmsg md5 [message], 显示某个消息的md5sum

• rosnode cleanup， 清除僵尸节点, 也就是清除那些实际关闭但能用rosnode list查看到的节点

• rosnode machine <hostname>： 用于列出指定计算机上运行的节点

• rospack find turtlesim, 输出指定package的路径, 输出/opt/ros/kinetic/share/turtlesim

• rospack depends turtlesim, 查看package的依赖项

roslaunch

• --wait, 延迟launch直到检测到roscore

roslaunch启动的时候，会尝试获得已有的rosmaster的id，如果没有获得，将会创建一个自己的。假如同时也有一个roslaunch和roscore启动，可能会导致run_id冲突。比如

可以使用roslaunch --wait rplidar_ros a2.launch，它会输出这样的信息：

1
2

roscore/master is not yet running, will wait for it to start
master has started, initiating launch

• --screen, 输出所有节点的日志，用于debugging.

• --dump-params, 以YAML格式输出launch文件中的所有参数

参考：roslaunch Commandline Tools

Local SLAM生成一系列子图时，一个global优化（通常被称为“优化问题”或者“sparse pose adjustment”）在后台运行，其主要工作是找到回环约束。它是通过scan-matching的方法用节点中收集到的scans与子地图进行匹配。它用于重新整理子地图，以便他们可以内联成一个全局地图。比如改变当前建成的轨迹以适应于回环检测的子地图对齐(align submaps with regards to loop closures)

• optimize_every_n_nodes: 一旦一定的trajectory nodes插入，就会分批执行优化，由你决定多么频繁的运行优化和调整大小。如果设置为0，是手动关闭全局SLAM，主要精力集中于局部SLAM。这是调试cartographer的第一件事情。 optimize_every_n_nodes里对应优化标志kRunOptimization，这个标志一共出现三次，还有FinishTrajectory和RunFinalOptimization。 前者的根源是node.FinishAllTrajectories();，     后者的根源在node.RunFinalOptimization();

全局SLAM是一种基于图优化的SLAM，本质上是位姿图优化，其是通过构建节点之间的约束，子地图之间的约束，然后优化产生的约束图。约束可以认为是一根连接所有节点的绳子。sparse pose adjustment加速这些绳子的结合。产生的网称为位姿图(Pose Graph)

约束可以在rviz中观察，这样调整global SLAM很方便

constraint_builder

1
2
3
4
5
6
7
8
9
10

POSE_GRAPH = {
  optimize_every_n_nodes = 90, # 每几个节点执行一次优化，设为0会关闭后端优化
  constraint_builder = {
    sampling_ratio = 0.3,  
    max_constraint_distance = 15., 
    min_score = 0.55, 
    global_localization_min_score = 0.6,    # 阈值，低于该阈值的全局定位不受信任
    loop_closure_translation_weight = 1.1e4, # 优化问题中的 回环约束 的 平移 的权重
    loop_closure_rotation_weight = 1e5,      # 优化问题中的 回环约束 的 旋转 的权重
    log_matches = true,     # 回环约束的日志

• sampling_ratio: 就是个采样器，用于 MaybeAddConstraint，值越小， 计算约束的频率越小

• max_constraint_distance: 非常重要 局部子图进行回环检测时，能成为约束的距离阈值，在 MaybeAddConstraint开头。如果设置过小，即使建图时走回同一个位置，也不会计算回环，函数里return。如果设置太大，计算量会增大很多，因为节点会和所有完成的子图匹配。

• log_matches: 得到 constraints builder(回环约束)的日志，默认是true。 在ConstraintBuilder2D::ComputeConstraint

• min_score: 局部子图进行回环检测时，扫描匹配分数的阈值，低于该阈值时不考虑匹配。 匹配得分不要太高，不然在有的地方不能回环。决定了哪些constraint添加到哪些节点间，这个会很大影响CPU。可以稍微增大到0.75 The histograms printed while Cartographer is running shows that you have a ton of loop closures constraints already, so lowering the score (which will raise the number of constraints) is probably not too useful. I’d rather ramp it up.

• global_localization_min_score: 用于有多条轨迹的情况，比如纯定位。不能设置太小，否则会出现超出地图的约束；也不能太大，否则难以出现约束。一般设置为 0.6

非全局约束（也称为子地图内部约束）是在节点之间自动建立，这些节点是轨迹上相对较近的节点。直观上，这些非全局约束保持了轨迹的内连关系。

全局约束（也称为回环检测约束或者子地图之间的约束）通常是在一个新的子地图和先前的节点之间进行搜索，那些先前的节点在空间上是足够的近（部分是在某个搜索window）。直观上，这些全局约束在结构上引进了一个结（打结），固定把两股子地图靠拢。

以下几项均为constraint_builder的子项

fast_correlative_scan_matcher

分支定界

1
2
3
4
5
6
7

fast_correlative_scan_matcher = 
{
  # 这里的window大小明显比前端 real_time_correlative_scan_matcher 的大
  linear_search_window = 7.,      # 依靠“分支定界”机制在不同的网格分辨率下工作，并有效地消除不正确的匹配
  angular_search_window = math.rad(30.), # 一旦找到足够好的分数（高于最低匹配分数），它（scan）就会被送入Ceres扫描匹配器以优化姿势。
  branch_and_bound_depth = 7,    # 至少为1，应当是值越大，后端的作用越强
},

ceres_scan_matcher

闭环检测的第二步，优化位姿图

1
2
3
4
5
6
7
8
9
10
11

ceres_scan_matcher = 
{
  occupied_space_weight = 20.,
  translation_weight = 10.,
  rotation_weight = 1.,
  ceres_solver_options = {
    use_nonmonotonic_steps = true,
    max_num_iterations = 10,
    num_threads = 1,
  },
},

use_nonmonotonic_steps：是否允许计算cost时，短暂增大。false会计算到局部最小; true则增加计算量，可越过局部最小。

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

  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  # 残差方程的参数配置
  optimization_problem = { 
  # Huber损失函数的尺度因子，核函数，用于filter outliers。该值越大，
  # (potential) outliers（潜在）异常值 的影响越大。
    huber_scale = 1e1, 
    acceleration_weight = 1e3, # IMU加速度的权重
    rotation_weight = 3e5,     # IMU旋转项的权重
    # 基于local SLAM的姿势在连续节点之间进行平移的权重
    local_slam_pose_translation_weight = 1e5,
    # 基于里程计的姿势在连续节点之间进行平移的权重
    local_slam_pose_rotation_weight = 1e5,    
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,

    fixed_frame_pose_translation_weight = 1e1, 
    fixed_frame_pose_rotation_weight = 1e2,
    # 可以记录Ceres全局优化的结果并用于改进您的外部校准
    log_solver_summary = false,
    # 作为IMU残差的一部分
    use_online_imu_extrinsics_in_3d = true, 
    fix_z_in_3d = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },
  
  max_num_final_iterations = 200, # 在建图结束之后会运行一个新的全局优化，不要求实时性，迭代次数多
  global_sampling_ratio = 0.003,
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10.,
  --  overlapping_submaps_trimmer_2d = {
  --    fresh_submaps_count = 1,
  --    min_covered_area = 2,
  --    min_added_submaps_count = 5,
  --  },
}

global_sampling_ratio只在PoseGraph2D::AddTrajectoryIfNeeded中的FixedRatioSampler机制使用

overlapping_submaps_trimmer_2d 机制

源码在PoseGraph2D的构造函数里，对应has_overlapping_submaps_trimmer_2d参数

Trimmer是一个删除子图的操作，其具体参数在cartographer/configuration_files/pose_graph.lua中

1
2
3
4
5
6

overlapping_submaps_trimmer_2d =
{
  fresh_submaps_count = 1,
  min_covered_area = 2,
  min_added_submaps_count = 5,
}

全局优化中的里程计

如果local SLAM 使用了单独的里程计(use_odometry = true), 我们可以相应地调整全局SLAM

有四个参数允许我们在优化中调整局部SLAM和里程计的各个权重:

1
2
3
4

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight
POSE_GRAPH.optimization_problem.odometry_translation_weight
POSE_GRAPH.optimization_problem.odometry_rotation_weight

参考：后端测试报告

现在的ROS提倡使用tf2

tf2经过重新设计，只提供tf的关键功能，不涉及转换等函数。

tf_conversions.transformations.quaternion_from_euler(roll, pitch, yaw)

每个listener都有一个buffer储存所有tf广播发出的transforms，当广播发出transform时，需要花点时间(毫秒级)才会进入buffer，所以请求now的transform时，会有一小段时间差。

使用tf2_ros::Buffer的lookupTransform()函数可以获得tf树的指定时间的转换。常用的指定时间是ros::Time(0)和ros::Time::now，前者是缓冲区中最新可用的转换，后者就是当前的时间。对于now，由于时间差，可能出现报警

有时会出现报警： Lookup would require extrapolation into the past

对于这个报警，我们有四种解决方法：

1. 完善lookupTransform的参数，增加ros::Duration(sec)，sec大于报警中的时间差即可
2. 使用tf2_ros::Buffer的canTransform函数，有可用的变换了再获得
3. 使用tf message filter
4. 忽视这个报警，未成功获取的新transform会放弃

一个比较优雅的程序是这样的:

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

// #include <ros/ros.h>
// #include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>

ros::init(argc, argv, "tf_node");
ros::NodeHandle nh;
tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
ros::Publisher pub = nh.advertise<geometry_msgs::TransformStamped>("transform",10);
ros::Rate rate(400);
while(nh.ok())
{
    geometry_msgs::TransformStamped trans;
    try {
        ros::Time now = ros::Time::now();
       // if(buff.canTransform("base_footprint","imu",now,
        // ros::Duration(0.03),NULL)
        // 30毫秒应该足够了
        trans = buff.lookupTransform("base_footprint",
                         "imu", now, 
                         ros::Duration(0.03) );
       // else
           // ROS_WARN("no transform in buffer");
    } catch (tf2::TransformException & ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    pub.publish(trans);
    rate.sleep();
}

lookupTransform获取的是TransformStamped消息，最后发布出来。因为加了Duration，try catch可以去掉。

由于实际上加了Duration，所以得到的不是当前的，而是上一个“当前”。换句话说，想得到当前最新的transform其实是没有意义的，一般都用ros::Time(0)，Wiki上也是这么说的

怎么才知道我们修改后的程序有效了？一个是看程序运行后是否报警，还有就是echo transform话题，结果可以看到头部

只看seq和时间，如果增长一直很流畅，那就是修改生效了。如果修改还有问题，比如Duration时间太短，seq在增长一会后会有停顿，然后继续增长，这就是buffer里面空了，tf数据还没有插入到里面

已知点在子坐标系中的坐标和父子坐标系的变换，求点在父坐标系的坐标

1
2
3
4
5
6
7
8
9
10

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

使用这段程序前，必须在find_package里添加 tf2-geometry-msgs，否则编译不成功。如果没有，需要先安装这两个包：

1
2
3

# 依赖库，  即使目前有，也要安装，可能需要更新
sudo apt-get install ros-kinetic-orocos-kdl
sudo apt-get install ros-kinetic-tf2-geometry-msgs

手动实现 tf2_ros::Buffer::transform函数

也就是用程序实现上面的转换

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

ros::init(argc, argv, "test_tf");
ros::NodeHandle nh;

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
geometry_msgs::TransformStamped transform =
        buff.lookupTransform("parent","child",ros::Time(0),ros::Duration(1));

ROS_INFO("transform from parent to child  x: %f",transform.transform.translation.x);
ROS_INFO("transform from parent to child  y: %f",transform.transform.translation.y);
ROS_INFO("transform from parent to child  z: %f\n",transform.transform.translation.z);

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
// 在 child 坐标系中的点
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

ROS_INFO("point in child: 2, 1, 4\n");
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ROS_INFO("point int parent, x: %f",point_out.point.x);
ROS_INFO("point int parent, y: %f",point_out.point.y);
ROS_INFO("point int parent, z: %f\n",point_out.point.z);

cout<<fixed<<setprecision(2);

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();
cout << "rotation matrix: " <<endl<< rotation <<endl<<endl;

Eigen::Matrix4d m;
m.block(0,0,3,3) = rotation;

Eigen::Vector4d Vcol, Vrow;
Vcol << transform.transform.translation.x,
        transform.transform.translation.y,
        transform.transform.translation.z, 1;
m.col(3) = Vcol;

Vrow << 0,0,0,1;
m.row(3) = Vrow.transpose();
cout << "transform matrix: "<<endl<< m<<endl<<endl;

Eigen::Vector4d point;
point << point_in.point.x,
         point_in.point.y,
         point_in.point.z, 1;

Eigen::Vector4d transformed_point = m * point;
cout << "transformed point: "<<endl<<transformed_point.head(3) << endl;

我们已知parent-child的变换，首先需要从位移和欧拉角获得齐次变换矩阵，把point_in变换为齐次坐标，然后左乘齐次矩阵，再取结果的前三个元素。 tf变换的本质就是左乘变换矩阵

常用函数

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

void tf2::Transform::setIdentity()
{
    // Matrix3x3 m_basis;
    m_basis.setIdentity();   // 单位矩阵
    // Vector3   m_origin;
    m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
}

Transform::operator*(const Transform& t) const
{
    return Transform(m_basis * t.m_basis, 
               (*this)(t.m_origin)  );
}

参考：
tf2_ros::Buffer Class
tf2_ros::MessageFilter
tf2_ros::BufferInterface
using “tf2” to transform coordinates

古月学院：位置角度平移旋转，“乱七八糟”的坐标变换

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