cartographer 源码解析 (五)

墨蓝 2022-09-07 01:31 168阅读 0赞

### 相关链接： ###

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

本章节主要讲的内容是激光畸变的来源和矫正，即对外推器的数据进行激光矫正。主要还是数据预处理之激光匹配的问题。但是这些预处理的东西都非常关键，这些数据的外推器，对激光进行畸变矫正。  
其实激光矫正的原理是很简单，只要你想通了，就很简单。然后就是激光如何进行矫正。

怎么进行配准呢，首先找到一段激光A，一段激光B， 然后找到他们的R和T然后放到一块去。重复的地方放到一起，不重复的地方就是地图的扩展，激光的地图匹配都是以帧为单位。

首先说一下畸变的来源，由于激光雷达是运动的，但是我们收到的点都假设起始点是跟第一个点发出的时候的雷达的位置是一样的，所有就出现了畸变。  
绿色的是墙，圆圈是雷达，自上往下分别是雷达的运动，紫色的是雷达发出的激光的位置。  
从图中，我们可以看到是未矫正的激光最后呈现出的样子  
![在这里插入图片描述][3c5916d71fd94c82aacd8bc44d2d3541.png_pic_center]

最终呈现出的效果如下图所示

![在这里插入图片描述][74a1495d64c7488a9b2b5cb092203868.png_pic_center]  
这是矫正之前的图，可以发现激光雷达呈现出的墙已经斜了。  
而我们根据雷达起始点的位置。对激光进行矫正，获得效果会是什么样子呢？  
先看看矫正前的激光点连在一起形成的墙吧。黄色的就是墙。  
![在这里插入图片描述][0261584719d34cd5b1f0af583db1ed44.png_pic_center]  
而矫正之后的蓝色部分是我们的墙  
![在这里插入图片描述][9898ca621cdb483699094a2bc967089e.png_pic_center]  
从这里可以意识到矫正的重要性。

直接开始了。  
上回书说道。2D激光的局部轨迹构建器添加测距数据。什么？忘了啊，那直接上代码吧。  
**local\_trajectory\_builder\_2d.cc**

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);

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

*  **情况1**： 你有两个激光雷达，两个激光雷达的名字重复了，比如雷达1 叫`scan1` 雷达2也叫`scan1`.但是两帧激光雷达的数据有重叠的部分。如下图所示  
    ![在这里插入图片描述][1e091624a93243b49ab4451340492af4.png_pic_center]  
    其中橙色的是雷达1 的一帧激光数据，红色是雷达2的一帧激光数据，蓝色是两帧激光雷达数据重复的部分。  
    看上面的代码我们知道，我们有一个`range_data_collator_`, 之前我们有一个`collator_trajectory_builder` 里面有一个`collator`, 我们的局部轨迹构建器中也有个`collator`。他们共同作用就是收集加同步的作用。这个`range_data_collator_`收集齐了所有的测距数据，比如`scan1、scan2、scan3`然后再对这些数据进行同步，我们会在这个基础上，找到时间重复的，把重复的那一部分去掉。只保留不重复的部分。
 *  **情况2**: 还有一种情况就是有多个雷达，他们分别是`scan1、scan2、scan3`。当这些scan具有相同部分的时候，不是去掉时间同步的部分，而是将重复部分的点以时间的顺序重新排序，排序后生成一个新的scan，我们暂时起名叫做`scan_merge`吧。  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center]  
    其中红色是`scan1`、紫色是`scan2`、最下面是`scan1和scan2`结合后的一整帧点云数据。中间重复部分以时间顺序排列好，点云会特别的密。

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

激光如何配准呢？

*  首先要有一帧激光A
 *  然后要有一帧激光B
 *  接着找到激光A和激光B相同的部分，把相同的部分拼接到一起。首先要算出`R`和`T`，不重复的部分相当于激光的扩展了。

以上有一个细节就是进行激光配准的时候都是以帧为单位的。  
比如说一帧激光雷达有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  
![在这里插入图片描述][15b0307df22a44aba61aa48711e55116.png_pic_center]

它再激光坐标系下的是这样的  
![在这里插入图片描述][6ae9f8e6e63b4209a3309d96ac96452b.png_pic_center]  
而激光坐标系在地图坐标系下是这样的。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 1]  
怎么表示点p在map坐标系下的位置呢？  
其实就是`laser to map, laser is map laser at map`的表示。  
也就是 T l a s e r m a p ∗ P T\_\{laser\}^\{map\}\*P Tlasermap∗P  
其实就是laser 坐标系在 map坐标系下的位置。

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

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

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 2]  
其中红色的点为激光雷达的点，`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]`，

其实就是`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 点的坐标`  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 3]我们记录`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]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[3c5916d71fd94c82aacd8bc44d2d3541.png_pic_center]: /images/20220829/d304bbb4d12446a5984135108de4803c.png
[74a1495d64c7488a9b2b5cb092203868.png_pic_center]: /images/20220829/def0907a510947939d5c11d9ad449513.png
[0261584719d34cd5b1f0af583db1ed44.png_pic_center]: /images/20220829/1cbbcac8b5de47599f99f4dc7790eb1f.png
[9898ca621cdb483699094a2bc967089e.png_pic_center]: /images/20220829/baf43fc220f4407dbe4b7f378e875a97.png
[1e091624a93243b49ab4451340492af4.png_pic_center]: /images/20220829/e2ed4cebf6be47cab6db096560512c13.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center]: /images/20220829/459179850e64413889474707d5b4ed1e.png
[15b0307df22a44aba61aa48711e55116.png_pic_center]: /images/20220829/561642ac6b434d27bc3d2de5c2ce52c2.png
[6ae9f8e6e63b4209a3309d96ac96452b.png_pic_center]: /images/20220829/936d2ec6a2ca4335ad529efb6d47218c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20220829/f63fce046c554eee95ff0a7d67f3c7cc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 2]: /images/20220829/c2693b009d2640c2a1c360966662217d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20220829/2bc05e084950406f887f94efee811bbd.png