先看main函数，省略google库的部分：

1
2
3
4
5
6

::ros::init(argc, argv, "cartographer_node");
::ros::start();

cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();

实际上就是Run函数，流程如下：

1
2
3
4
5
6
7
8

constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
// 从lua文件加载配置
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
    LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

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

// 核心变量： Cartographer的地图构建器
auto map_builder =
    cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
        node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer);
// 加载现有地图的情况
if (!FLAGS_load_state_filename.empty()) {
  node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
  // 核心函数
if (FLAGS_start_trajectory_with_default_topics) {
  node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();

注意：MapBuilder类的对象在这里就实例化了，也是唯一的对象，而不是在以为的MapBuilderBridge

1
2
3
4
5
6

node.FinishAllTrajectories();
node.RunFinalOptimization();

if (!FLAGS_save_state_filename.empty()) {
  node.SerializeState(FLAGS_save_state_filename);
}

显然我们需要看的函数是StartTrajectoryWithDefaultTopics

node.FinishAllTrajectories(); 和 node.RunFinalOptimization();是停止建图才执行的，一直到后端优化那里再分析。

proto机制

编写应用程序的时候，往往需要将程序的某些数据存储在内存中，然后将其写入某个文件或是将它传输到网络中的另一台计算机上以实现通讯。这个将程序数据转化成能被存储并传输的格式的过程被称为序列化，而它的逆过程则可被称为反序列化。简单来说，序列化：将对象变成字节流的形式传出去。反序列化：从字节流恢复成原来的对象。

为什么要序列化：假如你有一个数据结构，里面存储的数据是经过很多其它数据通过非常复杂的算法生成的，由于数据量很大，算法又复杂，因此生成该数据结构所用数据的时间可能要很久（也许几个小时），生成该数据结构后又要用作其它的计算，那么你在调试阶段，每次运行个程序，就光生成数据结构就要花上这么长的时间，无疑代价是非常大的。如果你确定生成数据结构的算法不会变，那么就可以通过序列化技术生成数据结构数据存储到磁盘上，下次重新运行程序时只需要从磁盘上读取该对象数据即可，所花时间只有读一个文件的时间，节省了我们的开发时间。

Protobuf性能好，效率高；拥有代码生成机制，数据解析类自动生成；支持很多语言，包括C++、Java、Python；同时也是跨平台的，所以得到了广泛的应用。正常情况下你需要定义proto文件，然后使用IDL编译器编译成你需要的语言。目前proto存在2和3两个版本，最好用3.与2相比，3的改变有：移除了required字段、移除了缺省值等

proto的语法见proto文件语法 和 Protobuf 终极教程

生成C++代码的命令是： protoc -I=. --cpp_out=. test.proto. -I指定protoc的搜索import的proto的文件夹,cpp_out是生成的目录，生成的文件类型是pb.h和pb.cc。 对于Cartographer，proto文件在源码的cartographer/cartographer/，生成的pb.h文件在/usr/local/include/cartographer

以后会发现有些参数不在lua里，但在proto里，proto是 Cartographer原本的参数描述的方式，lua是catographer_ros的封装。proto定义了参数，lua具体描述参数的数值，他俩就跟class和实例化的对象一样

我尝试在lua中添加proto中特有的参数，比如collate_by_trajectory，结果运行Cartographer还是失败，可能还是不兼容。

之前看depthimage_to_laserscan包，发现它的订阅发布采用的是所谓 lazy模式。今天研究了一下，发现并不复杂。先看advertise函数的一个原型：

1
2
3
4
5
6
7

Publisher ros::NodeHandle::advertise(const std::string& topic,
	uint32_t 	queue_size,
	const  SubscriberStatusCallback & 	connect_cb,
	const  SubscriberStatusCallback & 	disconnect_cb = SubscriberStatusCallback(),
	const  VoidConstPtr & 	tracked_object = VoidConstPtr(),
	bool   latch = false 
)

话题有订阅者时，发布者会触发connect_cb函数，停止订阅时又触发disconnect_cb函数。如果让它们做类成员函数，就使用Boost::bind形式，tracked_object暂时不用。所以程序可以这样写：

1
2
3
4
5
6
7
8
9
10

void connectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("Subscriber Name: %s", singlePub.getSubscriberName().c_str());
}
void disconnectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("disconnectCb");
}
......
  ros::Publisher pub = nh.advertise<std_msgs::Int8>("topic", 50, connectCb,disconnectCb);

再来看depthimage_to_laserscan的程序，构造函数里是这样的：

1
2
3
4

pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, 
	boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), 
	boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1)
	);

1
2
3
4
5
6

if (!sub_ && pub_.getNumSubscribers() > 0)
{
	ROS_DEBUG("Connecting to depth topic.");
	image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
	sub_ = it_.subscribeCamera("/camera/depth/image_raw", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
}

而depthCb是：

1
2

sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg);
    pub_.publish(scan_msg);

disconnectCb里，如果另一个节点不再订阅scan话题，本节点就会sub_.shutdown();；如果再有订阅scan，又可以通过connectCb使用sub_，设计确实很巧妙

参考：
Publisher advertise
SingleSubscriberPublisher

landmark是路标点，apriltag、二维码等强特征，static landmark 的作用是用于快速定位的。

对landmark的观测是相对于Cartographer的tracking frame，用户负责发布话题landmark，类型cartographer_ros_msgs/LandmarkList. cartographer订阅landmark之后，会发布话题/landmark_poses_list，类型visualization_msgs/MarkerArray，在rviz上显示出来，用于确认cartographer计算出landmark在map坐标系中的坐标。 Cartographer负责建立landmark的约束，与栅格地图一起优化。

机器人应当一直运动以产生trajectory优化，如果机器人不动，无法触发优化，landmark无法加入图优化框架。landmark的观测应当是相对于 tracking frame

Try the following to see an impressive effect (only for testing):
假如进行以下步骤:

1. 将 landmark 的权重设置的很大，比如 1e8
2. 观测一个id为0的 landmark
3. 把机器人开到另一个地方
4. 又一个id为0的 landmark

第二次观测显然是错误的，但是由于landmark的极高权重, 机器人位姿会跳跃

lua 设置

设置 use_landmarks = true，话题landmark应具备一定频率，默认要求最低10Hz。话题的消息可以是空的，但必须提供，因为Cartographer严格给传感器数据按时间排序，使landmarks deterministic. 但是也可以设置TRAJECTORY_BUILDER.collate_landmarks = false，这样landmark就是non-deterministic而且是非阻塞的。GPS也是同理。

landmarks_sampling_ratio = 1 landmark观测的权重比，当某个观测不太靠谱的时候，可以把1改为较小的权重，例如0.5，从而减小对整体优化的错误影响。

如果只获得landmark的相对位置，没有朝向，那就把landmark rotation weight设置为0

源码

要知道tag在相机坐标系的位置，相机坐标系和base_link之间的关系

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

void Node::HandleLandmarkMessage(
    const int trajectory_id,  const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr&  msg)
{
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse())
      return;

  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
  // 添加下面代码
  geometry_msgs::TransformStamped stamped_transform;

  stamped_transform.header.stamp = ros::Time::now();
  stamped_transform.header.frame_id = "base_link";
  stamped_transform.child_frame_id = "QR_Code";
  stamped_transform.transform =  ToGeometryMsgTransform(ToRigid3d(msg->landmarks[0].tracking_from_landmark_transform).inverse() );

  tf_broadcaster_.sendTransform(stamped_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

void Node::PublishLandmarkPosesList(
    const ::ros::WallTimerEvent& unused_timer_event)
{
   // landmark话题必须有订阅者，一般是rviz
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0)
  {
    absl::MutexLock lock(&mutex_);
    landmark_poses_list_publisher_.publish(
        map_builder_bridge_.GetLandmarkPosesList() );
  }
}

visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList()
{
  visualization_msgs::MarkerArray landmark_poses_list;
  const std::map<std::string, Rigid3d> landmark_poses =
      map_builder_->pose_graph()->GetLandmarkPoses();
  LOG(INFO)<<" landmark pose size: "<< landmark_poses.size();

  for (const auto& id_to_pose : landmark_poses) {
    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
        id_to_pose.second, node_options_.map_frame));
  }
  return landmark_poses_list;
}

publish_fake_random_landmarks.py

Cartographer提供这个文件发布假的landmark，不用仔细研究内容。 节点landmark_sampler，发布话题landmark，类型cartographer_ros_msgs/LandmarkList。

这个节点不是一直发布landmark，有的消息是空的

1
2
3
4
5
6
7

header: 
  seq: 546
  stamp: 
    secs: 1650511353
    nsecs:  89869022
  frame_id: ''
landmarks: []

有内容的消息是

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

header: 
  seq: 129
  stamp: 
    secs: 1650511071
    nsecs: 393965959
  frame_id: ''
landmarks: 
  - 
    id: "112b2"
    tracking_from_landmark_transform: 
      position: 
        x: 0.329674929632
        y: 0.601299422592
        z: 0.0813299308386
      orientation: 
        x: -0.881422944417
        y: 0.388576528204
        z: -0.260058416952
        w: 0.0668692347593
    translation_weight: 100000.0
    rotation_weight: 100000.0
  - 
    id: "32c2b"
    tracking_from_landmark_transform: 
      position: 
        x: 0.747269396167
        y: 0.256305093347
        z: 0.389155962116
      orientation: 
        x: -0.349830783746
        y: -0.891493805837
        z: -0.163724271997
        w: -0.236752148157
    translation_weight: 100000.0
    rotation_weight: 100000.0
  - 
    id: "2c33a"
    tracking_from_landmark_transform: 
      position: 
        x: 0.698094719569
        y: 0.886938424567
        z: 0.408522616781
      orientation: 
        x: -0.242061311724
        y: 0.292763269814
        z: 0.419295012755
        w: 0.824553019213
    translation_weight: 100000.0
    rotation_weight: 100000.0
  - 
    id: "c3b1c"
    tracking_from_landmark_transform: 
      position: 
        x: 0.703663491421
        y: 0.146106286628
        z: 0.589856404568
      orientation: 
        x: -0.0750314150542
        y: -0.80116295871
        z: 0.151662985651
        w: 0.574026601412
    translation_weight: 100000.0
    rotation_weight: 100000.0
  - 
    id: "c12aa"
    tracking_from_landmark_transform: 
      position: 
        x: 0.409531720117
        y: 0.286439128213
        z: 0.686302598836
      orientation: 
        x: -0.326127457574
        y: -0.221216268782
        z: -0.89427654661
        w: -0.212070040376
    translation_weight: 100000.0
    rotation_weight: 100000.0

参考:
Understanding the Landmark Observation Concept
Cartographer源码阅读——带Landmark的demo运行和测试

参考：这篇博客，意外的是我配置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用于机器人纯定位