cartographer 源码解析 (五)

相关链接：

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id,const sensor::TimedPointCloudData& unsynchronized_data) {auto synchronized_data =range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);...}

我们往局部轨迹构建器中添加测距数据，如果是激光数据的话就是激光测距数据。进来之后首先对激光数据进行同步，代码如下：

 auto synchronized_data =range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

那么什么情况下才会对激光数据进行同步呢？主要有一下情况:

好了，我们正式开始，虽然还没有涉及到scan-match，也就是激光匹配。不过我们还是数据预处理的模块。主要就是外推器对激光数据的畸变矫正处理。

激光如何配准呢？

以上有一个细节就是进行激光配准的时候都是以帧为单位的。
比如说一帧激光雷达有32768个点，为什么有这么多个点呢，那是因为激光雷达的开发人员一个点一个点传比较麻烦，发的数据一定要有开头和结尾。进行验证校验。如果都对了，证明数据是传输中没有错误。所以传输数据是比较麻烦的。其实激光没有一帧一帧的说法，只不过是激光转了一圈，人为的发送给你。最合理处理激光的方法是一个点一个点的去处理，而不是一帧一帧的去处理，如果你觉得数据比较少，想cartographer，他会收集到3-4帧数据结合起来去处理。

我们从头到尾的讲一下这个代码来捋一捋。
首先是对激光雷达的数据做一个同步
local_trajectory_builder_2d.cc

const sensor::TimedPointCloudData& unsynchronized_data) {auto synchronized_data =range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);)) {LOG(INFO) << "Range data collator filling buffer.";return nullptr;}

下面是得到当前激光的时间戳

const common::Time& time = synchronized_data.time;

如果不使用imu，我们就尝试自己初始化一个外推器

if (!options_.use_imu_data()) {InitializeExtrapolator(time);}

然后我们再看看这个初始化外推器是如何实现的呢？

void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {if (extrapolator_ != nullptr) {return;}CHECK(!options_.pose_extrapolator_options().use_imu_based());// TODO(gaschler): Consider using InitializeWithImu as 3D does.extrapolator_ = absl::make_unique<PoseExtrapolator>(::cartographer::common::FromSeconds(options_.pose_extrapolator_options().constant_velocity().pose_queue_duration()),options_.pose_extrapolator_options()2015-03-08.constant_velocity().imu_gravity_time_constant());extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}

这个外推器的代码我们后面就进行详细的讲解。

如果我们使用imu数据的话，我们会在添加imu数据的时候添加外推器。

void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";InitializeExtrapolator(imu_data.time);extrapolator_->AddImuData(imu_data);
}

所谓的添加imu数据，实际上就是把imu数据给到外推器里面。
这一步InitializeExtrapolator(imu_data.time);生成imu数据，直接送到这一步的extrapolator_->AddImuData(imu_data);的外推器里面。

岔开话题了哈，我们接着刚才的没有使用imu数据，自己创建外推器的代码开始讲。

if (extrapolator_ == nullptr) {// Until we've initialized the extrapolator with our first IMU message, we// cannot compute the orientation of the rangefinder.LOG(INFO) << "Extrapolator not yet initialized.";return nullptr;}

如果外推器是空指针，那么我们就发出警告，并返回空。

然后做一个检查，看看同步之后的数据是否是空的

CHECK(!synchronized_data.ranges.empty());

然后再检查同步之后的激光的时间戳，是否是小于等于0的

CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);

下面又是一个公式，就是第一个点的绝对时间戳等于相对时间戳＋第一个点的绝对时间戳

 const common::Time time_first_point =time +common::FromSeconds(synchronized_data.ranges.front().point_time.time);

然后再接着往下看：

if (time_first_point < extrapolator_->GetLastPoseTime()) {LOG(INFO) << "Extrapolator is still initializing.";return nullptr;}

从条件语句可以知道，如果满足当前第一个点的时间戳比外推器最后一个（最新）位姿的时间戳要早。我们就不处理了，直接return了。然后接着往下看。我们每个点的发射位置都是变化的，每个点都要优化估计发射的位姿。
local_trajectory_builder_2d.cc

 std::vector<transform::Rigid3f> range_data_poses;range_data_poses.reserve(synchronized_data.ranges.size());bool warned = false;for (const auto& range : synchronized_data.ranges) {common::Time time_point = time + common::FromSeconds(range.point_time.time);if (time_point < extrapolator_->GetLastExtrapolatedTime()) {if (!warned) {LOG(ERROR)<< "Timestamp of individual range data point jumps backwards from "<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;warned = true;}time_point = extrapolator_->GetLastExtrapolatedTime();}range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());}

我们逐行的看

std::vector<transform::Rigid3f> range_data_poses;

首先这一行就是定义一个容器存放每个点被发射的时候的雷达的位姿。

range_data_poses.reserve(synchronized_data.ranges.size());

然后是预留多少空间，开辟多大的存储呢？当然是有多少个点，就预留多少的空间了。多少个点呢？------synchronized_data.ranges.size()
接下来不用说，我们猜也能猜到，肯定是存储每个点的发射位置。我们继续往下看吧。

bool warned = false;for (const auto& range : synchronized_data.ranges) {common::Time time_point = time + common::FromSeconds(range.point_time.time);if (time_point < extrapolator_->GetLastExtrapolatedTime()) {if (!warned) {LOG(ERROR)<< "Timestamp of individual range data point jumps backwards from "<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;warned = true;}time_point = extrapolator_->GetLastExtrapolatedTime();}range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());}

继续逐行解析，

bool warned = false;

首先是设置一个布尔值类型的判断，用来负责断定是否要发出警告。

接着是一个for循环，从循环的执行条件可以看出是遍历所有的点。

 for (const auto& range : synchronized_data.ranges) {......
}

再讲下一行代码

    common::Time time_point = time + common::FromSeconds(range.point_time.time);

这个time_point是什么呢？他是当前一帧点云的时间戳time + 当前点的相对时间戳，结果就等于当前点的绝对时间戳。

再讲一下行代码的时候，我们要清楚一个概念，首先是前面讲了一个extrapolator_->GetLastPoseTime()，这一段有提到了一个extrapolator_->GetLastExtrapolatedTime()。虽然都是获取时间，但是他们的概念是不一样的，首先是第一个是最近的位姿的时间，然后这个的是上一次外推的时间。

  if (time_point < extrapolator_->GetLastExtrapolatedTime()) {if (!warned) {LOG(ERROR)<< "Timestamp of individual range data point jumps backwards from "<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;warned = true;}time_point = extrapolator_->GetLastExtrapolatedTime();}

跟上一次外推时间作对比，发现当前点的时间比上一次外推时间小，说明比外推时间早，这不太对，说明这一堆点排序不对，没有排好顺序。并发出警告。你上一次的点的时间戳获取到了，得到一个外推时间，下一个点比上一个点的时间还要早，这样你就不是预测发射点的位姿，而是回退验证之前点的位姿了。这说明这次操作是异常的，那么我们怎么做呢？直接把time_point获取上一次外推器的时间。

time_point = extrapolator_->GetLastExtrapolatedTime();

这时间就算不增长，也不能外推一个比之前外推时间要早的时间戳。干脆还有上一次的外推时间戳。

紧接着，用外推器外推一个pose，然后添加到位姿容器中。

range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());

我们先不看外推器外推的原理，我们先把这个逻辑走完。
我们接着往下看

 if (num_accumulated_ == 0) {// 'accumulated_range_data_.origin' is uninitialized until the last// accumulation.accumulated_range_data_ = sensor::RangeData{{}, {}, {}};}

这个num_accumulated_是什么呢？我们cartographer支持好几帧连在一起去处理的。每一帧激光来的时候num_accumulated_都会增加一下，同时如果是第一帧激光来的话。accumulated_range_data_会初始化一下，初始化成RangeData这个类型。然后等到指定的几帧激光拼成一个大激光之后。num_accumulated_就又会变成0。
接下来重点来了，是激光矫正的核心，那么怎么突出这个重点呢？我要开始变色了，变色之后，你就知道这个里特别重点了。
变色！
变红色！
变绿色！
变黄色！
变紫色！
变橙色！
变黑色！
变粉色！
变蓝色！
好了，相信你已经意识到问题的严重性，不对，说错了，应该是下面代码的重要性了，那么我要开始我的表演了！

啥也不说了，翠花。上酸菜。

 // Drop any returns below the minimum range and convert returns beyond the// maximum range into misses.for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {const sensor::TimedRangefinderPoint& hit =synchronized_data.ranges[i].point_time;const Eigen::Vector3f origin_in_local =range_data_poses[i] *synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);sensor::RangefinderPoint hit_in_local =range_data_poses[i] * sensor::ToRangefinderPoint(hit);const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;const float range = delta.norm();if (range >= options_.min_range()) {if (range <= options_.max_range()) {accumulated_range_data_.returns.push_back(hit_in_local);} else {hit_in_local.position =origin_in_local +options_.missing_data_ray_length() / range * delta;accumulated_range_data_.misses.push_back(hit_in_local);}}}

代码上完了，是不是有点上头，我们一点点的说，先从for循环开始吧。我们回溯一下，弄明白这到底是怎么一回事。

 for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {......}

首先有个synchronized_data，这同步的数据是怎么从ros的LaserScan变成的同步数据的呢？
首先我们要看一下synchronized_data是什么数据类型，先找到他的定义。
local_trajectory_builder_2d.cc

auto synchronized_data =range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);)) {LOG(INFO) << "Range data collator filling buffer.";return nullptr;}

这里定义的是auto，那么紧接着我们看看函数返回的类型就知道它是定义的什么类型了。
range_data_collator.cc

sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(const std::string& sensor_id,sensor::TimedPointCloudData timed_point_cloud_data) {......}

找到函数定义的位置，发现synchronized_data的数据类型是sensor::TimedPointCloudOriginData。
然后再看看TimedPointCloudOriginData。

struct TimedPointCloudOriginData {struct RangeMeasurement {TimedRangefinderPoint point_time;float intensity;size_t origin_index;};common::Time time;std::vector<Eigen::Vector3f> origins;std::vector<RangeMeasurement> ranges;
};

其中包含了一个RangeMeasurement的结构体，然后是时间time，origins是雷达位置的可变数组，ranges是相关的点的可变数组。
timed_point_cloud_data.h

  struct RangeMeasurement {TimedRangefinderPoint point_time;float intensity;size_t origin_index;};

首先是TimedRangefinderPoint这个数据类型。具体是什么呢？我们看看

struct TimedRangefinderPoint {Eigen::Vector3f position;float time;
};

我们一目了然，就是带有时间戳的三维点。
这样我们再回溯一下：RangeMeasurement其实是一个带有时间戳的三维点point_time以及信号强度intensity，再加上激光雷达的发射激光的位置的索引origin_index。
然后我们再看const sensor::TimedPointCloudData& unsynchronized_data这个没有同步的数据是什么格式的。可以知道数据类型是TimedPointCloudData。具体里面都有什么呢？
timed_point_cloud_data.h

struct TimedPointCloudData {common::Time time;Eigen::Vector3f origin;TimedPointCloud ranges;// 'intensities' has to be same size as 'ranges', or empty.std::vector<float> intensities;
};

然后我们就可以知道，上面的数据类型是存放一个激光源发出的一帧激光，而之前的TimedPointCloudOriginData存放的是几帧激光合在一起形成的激光。这就是AddRangeData接收到的unsynchronized_data数据。那这个AddRangeData是哪里调用的呢？
global_trajectory_builder.cc

 void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) override {CHECK(local_trajectory_builder_)<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);......}

这个AddSensorData是怎么传到这里面的呢？我们之前也分析过。是警告过collatored_trajectory_builder里面，其中包括利用collator进行多个不同传感器之间的数据进行同步。然后逐个分发。这个中间过程是不会有数据进行转变的。
离这个最近的转变是什么，那不就是cartographer_ros里面。把ros格式的msg转化成cartographer需要的点云格式，说到这里你肯定是忘了，所有我们回忆一下吧。
sensor_bridge.cc

void SensorBridge::HandleLaserScanMessage(const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {carto::sensor::PointCloudWithIntensities point_cloud;carto::common::Time time;std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

首先，它接收到了一个ros格式的msg，然后它将msg转化成了PointCloudWithIntensities格式消息，然后我们再去看看PointCloudWithIntensities具体的数据结构是什么类型的。

struct PointCloudWithIntensities {TimedPointCloud points;std::vector<float> intensities;
};

进一步的再看看TimedPointCloud数据类型。

using TimedPointCloud = std::vector<TimedRangefinderPoint>;

发现它是一个TimedRangefinderPoint类型的数组，但具体我们看看这个数组的数据类型是什么呢？

struct TimedRangefinderPoint {Eigen::Vector3f position;float time;
};

我们发现是带时间戳的三维点的数据结构。
也就是说TimedPointCloud，内容如其名字，也就存储一系列带时间戳的点的数组。PointCloudWithIntensities是在此基础上的再加上点云的强度信息。格式转换成功了，我们再将数据传给HandleLaserScan

  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);

sensor_bridge.cc

void SensorBridge::HandleLaserScan(const std::string& sensor_id, const carto::common::Time time,const std::string& frame_id,const carto::sensor::PointCloudWithIntensities& points) {if (points.points.empty()) {return;}CHECK_LE(points.points.back().time, 0.f);// TODO(gaschler): Use per-point time instead of subdivisions.for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {const size_t start_index =points.points.size() * i / num_subdivisions_per_laser_scan_;const size_t end_index =points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;carto::sensor::TimedPointCloud subdivision(points.points.begin() + start_index, points.points.begin() + end_index);if (start_index == end_index) {continue;}const double time_to_subdivision_end = subdivision.back().time;// `subdivision_time` is the end of the measurement so sensor::Collator will// send all other sensor data first.const carto::common::Time subdivision_time =time + carto::common::FromSeconds(time_to_subdivision_end);auto it = sensor_to_previous_subdivision_time_.find(sensor_id);if (it != sensor_to_previous_subdivision_time_.end() &&it->second >= subdivision_time) {LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "<< sensor_id << " because previous subdivision time "<< it->second << " is not before current subdivision time "<< subdivision_time;continue;}sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;for (auto& point : subdivision) {point.time -= time_to_subdivision_end;}CHECK_EQ(subdivision.back().time, 0.f);HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);}
}

由于之前讲过，这里就不细讲了，主要是将points转化成一个个的subdivision。

我们注意到了最下面有一个函数HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);。在这里我们进一步的来看到底是干什么的。
sensor_bridge.cc

void SensorBridge::HandleRangefinder(const std::string& sensor_id, const carto::common::Time time,const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {if (!ranges.empty()) {CHECK_LE(ranges.back().time, 0.f);}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>())});}
}

下面我们继续逐行分析。

const auto sensor_to_tracking =tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

首先我们经过tf_bridge的查找器进行查找，tf_bridge是用来进行坐标转换的，记录坐标系之间的转换关系的工具，查找的是sensor_to_tracking。
我又要开始变色了。

to is in !
to is at !
举个例子来说明情况
首先是一个激光点P

它再激光坐标系下的是这样的

而激光坐标系在地图坐标系下是这样的。

怎么表示点p在map坐标系下的位置呢？
其实就是laser to map, laser is map laser at map的表示。
也就是Tlasermap∗PT_{laser}^{map}*PTlasermap∗P
其实就是laser 坐标系在 map坐标系下的位置。

而这个sensor_to_tracking其实就是sensor in tracking, 这个sensor在这里是雷达，其实就是激光雷达的外参。
接下来我们做什么呢？

void SensorBridge::HandleRangefinder(const std::string& sensor_id, const carto::common::Time time,const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {......}

我们经过一系列的转换，转换成TimedPointCloud，其实就是带有时间戳的3D位置点的数组。
而转化之后的格式，TimedPointCloudData是多了个origin，也即是雷达发射该点的时候的位置。

struct TimedPointCloudData {common::Time time;Eigen::Vector3f origin;TimedPointCloud ranges;// 'intensities' has to be same size as 'ranges', or empty.std::vector<float> intensities;
};

其中红色的点为激光雷达的点，tracking_frame 坐标系是再两轮的中心位置。前头是激光雷达。
蓝色的点P在激光雷达的坐标转换到tracking_frame的坐标转换关系是sensor_to_tracking。通过sensor_to_tracking我们可以将激光雷达坐标系转换到tracking_frame坐标系中。
我们可以在代码中看到

 sensor_id, carto::sensor::TimedPointCloudData{time, sensor_to_tracking->translation().cast<float>(),carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())}

time作为当前一帧激光的绝对时间戳被传送进去，然后sensor_to_tracking->translation().cast<float>()是origin,也就是激光雷达发射的原点，sensor_to_tracking，也就是激光雷达的坐标系，在track_frame里的位置。

carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())

将这一堆点ranges用 sensor_to_tracking来进行转换。也就是将这些点在雷达中的坐标转换为在tracking_frame中的坐标。
比如我们把点P转换到了tracking_frame中，那么为什么我们还要记录它的发射原点。为了方便到时候计算一个某一束激光的长度，看其是否太长，或者太短。可能就有人要问了，为什么要把所有的点都记录到tracking_frame中呢？就是因为cartographer就非常的工程，支持多激光建图的。比如我们有多个激光雷达。最后都会集中到tracking_frame中。综合在一起需要一个统一的坐标系，所以跟踪什么，我们都转换到什么上去。
那我们回到cartographer中，继续讲激光的那一段代码中。
之前我们讲畸变矫正，讲到了for循环。

for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {const sensor::TimedRangefinderPoint& hit =synchronized_data.ranges[i].point_time;const Eigen::Vector3f origin_in_local =range_data_poses[i] *synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);sensor::RangefinderPoint hit_in_local =range_data_poses[i] * sensor::ToRangefinderPoint(hit);const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;const float range = delta.norm();if (range >= options_.min_range()) {if (range <= options_.max_range()) {accumulated_range_data_.returns.push_back(hit_in_local);} else {hit_in_local.position =origin_in_local +options_.missing_data_ray_length() / range * delta;accumulated_range_data_.misses.push_back(hit_in_local);}}}

我们看到了hit 经过了同步之后所有点中的一帧点云synchronized_data.ranges[i]，

struct TimedPointCloudOriginData {struct RangeMeasurement {TimedRangefinderPoint point_time;float intensity;size_t origin_index;};common::Time time;std::vector<Eigen::Vector3f> origins;std::vector<RangeMeasurement> ranges;
};

其实就是TimedPointCloud ranges;，也就是using TimedPointCloud = std::vector<TimedRangefinderPoint>;带有时间戳的三维点的数组。

首先把ranges的point_time拿出来，也就是带着时间戳的点的坐标拿到。
origin我们先跳过，我们先看看hit主要用来干什么。

 sensor::RangefinderPoint hit_in_local =range_data_poses[i] * sensor::ToRangefinderPoint(hit);

然后我们看看ToRangefinderPoint做了什么？这个hit就是一帧激光里的相对坐标
rangefinder_point.h

inline RangefinderPoint ToRangefinderPoint(const TimedRangefinderPoint& timed_rangefinder_point) {return {timed_rangefinder_point.position};
}

这个很简单，直接返回它的坐标。
坐标拿出来之后，又乘以range_data_poses[i]，那么这个range_data_poses[i]又是什么呢？
我们之前也讲过，我们给每一个点在每一个点的外推时间戳上都外推了一个pose
把每一个点的外推pose都乘以点的坐标。
我们希望做一下畸变矫正，就需要给点一个相对的坐标原点。而不是一开始上电时刻的原点。这样才会尽量的去矫正这个畸变。主要作用畸变矫正，并把坐标都映射到了local坐标系下。

 const Eigen::Vector3f origin_in_local =range_data_poses[i] *synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

这是激光雷达的一帧点云中的点的发射的位置。在local坐标系下的表示。公式就是local 坐标系 = 点的外推位姿 X 点的坐标
我们记录origin原点的坐标作用是什么呢？主要就是计算发射原点到被击中的这段距离
下来是什么呢？
hit是在雷达坐标系下的被击中的坐标，hit_in_local是local坐标系下被击中的坐标。然后是origin是雷达坐标系下的雷达发射该点坐标，origin_in_local是激光雷达发射该点的位置。

 const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;

所以hit_in_local.position - origin_in_local.norm()就是测距长度。

  if (range >= options_.min_range()) {...}

如果长度太小就不往里面添加了

虽然长，但有一句话我们需要记住，将来我们跟别人拉开差距的地方，其实就是细节。

 if (range <= options_.max_range()) {accumulated_range_data_.returns.push_back(hit_in_local);} else {hit_in_local.position =origin_in_local +options_.missing_data_ray_length() / range * delta;accumulated_range_data_.misses.push_back(hit_in_local);}

如果在有效范围内，就添加上，如果超过了最远测距的距离。那么也不会舍弃。
就是在此基础上，雷达发射原点的基础上，在这个方向上设定长度 options_.missing_data_ray_length() 。比如激光雷达照射的太远，我们只用这一小块。比如说你在一个空旷的地方，但是照射的点距离都贼长，那么我们就认识机器人附近是没有东西的。

  if (num_accumulated_ >= options_.num_accumulated_range_data()) {......}

其中options_.num_accumulated_range_data()是我们设置的收集雷达的帧数，比如我们设置为2或者3，就是等到收集了2帧或者3帧数据了。

const common::Time current_sensor_time = synchronized_data.time;absl::optional<common::Duration> sensor_duration;if (last_sensor_time_.has_value()) {sensor_duration = current_sensor_time - last_sensor_time_.value();}

首先计算一下，上一次传感器传到这里的时间，然后获取当前的时间，二者相减获得差值。后面会有用到。

 last_sensor_time_ = current_sensor_time;num_accumulated_ = 0;

更新last_sensor_time_，同时统计收集雷达帧数的变量为0，重新开始收集。

      const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(extrapolator_->EstimateGravityOrientation(time));

这个先跳过去不讲，后面我们再说。

 accumulated_range_data_.origin = range_data_poses.back().translation();

我们用多个激光雷达建图的时候，会有多个origin。上面代码表示，用最后的外推位姿的发射原点作为整个一帧大激光，也即是合并之后的激光，作为它的原点。

return AddAccumulatedRangeData(time,TransformToGravityAlignedFrameAndFilter(gravity_alignment.cast<float>() * range_data_poses.back().inverse(),accumulated_range_data_),gravity_alignment, sensor_duration);

最后是拿过去做激光匹配。

一帧激光里的点也会发生畸变，其原因就是发射点的位置坐标一直在变，要想纠正激光雷达的点投射的正确几何形状，就要求估计激光雷达发射点的位置尽可能的正确。那么这个发射点的坐标系是什么呢？是local，local在前几章节讲过，就是前端。在cartographer里面，就是前端的意思，就是激光里程计。

我们是对传感器数据做处理，就是处理激光雷达的畸变矫正。所以我们还要恢复成一个激光雷达的坐标系里去。针对于雷达坐标系去做矫正，具体怎么做呢？我们来看看TransformToGravityAlignedFrameAndFilter这个函数吧。
local_trajectory_builder_2d.cc

LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id,const sensor::TimedPointCloudData& unsynchronized_data) {......return AddAccumulatedRangeData(time,TransformToGravityAlignedFrameAndFilter(gravity_alignment.cast<float>() * range_data_poses.back().inverse(),accumulated_range_data_),gravity_alignment, sensor_duration);}return nullptr;}

TransformToGravityAlignedFrameAndFilter，这个函数具体做了什么呢？gravity_alignment是重力对齐，因为我们不用3D建图，所以先不考虑。然后再看range_data_poses就是每一个激光点的发射原点中，而这个 range_data_poses.back().inverse()也都是相对于local坐标系下的。然后我们看这个函数具体怎么实现的，具体做了什么。
local_trajectory_builder_2d.cc

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(const transform::Rigid3f& transform_to_gravity_aligned_frame,const sensor::RangeData& range_data) const {const sensor::RangeData cropped =sensor::CropRangeData(sensor::TransformRangeData(range_data, transform_to_gravity_aligned_frame),options_.min_z(), options_.max_z());return sensor::RangeData{cropped.origin,sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}

CropRangeData 比如说做重力矫正之后的，只取一定范围内的激光。然后我们要知道transform_to_gravity_aligned_frame是最后一个激光点的位置坐标（相对于local坐标系下的）。然后呢，之前的哪些点都变成以最后一个点的坐标为原点，进行转化。就相当于之前的点的每个发射坐标都是相对于local坐标系下的，然后一转化，都变成基于最后那个点的坐标系为原点了，就不依靠local坐标系了。最后还有一个就是体素滤波VoxelFilter，主要是做什么的呢？就是降采样，如果点太密了，那就降采样。以上，就是TransformToGravityAlignedFrameAndFilter干的事情。

最后总结一下，就是做激光矫正的时候，都把每个点的位置坐标转化到local坐标系下，转换完了之后呢，再转化到统一的同一个激光雷达坐标系下，变成一帧比较独立的激光。

cartographer 源码解析 (五)相关推荐

(Nacos源码解析五)Nacos服务事件变动源码解析
Nacos源码解析系列目录 Nacos 源码编译运行 (Nacos源码解析一)Nacos 注册实例源码解析 (Nacos源码解析二)Nacos 服务发现源码解析 (Nacos源码解析三)Nacos 心 ...
【cartographer源码解析--外推器】
cartographer源码解析–外推器文章目录 cartographer源码解析--外推器前言一.cartographer中的PoseExtrapolator类二.接受数据,并分析处理逻辑 ...
Tomcat源码解析五：Tomcat请求处理过程
前面已经分析完了Tomcat的启动和关闭过程,本篇就来接着分析一下Tomcat中请求的处理过程. 在开始本文之前,咋们首先来看看一个Http请求处理的过程,一般情况下是浏览器发送http请求-> ...
Spring源码解析(五)-Bean的实例化流程(上)
在前面已经完成了对需要实例化bean的收集并封装成BeanDefinition,并且将BeanPostProcess等组件进行了提前实例化.接下来就到了容器启动的最后一步,也是最复杂的一步-实例化be ...
spring源码解析五
2019独角兽企业重金招聘Python工程师标准>>> 1.创建用于承载属性的BeanDefinition 这是一个接口,在spring中存在三种实现:RootBeanDefinit ...
cartographer源码解析（二）node_main.cc文件详解
描述 cartographer学习笔记(二) 这一篇主要分析node_main.cc的代码主函数main() 主函数 int main(int argc, char** argv) {google: ...
Cartographer源码解析——位姿推测器PoseExtrapolator
这一章主要了解外推器,上一章讲到激光点云畸变的矫正,其中最关键的是推测估计出一帧点云中每一个激光点所对应的机器人的位姿.这里就用到了外推器去推测每个发射点的位置. 具体代码调用如下: // Step: ...
mybatis源码解析(五) --- typehandler注册和处理的查询结果对象的类型转换
上一次分析了mapper接口动态代理调用的处理过程,在查询结果后,然后转换的java所对应的类型,这篇文章将对这个类型转换处理详细介绍,在Configuration中TypeHandlerRegist ...
QT源码解析(一) QT创建窗口程序、消息循环和WinMain函数
版权声明请尊重原创作品.转载请保持文章完整性,并以超链接形式注明原始作者"tingsking18"和主站点地址,方便其他朋友提问和指正. QT源码解析(一) QT创建窗口程序.消 ...

cartographer 源码解析 (五)

相关链接：

cartographer 源码解析(一)

cartographer 源码解析(二)

cartographer 源码解析(三)

cartographer 源码解析(四)

cartographer 源码解析(五)

cartographer 源码解析(六)

cartographer 源码解析 (五)相关推荐

最新文章

热门文章