源码注释参考: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时刻。