cartographer坐标系_cartographer个人对框架解读

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

一、cartographer和cartographer_ros的关系

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

1、新建自定义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开启订阅的激光点云回调函数//Node::HandleLaserScanMessage中会进行map_builder_bridge_.sensor_bridge(trajectory_id)

->HandleLaserScanMessage(sensor_id, msg, use_odom);

//再其中调用HandleLaserScanMessage会调用 SensorBridge::HandleLaserScan,//再其中调用SensorBridge::HandleRangefinder//再其中调用库函数trajectory_builder_->AddSensorData( trajectory_builder_->AddSensorData(

sensor_id, carto::sensor::TimedPointCloudData{

time,//时间 sensor_to_tracking->translation().cast(),//点云原点 carto::sensor::TransformTimedPointCloud( //将点云从Lidar坐标系转到body坐标系上 ranges, sensor_to_tracking->cast())});

注意：从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(

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】