cartographer安装_cartographer个人对框架解读

最新推荐文章于 2024-07-13 17:15:55 发布

weixin_39830225

最新推荐文章于 2024-07-13 17:15:55 发布

阅读量486

点赞数

文章标签： cartographer安装

源码注释参考：slamcode源码注释2017版

整体介绍：April Lee：Cartographer源码阅读1——整体框架介绍

必读：知乎 April Lee：Cartographer源码阅读

还没来得及看无处不在的小土-Cartographer的总体框架与安装试用

没来得及看的博客 2020 年 3月随笔档案 - heimazaifei - 博客园

cartographer的代码主要包括两个部分：cartographer和cartographer_ros。

一、cartographer和cartographer_ros的关系

cartographer_ros相当于只给我们封装了一个ROS的皮，新建了一个cartographer_node节点，但其核心的任务都还是由cartographer来完成的。所以我们刚看源码时，当查看该节点的情况，追溯到map_builder_bridge_后就先暂时停止，知道这些任务最后交给了cartographer这个包中的MapBuilder_去处理就可以了。等我们研究完这个节点后再集中精力去看cartographer中的内容。

1、入口是nodehttp://main.cc【其中run函数中会：新建自定义node对象 和 库mapbuilder_对象】，所有ros端操作由node构造函数中加载和开启，最终通过Cator库的mapbuilder_类【map_builder_bridge_的成员变量】对象进行实质性操作. 库mapbuilder_的类是Cator库最核心的类，包含多条轨迹CollatedTrajectoryBuilder类型【含sensor_collated传感器分选器以及 GlobalTrajectoryBuilder类型{共同poseGragh和各自前端}】

2、在自定义的node构造时调用http://node.cc中构造函数【主要：发布消息【定时器】、提供服务【其中新轨迹服务中会launch开启所有传感器订阅】，均含对应回调函数】，提供服务/发布消息的回调函数中会调用某功能函数A【例如某回调函数中会调用：函数Node::Function A，其调用的是http://map_builder_bridge_.cc的同名函数A，再在里面调用cartographer库里的http://map_builder.cc的同名函数A，至此ros封装工作结束，转到cartographer库。

举例1：

http://node.cc中服务回调函数中会调用Node::FinishTrajectory()，再调用http://map_builder_bridge.cc的同名函数map_builder_bridge_.FinishTrajectory()，再调用cartographer库中http://map_builder.cc的同名函数MapBuilder::FinishTrajectory()，再调用cartographer库中二级FinishTrajectory()

举例2：

在StartTrajectoryService中回调函数Node::HandleStartTrajectory中会调用Node::AddTrajectory，在其中会调用map_builder_bridge_.AddTrajectory，在其中会调用库函数map_builder_->AddTrajectoryBuilder。

//Node::Run函数的map_builder是该函数局部变量[我之前理解错了，以为是全局变量]【根据配置构造好后】，
//通过Node::Run函数中Node构造函数将其传给了map_builder_bridge_对象的成员变量map_builder_
//之后调用的这个map_builder_bridge_的成员变量map_builder_

3、注意：ros端主要有三个bridge_：map_builder_bridge_【Node的成员对象】、 sensor_bridge【属于前者的成员类对象map_builder_bridge_.sensor_bridge】和tf_bridge【属于前者的成员类对象map_builder_bridge_.sensor_bridge.tf_bridge_】。

MapBuilderBridge和SensorBridge中分别主要调用了库MapBuilderInterface(构建了其对象map_builder_)和库TrajectoryBuilderInterface(构建了其对象trajectory_builder_)【Sensor信息通过回调直接调用TrajectoryBuilder库函数输入前端】这两个类的成员函数来处理

A、map_builder_bridge_：Node的成员变量。在提供服务和发布消息时会调用，该bridge最终调用库中map_builder的功能。MapBuilderBridge定义在/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h中，实现由...map_builder_bridge.cc完成。

B、 sensor_bridges_：map_builder_bridge_的成员变量。在StartTrajectoryService中回调函数Node::HandleStartTrajectory中会调用Node::AddTrajectory，在其中除了会调用map_builder_bridge_.AddTrajectory开启新的轨迹，同时也调用Node::LaunchSubscribers这个函数【开启消息订阅】。这个函数负责开启订阅各个传感器的消息，从而在消息回调函数中开启主要处理工作！注意：所有传感器的回调函数中会通过map_builder_bridge_.sensor_bridges_调用相关库函数。

//例如在Node::LaunchSubscribers开启订阅的激光点云回调函数

注意：从sensor_bridge的构造函数和赋值可知：ros端的sensor_bridges_[]的成员变量trajectory_builder_是个指针并在构造sensor_bridges_[]时会指向map_builder_对象中同一trajectory_id的trajectory_builder对象。ros端不会额外维护一段map_builder_，ros端和库使用的都是同一个map_builder对象。

  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));
//SensorBridge的构造函数
  explicit SensorBridge(
      int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
      ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);

按轨迹trajectory_id从MapBuilderBridge成员变量sensor_bridges_【unorder_map类型】中取出一个SensorBridge类的临时指针sensor_bridge_ptr。在处理传感器的数据时，都是通过调用map_builder_bridge_的一个类成员sensor_bridge_ptr来处理的，最终调用TrajectoryBuilder库函数输入进前端。

例如：

IMU：处理IMU的Node::HandleImuMessage回调函数，会进行两部分操作：

A：先传给Node.extrapolators_实现ros端IMU递推，

B：同时会调用调用的是map_builder_bridge_中的一个成员sensor_bridges_中某个trajectory_id的sensor_bridge对象临时指针sensor_bridge_ptr的成员函数来处理：sensor_bridge_ptr->HandleImuMessage，再在其中调用cartographer库中trajectory_builder_->AddSensorData函数【注意该trajectory_builder_指针尽管属于sensor_bridge的成员变量，但是他是指针且指向map_builder_对象中同一trajectory_id的trajectory_builder】【trajectory_builder_是CollatedTrajectoryBuilder类型【含sensor_collated传感器分选器以及 GlobalTrajectoryBuilder类型{共同poseGragh和各自前端}】，故分别传给前端LocalTrajectoryBuilder2D，也传给了后端poseGragh

Lidar：处理Lidar的Node::HandleLaserScanMessage回调函数，会调用map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage，深一步再调用 SensorBridge::HandleRangefinder()，再深一步利用sensor_bridge.tf_bridge_转换激光点云后调用cartographer库中trajectory_builder_->AddSensorData函数【http://global_trajectory_builder.cc，再在其中把激光点云信息传给前端和后端，前端传给local_trajectory_builder_->AddRangeData得到匹配结果，后端将该帧激光数据传给pose_graph_->AddNode】

4、ros端会有两个builder，一个是map_builder_，另一个是trajectory_builder_【结论：trajectory_builder_属于sensor_bridges成员指针变量，但本质上仍指向map_builder_->trajectory_builders_同一ID的Trajectory】

A、库类对象map_builder_是在http://node_main.cc的run函数中Node构造函数之前新建的对象。注意：map_builder的类成员中有一个后端pose_graph_指针和前端trajectory_builders_指针vector列表【含多条轨迹】，map_builder封装了整个Carto库的前端和后端功能！！！ros端封装的所有方法都会最终用到该对象。详见之后的对象分析！

B、库类对象指针trajectory_builder_【mapping::TrajectoryBuilderInterface* 类型】，该对象是放在单个sensor_bridge中，即属于std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges的成员变量，而sensor_bridges又属于map_builder_bridge_的成员变量【从这里也可以看出整个Carto主要就是Node.map_builder_bridge_和map_builder_两个对象，但最终本质还是同一map_builder_】。从下面SensorBridge的构造过程中可知：

  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));

结论：trajectory_builder_ 并没有新生成额外一个Trajectory对象，而是指向map_builder_->trajectory_builders_同一ID的Trajectory。故也是CollatedTrajectoryBuilder类型【详见map_builder的构造分析，含sensor_collator_和GlobalTrajectoryBuilder对象【构建需要前端local_trajectory_builder【LocalTrajectoryBuilder2D】和后端pose_graph_】】。

二、cartographer_node总结：

cartographer启动以后，在主函数main()中Run()进行Node构造,在Node构造中注册并发布了Topic和service：

[回调均是在开启新轨迹ID中调用Node::LaunchSubscribers是统一开启,详看上面senors_bridges_]

1. 在Node构造注册并发布了5个Topic, 并为5个Topic分别设置了定时器函数，在定时器函数中定期向Topic上广播数据：

|===1) Topic 1: kSubmapListTopic: 广播构建出来的submap的list

|-----------发布数据的函数：Node::PublishSubmapList

|-----------调用函数：map_builder_bridge_.GetSubmapList();再调用map_builder_的同名函数

|===2) Topic 2: kTrajectoryNodeListTopic：发布trajectory

|-----------发布数据的函数：Node::PublishTrajectoryNodeList Node::PublishTrajectoryStates：

|-----------调用函数：map_builder_bridge_.GetTrajectoryNodeList();其中再调用map_builder_的同名函数

|===3) Topic3: kLandmarkPoseListTopic

|-----------发布数据的函数：Node::PublishLandmarkPosesList

|-----------调用的函数：map_builder_bridge_.GetLandmarkPoseList()；其中再调用map_builder_的同名函数

|===4) Topic4: kConstraintListTopic

|-----------发布数据的函数：Node::PublishConstraintList

|-----------调用的函数：map_builder_bridge_.GetConstraintList()；其中再调用map_builder_的同名函数

|===5）Topic5: kScanMatchedPointCloudTopic

|-----------发布数据的函数：Node::PublishTrajectoryStates

|-----------调用的函数：map_builder_bridge_.GetTrajectoryStates()；其中再调用map_builder_的同名函数

|-----------这个函数名与Topic名对应不上，但是查看Node::PublishTrajectoryStates不难发现，函数中有一句scan_matched_point_cloud_publisher_.publish(...)实现了该功能：

scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
            carto::common::ToUniversal(trajectory_state.local_slam_data->time),
            node_options_.map_frame,
            carto::sensor::TransformTimedPointCloud(
                point_cloud, trajectory_state.local_to_map.cast<float>())));

2. 发布了4个Service，并为4个Service分别设置了句柄函数，而句柄函数也是通过调用map_builder_bridge_的成员函数来处理的。

|===1）Service 1: kSubmapQueryServiceName

|-----------句柄函数：Node::HandleSubmapQuery

|-----------实际处理的函数：map_builder_bridge_.HandleSubmapQuery；其中再调用map_builder_的同名函数

|===2) Service 2: kStartTrajectoryServiceName

|-----------句柄函数：Node::HandleStartTrajectory

|-----------调用Node.AddTrajectory,其中实际处理两个函数，一是：map_builder_bridge_.AddTrajectory等; 其中会调用map_builder_的同名函数在库map_builder对象中，新添加一条Trajectory对象，并赋予trajectory_id，同时并将其的指针放入新构造的SensorBridge对象中【map_builder_bridge_.sensor_bridges_[trajectory_id]】，使得sensor_bridges_的成员变量trajectory_builder_指针指向同一ID轨迹对象

|-----------二是：Node::LaunchSubscribers[trajectory_id]，这里需要额外注意的是这个函数。这个函数负责开启各个传感器的回调函数！！！很重要！！！。仔细读其中的每个处理函数，比如处理IMU的Node::HandleImuMessage[trajectory_id]函数，发现其实际调用的是map_builder_bridge_中的一个成员对象 auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id),并在该对象中sensor_bridge_ptr的函数来处理：sensor_bridge_ptr->HandleImuMessage。

|===3) Service 3: kFinishTrajectoryServiceName

|------------句柄函数：Node::HandleFinishTrajectory

|------------实际处理的函数：map_builder_bridge_.FinishTrajectory；其中再调用map_builder_的同名函数

|===4) Service 4: kWriteStateServiceName

|------------句柄函数：Node::HandleWriteState

|------------实际处理的函数：map_builder_bridge_.SerializeState；其中再调用map_builder_的同名函数

可以看到，cartographer_ros这个节点启动后所有东西都交给了map_builder_bridge_去处理【里面最终调用Carto库类对象map_builder_，开启实质操作】。

三、复杂的PoseExtrapolator对象：

第一部分：代码存在两个PoseExtrapolator对象

cartographer_node和cartographer都会维护一个extrapolator_对象。依次是：

1、ros端递推器：node->extrapolator_ 主要负责ros端的递推工作

2、库的前端递推器：LocalTrajectoryBuilder2D->extrapolator_主要负责库里前端的递推工作

举例：一个odom数据来了后，Node::HandleOdometryMessage[trajectory_id]会做两件事：

1、自身递推器：ros端会将消息添加到自己递推器，

2、发给Carto库：auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id),通过调用sensor_bridge_ptr->HandleOdometryMessage，来调用 trajectory_builder_->AddSensorData(sensorid)来实现给carto库传入指定sensorid传感器数据给前端和后端【从sensor_bridge的构造函数和赋值可知：ros端的sensor_bridge的成员变量trajectory_builder_是指向map_builder_对象中同一trajectory_id的trajectory_builder。ros端不会额外维护一段map_builder_，ros端和库使用的都是同一个map_builder对象。】【trajectory_builder_是CollatedTrajectoryBuilder类型【含sensor_collated传感器分选器以及 GlobalTrajectoryBuilder类型 {共同poseGragh和各自前端}】，故分别传给前端LocalTrajectoryBuilder2D，也传给了后端poseGragh：

A、前端：http://global_trajectory_builder.cc中AddSensorData 会调用前端

local_trajectory_builder_->AddOdometryData(odometry_data)

B、后端：

http://global_trajectory_builder.cc中AddSensorData会调用后端PoseGraph2D::AddOdometryData ，其中再会调用optimization_problem_->AddOdometryData(trajectory_id, odometry_data)

第二部分：每个PoseExtrapolator对象中都包含三个ImuTracker类型成员变量

PoseExtrapolator类中包含三个IMU的对象：

std

!!!注意:尽管都是ImuTracker对象，名字叫ImuTracker其实应该理解为广义的Tracker的意思【只负责track旋转量，因为当前源码中平移不用IMU，其中匀速假设用的是激光帧间线速度，里程计只用最新消息和激光帧前最新消息求平均线速度，】

imu_tracker_维护当前激光帧的CSM解算位姿，track当前激光帧位姿旋转量信息

odometry_imu_tracker_维护最新odom帧的递推位姿，track当前里程计帧位姿旋转量信息

extrapolation_imu_tracker_维护最新imu帧的递推位姿，track当前IMU帧位姿旋转量信息

当前端CSM 解算出位姿时：会用该位姿，统一更新/对齐上面三个位姿。

故imu_tracker_更新频率是随着激光频率的，odometry_imu_tracker_是随着odom频率，extrapolation_imu_tracker_是随着IMU消息频率的！！！

故由于有两个PoseExtrapolator都各自维护三个ImuTracker成员变量，有浪费计算能力的嫌疑！！！【例如：IMU被各自分别递推了一次】

第三部分：传感器消息的处理流程

以里程计数据为例，我们可以梳理一下传感器数据整个的处理流程：

cartographer_node中启动的StartTrajectory这个服务会订阅指定传感器数据---->传感器的ROS节点/Playbag广播到指定Topic上相关数据的Message---->接收到该数据由相应的处理函数处理，比如Node::HandleOdomMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridges[trajectory_id]变量进行处理---->调用了TrajectoryBuilder的虚函数AddSensorData()---->CollatedTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数

第四部分：cartographer库的主要实现【MapBuilder类】

MapBuilder是cartographer算法的最顶层设计，继承于MapBuilderInterface这个抽象接口。MapBuilder包括了两个部分，其中前端是TrajectoryBuilders用于多组Local Submaps的多个轨迹建立与维护；后端PoseGraph部分用于Loop Closure.

// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface
{
  common::ThreadPool thread_pool_;//线程池。个人猜测，应该是为每一条trajectory都单独开辟一个线程
  std::unique_ptr<PoseGraph> pose_graph_;//一个PoseGraph的智能指针
  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;//传感器数据的智能指针
  //一个vector，管理所有的TrajectoryBuilderInterface的轨迹;应该是每一个trajectory对应了该vector的一个元素
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> trajectory_builders_;
}

注意：在http://node.cc中构造的map_builder对象类成员主要有：

|===成员1、唯一的一个pose_graph_指针 ：std::unique_ptr<PoseGraph> pose_graph_

|===成员2、sensor_collator_指针：std::unique_ptr<sensor::CollatorInterface> sensor_collator_传感器数据分发器

|===成员3、trajectory_builders_指针vector列表： std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>，其中每个成员代表一个轨迹信息，成员类型是CollatedTrajectoryBuilder【继承于mapping::TrajectoryBuilderInterface】。

从下面trajectory_builders_的push_back过程可发现：CollatedTrajectoryBuilder的构造函数需要sensor_collator_【成员2】和GlobalTrajectoryBuilder对象【构建需要前端local_trajectory_builder【局部变量，LocalTrajectoryBuilder2D类型】和后端pose_graph_指针【成员1】】

//注意：只有CollatedTrajectoryBuilder 和 GlobalTrajectoryBuilder 是继承TrajectoryBuilderInterface

//注意：LocalTrajectoryBuilder2D并没有继承TrajectoryBuilderInterface.【TrajectoryBuilderInterface对象需要包含前端和后端】，
//而LocalTrajectoryBuilder2D只有前端(i.e. pose extrapolator, scan matching, etc.)

//创建一个新的TrajectoryBuilder并返回它的trajectory_id.

!!!注意:CollatedTrajectoryBuilder构造时会调用sensor_collator_->AddTrajectory

sensor_collator_

！！！注意：CollatedTrajectoryBuilder【分发轨迹建造器】类型继承自mapping::TrajectoryBuilderInterface。为什么叫这个名字呢？我理解是该类的是由sensor_collator 和 GlobalTrajectoryBuilder对象【也是继承于TrajectoryBuilderInterface】构叠加造成的。相当于给TrajectoryBuilder叠加了sensor_collator的内容！叠加前后都是继承自mapping::TrajectoryBuilderInterface。其中GlobalTrajectoryBuilder对象中包括了前端一条轨迹local对象和后端位姿图【含前端LocalTrajectoryBuilder2D类型的 local_trajectory_builder和后端mapping::PoseGraph2D* 类型pose_graph【也就是成员1】】

一个MapBuilder的类对应了一整个建图过程【内含多条/次轨迹】，在整个建图过程中，用于全局优化的PoseGraph的对象只有一个，即pose_graph_，而这个变量是在MapBuilder的类构造函数中就生成了。在AddTrajectorybuilder函数中只需要检查一下pose_graph_是否符合PoseGraph2D或PoseGraph3D的情况。而一个trajectory对应了机器人运行一次的轨迹。在图建好后机器人可能多次运行。每一次运行都是新增一条trajectory，因此，需要动态地维护一个trajectory的列表。每生成一个trajectory时都是调用AddTrajectoryBuilder来创建的。

Node

四、submaps

//应该一个轨迹只有一个ActiveSubmaps2D对象【注意该submaps_只有两个submaps，并不是所有submap的list】，注意ActiveSubmaps2D是带有s的【其成员变量有submaps_】，不同上面的Submap2D对象【代表一个submap】
//在ActiveSubmaps2D构造函数中会Create了一个range_data_inserter_，submap2d构造中不会create一个range_data_inserter_
//ActiveSubmaps2D::InsertRangeData中会调用submap->InsertRangeData

ActiveSubmaps2D的两个成员变量  
std::vector<std::shared_ptr<Submap2D>> submaps_;//重要成员变量，一个轨迹只有一个submaps_【注意该submaps_只有两个submaps】
std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;//只有ActiveSubmaps2D对象维护一个

五、PoseExtrapolator对象的初始化

5.1 代码存在两个PoseExtrapolator对象。 cartographer_node【Node的成员变量extrapolators_中每个轨迹ID一个】和cartographer库都会维护一个extrapolator_【LocalTrajectoryBuilder2D的成员变量】对象。

5.2 调用和构造

5.2.1：cartographer_node中：

Node::AddTrajectory中调用Node::AddExtrapolator(trajectory_id, options)会给每个轨迹ID新建一个PoseExtrapolator对象对象放入extrapolators_

5.2.2：cartographer库中：库前端的PoseExtrapolator对象初始化是在AddImuData或AddRangeData，判断为空时新构造extrapolator_对象

A：3d时：LocalTrajectoryBuilder3D::AddImuData时会判断extrapolator_ == nullptr时，调用extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu新初始化构建一个extrapolator_对象

B：2d时：会在添加IMU数据 LocalTrajectoryBuilder2D::AddImuData【优先】或添加激光帧数据时LocalTrajectoryBuilder2D::AddRangeData 【不用IMU时】时调用 LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time)判断extrapolator_ == nullptr时构建新的extrapolator_，在其内部会调用PoseExtrapolator::AddPose构建三个ImuTracker。

5.3 三个ImuTracker都是extrapolator_的成员变量，其初始化是在：

PoseExtrapolator::AddPose会判断imu_tracker_为空时构建imu_tracker_，

if (imu_tracker_ == nullptr)
imu_tracker_ = common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);

进而将imu_tracker_复制给odometry_imu_tracker_和extrapolation_imu_tracker_

odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);//初始化情况时：相当于新构造一个；正常：直接拷贝
  extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);//初始化情况时：相当于新构造一个；正常：直接拷贝

六、

从开头回顾一下整个处理过程：

1. 当刚初始化一个TrajectoryBuilder时：

初始时，用处发出AddTrajectory的服务请求-->cartographer_node调用MapBuilderBridge::AddTrajectory函数，同时LaunchSubscribers订阅传感器的Topic-->该函数调用MapBuilder::AddTrajectory-->该函数中生成一个LocalTrajectoryBuilder2D,然后通过CreateGlobalTrajectoryBuilder把LocalTrajectoryBuilder2D的参数、poseGragh和回调函数传给GlobalTrajectoryBuilder-->以GlobalTrajectoryBuilder和Sensor_collator_为参数构造了CollatedTrajectoryBuilder,并给CollatorInterface中的AddTrajectory函数注册回调函数的实际地址。此时消息也订阅了,也新建了一个轨迹(新生一个前端local,并对应上全局posegragh了)

2. 在初始化完成后，每次传感器往Topic上广播Message后：

传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理，比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的虚函数AddSensorData()---->CollatedTrajectoryBuilder或GlobalTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数-->这些函数通过LocalTrajectoryBuilder2D将传感器数据压入相应的数据队列-->LocalTrajectoryBuilder2D通过AddRangeData等函数处理传感器数据

七、

//1、Run函数中会根据路径加载配置文件
//调用LoadOptions函数读取目录下的lua配置文件，将一些参数赋给node_options, trajectory_options：
//调用定义在http://node_options.cc中的函数LoadOptions加载lua配置文件。
//注意：LoadOptions函数将Lua文件转成proto对象参数，以供通过map_builder构造函数喂给Carto库
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
//！！！注意ros端的配置文件backpack_2d.lua的开头include两个库配置文件
//"map_builder.lua"和trajectory_builder.lua。其中：
//map_builder.lua的开头 include "pose_graph.lua"
//trajectory_builder.lua的开头include "trajectory_builder_2d.lua" include "trajectory_builder_3d.lua"

由于都是在文件开头include，故后面可以重新在ros端赋值！故：所有ros端的配置参数，优先级是最高的

八、

//库前端主函数：http://Global_trajectory_builder.cc的AddSensorData会前端调用
//该LocalTrajectoryBuilder2D::AddRangeData，在其中会调用
//LocalTrajectoryBuilder2D::AddAccumulatedRangeData，在其中会调用
//LocalTrajectoryBuilder2D::InsertIntoSubmap，如果是关键帧Node会调用
//active_submaps_.InsertRangeData，会对
//1、其中老新submap分别调用submap->InsertRangeData，插入新激光数据，实现新老submap的更新
//2、如果Node数量超出阈值，就ActiveSubmaps2D::AddSubmap，将老submap移出、新变老、新建一个submap
//将该帧激光点云和位姿和重力方向放入 submap坐标系上，并返回中InsertionResult结构体
//这是该帧激光已经完成CSM匹配的情况下，调用该函数更新submap

九、

//submap2D.grid_在AddSubmap中实例化成了ProbabilityGrid类型对象
//注意class ProbabilityGrid : public Grid2D ，class Grid2D : public GridInterface
//该类型继承了 (/mapping/grid_interface.h)，在GridInterface只有一个空壳子

//按照Correspondence Cost与Probability之间的映射关系，
//应该说Probability是指栅格被占用的概率，而Correspondence Cost则是栅格空闲的概率

虽然ProbabilityGrid的接口都是占用概率，但Grid2D中的存储都是空闲概率。而且Cartographer通过查表的方式来更新栅格单元的占用概率，Grid2D使用uint16的形式存储数据。

十、

一个私有变量的获取函数两种形式

// 返回记录栅格地图概率值的向量，const 的别名&，不能变化，相对于mutable

注意：correspondence_cost_cells()后不要有赋值操作，否则仍然不是引用，开销会变大

有人可能会问GetData也可以写成这样：
const Data& GetData()
{
    return m_data;
}
这样的话，调用方常常容易写成这样：
1 MyData mydata("", Data(100));
2 Data data = mydata.GetData();   //赋值操作：这会有个赋值的过程，会把mydata.m_data赋给data
3 cout << data.value << endl;     //data.value = 100
4 data.value = 50;                //正确，data.value=50，但mydata.m_data.value还是100
这样调用时会有一个结果赋值的过程，如果Data是一个复杂的类，会有较大的开销，其效果与下面这种方式是一样的：
1 Data GetData()
2 {
3     return m_data;
4 }

当然，如果调用方这样使用是正确的：

const Data& GetData()
{
    return m_data;
 }
 MyData mydata("", Data(100));
 const Data& data = mydata.GetData();    //这会有个赋值的过程，会把mydata.m_data赋给data
 cout << data.value << endl;             //data.value = 100
 //data.value = 50;                      //错误，data是一个对常量的引用，不能改变其邦定的对象

十一、ScanMatch

（一）前端

Carto的前端有两种ScanMatch，都在点云对齐后的AddAccumulatedRangeData函数调用LocalTrajectoryBuilder2D::ScanMatch函数里面：

1、CSM：correlative_scan_matching：online_CSM开关控制是否执行：暴力匹配【枚举的，分辨率不能搞太高了，否则计算量太大。但不会局部收敛】。RealTimeCorrelativeScanMatcher匹配，当你没有其他数据或者不信任其他数据时，它使用了一种与回环检测相似的匹配策略，并把匹配的结果作为CeresScanMatcher匹配的初始值，这种匹配的代价较高它会淹没掉其他传感器的作用，但是它在特征丰富的环境比较鲁棒，缺点计算量增加。

2、Optimized：ceres_scan_matcher_：肯定执行的，用的GN或LM之类非线性最小二乘。使用ceres优化库构建优化方程来进行匹配，这种匹配适用于提供良好位姿初始值的情况，可以实现亚像素的匹配，速度快，缺点是如果初始值不理想，匹配也有可能不理想。

特点：前者CSM是暴力匹配，对初值不敏感！后者是基于非线性最小二乘方法，对初值敏感，容易局部收敛，优点是精度高。

（二）后端

后端的分枝定界方法，fast_correlative_scan_matcher_2d.h

http://constraint_builder_2d.cc中DispatchScanMatcherConstruction(submap_id, submap->grid())

十二、

ceres_scan_matcher_尝试切换为TSDF，计算消耗会变大，精度理论会变高。但源码作者实测不那么实用。精度没提升，受可调参数影响大。

？？？视频讲解中：【其源码有注释】

Imu_tracker就只有一个作用，估计IMU重力方向？

1、先Imu_tracker::Advance()根据上一帧角速度更新旋转向量

2、有IMU线加速度就估计当前帧重力方向【根据三个轴的加速度，就能估计出来重力方向】【如果是消费级IMU，噪声太大，导致重力方向噪声很大不准。故加入了指数滑动平均滤波】【离当前时刻越远的值，给的权重越小{指数权重}，的一阶低通滤波】【目前代码中无噪声的IMU的线加速度三维向量就是重力矢量】，得到更新后重力方向就可以修正上一步的旋转向量【只能修正roll和pitch】。没有IMU线加速度，就水平假设，给定单位向量(Eigen::Vector3d::UnitZ()

3、有IMU角速度就更新角速度，以备再来新IMU帧时使用【也就是第一步】。无IMU角速度，就优先选odom角速度，若无odom，就选位姿匀速假设的角速度

重力向量gravity_vector_：该帧坐标系下的重力方向表达矢量。源码中：如果上一时刻相距很远或者不用滤波器平滑，当前的线加速度三维矢量就等于当前的重力向量！！！每来一个IMU帧的加速度计信号既可以根据重力向量

//将imu_tracker之后，当前激光点之前的所有IMU数据进行递推，将imu_tracker递推到紧挨着当前激光点之前IMU时刻。

weixin_39830225

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
cartographer安装_cartographer个人对框架解读

源码注释参考：slamcode源码注释2017版整体介绍：April Lee：Cartographer源码阅读1——整体框架介绍必读：知乎 April Lee：Cartographer源码阅读还没来得及看无处不在的小土-Cartographer的总体框架与安装试用没来得及看的博客 2020 年 3月随笔档案 - heimazaifei - 博客园cartographer的代码主要包括两个部分：...
复制链接

扫一扫