本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156，现在上传资源自己不能选下载需要的积分。。。完全靠系统。

本篇文章只简要分析　cartographer_ros 下的代码，cartogrpaher 下的代码请见另一篇文章

cartographer_ros/cartographer_ros/node_main.cc

main()函数调用Run(), 在Run() 首先进行了参数的读取，包括node_options 和 trajectory_options，然后初始化了 MapBuilder 和 Node，然后有条件的分别调用 Node 的如下方法。

  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();node.FinishAllTrajectories();node.RunFinalOptimization();if (!FLAGS_save_state_filename.empty()) {node.SerializeState(FLAGS_save_state_filename);}

在main()函数中有一个 ros::start(); ，我没找到这个的定义。。。

cartographer_ros/cartographer_ros/node.cc

// Wires up ROS topics to SLAM. 将ROS主题连接到SLAM。

HandleStartTrajectory() --> AddTrajectory()

首先在Node的构造函数中将 HandleStartTrajectory() 注册到服务中，然后通过服务进行方法的调用。

HandleStartTrajectory() 函数首先检查trajectory options，然后检查topics，都通过了之后调用AddTrajectory()。

int Node::AddTrajectory(const TrajectoryOptions& options,const cartographer_ros_msgs::SensorTopics& topics) {const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>expected_sensor_ids = ComputeExpectedSensorIds(options, topics);const int trajectory_id =map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);AddExtrapolator(trajectory_id, options);AddSensorSamplers(trajectory_id, options);LaunchSubscribers(options, topics, trajectory_id);is_active_trajectory_[trajectory_id] = true;for (const auto& sensor_id : expected_sensor_ids) {subscribed_topics_.insert(sensor_id.id);}return trajectory_id;
}

AddTrajectory() 方法根据配置文档对 extrapolator 和 SensorSample 进行参数配置。然后通过 LaunchSubscribers() 方法将几种传感器的数据处理函数通过boost进行回调函数的注册，即每次相应类型的消息到来，都会进行 HandleImuMessage()等这几个数据处理函数的调用。

void Node::HandleImuMessage(const int trajectory_id,const std::string& sensor_id,const sensor_msgs::Imu::ConstPtr& msg) {carto::common::MutexLocker lock(&mutex_);if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {return;}auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);if (imu_data_ptr != nullptr) {extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);}sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

可以看到，如果存在IMU数据，就将IMU数据输入到 extrapolator 中，如果有 Odom 数据，也将 odom 数据输入到 extrapolator 中。

IMU数据类型的处理是通过 map_builder_bridge_.sensor_bridge(trajectory_id)->HandleImuMessage(sensor_id, msg); 进行的。也就是转到了 map_builder_bridge.h ，与 sensor_bridge.h , map_build 是进行地图构建的过程，sensor_bridge 是转换数据类型以及坐标变换。

cartographer_ros/cartographer_ros/sensor_bridge.h

Converts ROS messages into SensorData in tracking frame for the MapBuilder.

ROS消息转换为 MapBuilder 的 tracking frame 下的 SensorData

// 将 里程计数据 传入 carto::sensor::OdometryData, 返回指向 carto::sensor::OdometryData 的智能指针
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {const carto::common::Time time = FromRos(msg->header.stamp);const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(msg->child_frame_id));if (sensor_to_tracking == nullptr) {return nullptr;}return carto::common::make_unique<carto::sensor::OdometryData>(carto::sensor::OdometryData{time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

通过 ToOdometryData() ToImuData() 将 ROS 下的消息类型转换为 Cartographer 的数据类型。并将 odom 的数据通过乘以一个坐标变化矩阵转换到 tracking frame 坐标系下。

laserscan, MultiEchoLaserScanMessage 的数据处理是调用 HandleLaserScan() --> HandleRangefinder()，PointCloud2Message 的数据处理是直接调用 HandleRangefinder(), 代码如下

void SensorBridge::HandleLaserScan() {...}void SensorBridge::HandleRangefinder(const std::string& sensor_id, const carto::common::Time time,const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {const auto sensor_to_tracking =tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));if (sensor_to_tracking != nullptr) {trajectory_builder_->AddSensorData(sensor_id, carto::sensor::TimedPointCloudData{time, sensor_to_tracking->translation().cast<float>(),carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())});}
}

所有的传感器数据包括 odom imu nav_fix landmark laser multiEchoLaser pointcloud2 的处理都是先通过 cartographer_ros/msg_conversion.h 先将ROS 下的消息类型转换成 Carto 的消息类型，PointCloud2 的消息类型是直接应用 PCL 的　pcl::fromROSMsg() 转换的，在传入 caro 的数据类型里。

然后在将数据传入到 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData() 的重载函数中。

cartographer/mapping/trajectory_builder_interface.h　

// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.

用于2D和3D SLAM。实现了整体的SLAM过程，即用于　初始姿势估计的local SLAM，用于检测循环闭合的扫描匹配，以及用于计算优化姿势估计的稀疏姿势图优化。

cartographer_ros/cartographer_ros/map_builder_bridge.h

node.cc 调用了如下方法，也就是进入到了 sensor_bridge.h 文件。

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {return sensor_bridges_.at(trajectory_id).get();
}

首先，在头文件中定义了 TrajectoryState 结构体。内置结构体　LocalSlamData　包含了匹配之后的坐标变换 local_pose，以及估计 local_pose 的时刻，以及 RangeData 数据。之后　TrajectoryState 结构体　还包含　local - map 的　坐标变换，local - tracking 的坐标变换（为什么用指针？？？）, 以及　trajectory_options.

总结，TrajectoryState　就是进行 SLAM 之后得到的　位姿坐标，以及相对于 map 和 tracking frame 的坐标变换。

  struct TrajectoryState {// Contains the trajectory state data received from local SLAM, after// it had processed accumulated 'range_data_in_local' and estimated// current 'local_pose' at 'time'.struct LocalSlamData {::cartographer::common::Time time;::cartographer::transform::Rigid3d local_pose;::cartographer::sensor::RangeData range_data_in_local;};std::shared_ptr<const LocalSlamData> local_slam_data;cartographer::transform::Rigid3d local_to_map;std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;TrajectoryOptions trajectory_options;};

RangeData 包含了一帧雷达数据的　起点坐标，hit障碍物的点　returns，以及没有扫到障碍物的点　misses ，并将 misses 用一个配置的距离代替。

// cartographer/sensor/range_data.h// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance. It is assumed that
// between the 'origin' and 'misses' is free space.
struct RangeData {Eigen::Vector3f origin;PointCloud returns;PointCloud misses;
};

node.cc 的 AddTrajectory() --> map_build_bridge.h 的 AddTrajectory() --> map_builder_interface.h 的　AddTrajectoryBuilder() , 并且将 OnLocalSlamResult() 方法通过回调函数的形式注册进去。

并且对sensor_bridges_ 进行了第0个 trajectory_id 的初始化，并将　TrajectoryBuilder　传进去，再调用　 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData()　方法进行传感器数据的处理。

int MapBuilderBridge::AddTrajectory(const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&expected_sensor_ids,const TrajectoryOptions& trajectory_options) {const int trajectory_id = map_builder_->AddTrajectoryBuilder(expected_sensor_ids, trajectory_options.trajectory_builder_options,::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,::std::placeholders::_1, ::std::placeholders::_2,::std::placeholders::_3, ::std::placeholders::_4,::std::placeholders::_5));LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";// Make sure there is no trajectory with 'trajectory_id' yet.CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);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));auto emplace_result =trajectory_options_.emplace(trajectory_id, trajectory_options);CHECK(emplace_result.second == true);return trajectory_id;
}

OnLocalSlamResult()

void MapBuilderBridge::OnLocalSlamResult(const int trajectory_id, const ::cartographer::common::Time time,const Rigid3d local_pose,::cartographer::sensor::RangeData range_data_in_local,const std::unique_ptr<const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>) {std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =std::make_shared<TrajectoryState::LocalSlamData>(TrajectoryState::LocalSlamData{time, local_pose,std::move(range_data_in_local)});cartographer::common::MutexLocker lock(&mutex_);trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

OnLocalSlamResult() 这个方法大致就是确定当前的　localSlamData ，然后传入到　TrajectoryState 集合中。

OnLocalSlamResult() 的使用如下所示：( 具体的处理函数还没找到)

// cartographer/cartographer/mapping/trajectory_builder_interface.h#include<functional>using LocalSlamResultCallback =std::function<void(int /* trajectory ID */, common::Time,transform::Rigid3d /* local pose estimate */,sensor::RangeData /* in local frame */,std::unique_ptr<const InsertionResult>)>;// cartographer/cartographer/mapping/map_builder_interface.husing LocalSlamResultCallback =TrajectoryBuilderInterface::LocalSlamResultCallback;// Creates a new trajectory builder and returns its index.virtual int AddTrajectoryBuilder(const std::set<SensorId>& expected_sensor_ids,const proto::TrajectoryBuilderOptions& trajectory_options,LocalSlamResultCallback local_slam_result_callback) = 0;// cartographer/cartographer/mapping/map_builder.hint AddTrajectoryBuilder(const std::set<SensorId> &expected_sensor_ids,const proto::TrajectoryBuilderOptions &trajectory_options,LocalSlamResultCallback local_slam_result_callback) override;// cartographer/cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(const std::set<SensorId>& expected_sensor_ids,const proto::TrajectoryBuilderOptions& trajectory_options,LocalSlamResultCallback local_slam_result_callback) {// ...trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>(sensor_collator_.get(), trajectory_id, expected_sensor_ids,CreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder), trajectory_id,static_cast<PoseGraph3D*>(pose_graph_.get()),local_slam_result_callback)));
}

下边３个方法读取　map_builder_->pose_graph() 进行 rviz 的可视化。

visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {

剩下的方法都是调用　map_build.h中的方法，起到个　桥梁的作用。

cartographer探秘第四章之代码解析（七）--- Cartographer_ros相关推荐

学习笔记-第十四章恶意代码分析实战
第十四章恶意代码的网络特征 1.网络应对措施. 网络行为的基本属性包括IP地址,TCP端口,以及流量内容等,网络和安全设备可以利用它们,来提供网络应对措施.根据IP地址和端口,防火墙和路由器可以限 ...
第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解析步骤同样可以对L3GD20(3轴陀螺仪).HMC58 ...
第四章MPU6000传感器驱动解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解 ...
CodeCombat代码全记录（Python学习利器）--安息之云山峰（第四章）代码1
我们已经有了前三章的基础了,到了第四章,你会发现提示少之又少了.这时,我们大都要靠自己了!!!加油!!如果你没有思路,请仔细看下装备的说明,毕竟到了这关,你需要购买好多其他的新装备,而新装备中存在了新 ...
Javascript第四章匿名函数第七课
匿名函数的作用: 1.用于回调 2.一次性执行函数 Javascript第四章定义函数的形式.回调函数第五课 https://blog.csdn.net/qq_30225725/article/det ...
Paxos算法之旅（四）zookeeper代码解析--转载
ZooKeeper是近期比较热门的一个类Paxos实现.也是一个逐渐得到广泛应用的开源的分布式锁服务实现.被认为是Chubby的开源版,虽然具体实现有很多差异.ZooKeeper概要的介绍可以看官方文 ...
一个基于JRTPLIB的轻量级RTSP客户端(myRTSPClient)——收流篇：（四）example代码解析...
--------------------更新2018.08.20------------------- 添加http_tunnel_example.cpp作为RtspOverHttp示例程序. --- ...
视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...
视觉SLAM十四讲CH8代码解析及课后习题详解
第一版的代码: direct_semidense.cpp #include <iostream> #include <fstream> #include <list> ...
CE教程第四章《代码寻找》
步骤 5: 代码寻找 (密码=888899) 有时一些东西的保存位置在你重新开始游戏时会改变,甚至是在你玩的时候也会变,在这种情况下,你用2步仍然能做出可以用的内存列表. 在这一步会说明怎样用寻找代 ...

cartographer探秘第四章之代码解析（七）--- Cartographer_ros