cartographer 源码解析 (三)

相关链接：

cartographer 源码解析(一)

cartographer 源码解析(二)

cartographer 源码解析(三)

cartographer 源码解析(四)

cartographer 源码解析(五)

cartographer 源码解析(六)

今天讲一下数据格式转换以及数据的同步的具体的过程，首先以激光数据为例子，看一下在node.cc里面的激光的回调函数。
node.cc

void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
   
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
   
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

逐行讲解一下：

absl::MutexLock lock(&mutex_);

每一种传感器都有一种回调函数。为防止这么些传感器的回调函数共同访问一些东西。为了线程安全，加了锁。加了锁之后，因为很快，所以不会影响数据的存储。

  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
   
    return;
  }

由于有的传感器频率特别高，而我们不需要那么高频率的数据，没必要每一帧都处理，这时候需要采样器来选择性的采集数据。

  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);

最好是把ros格式的消息送到数据转换器中，去转换成cartographer可以识别的类型。
让map_builder_bridge里面的数据格式转化器sensor_bridge 处理ros格式的消息。然后我们进一步去看HandleLaserScanMessage函数具体如何实现的。
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);
}

下面进行逐行解析

 carto::sensor::PointCloudWithIntensities point_cloud;

首先是在另一个项目cartographer的命名空间种定义了一个传感器的PointCloudWithIntensities类型的point_cloud。我们再去看看carto命名空间的PointCloudWithIntensities数据类型是怎么定义的，不过从字面意思上看是带有强度信息的点云。
point_cloud.h

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

其中TimedPointCloud points;中的TimedPointCloud可以看到被定义为using TimedPointCloud = std::vector<TimedRangefinderPoint>; 另一个是以及每一个点的强度信息的intensities。而TimedPointCloud是一个带有时间戳的TimedRangefinderPoint类型的容器。那么我们再看看TimedRangefinderPoint的数据结构是这么定义的吧。定义如下：
rangefinder_point.h

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

他是由定义的三维的向量position，以及浮点类型的time。

现在回溯一下，我们知道了PointCloudWithIntensities是什么类型的数据格式了，那么我们怎么去把ros消息类型的数据转化成PointCloudWithIntensities数据类型呢？我们就要看一下ToPointCloudWithIntensities这个函数了。
msg_conversion.cc

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
   
  return LaserScanToPointCloudWithIntensities(msg);
}

他实际上是调用了LaserScanToPointCloudWithIntensities这个函数。我们再来看看这个函数的具体实现。
msg_conversion.cc

template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
   
  CHECK_GE(msg.range_min, 0.f);
  CHECK_GE(msg.range_max, msg.range_min);
  if (msg.angle_increment > 0.f) {
   
    CHECK_GT(msg.angle_max, msg.angle_min);
  } else {
   
    CHECK_GT(msg.angle_min, msg.angle_max);
  }
  PointCloudWithIntensities point_cloud;
  float angle = msg.angle_min;
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
   
    const auto& echoes = msg.ranges[i];
    if (HasEcho(echoes)) {
   
      const float first_echo = GetFirstEcho(echoes);
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
   
        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        const cartographer::sensor::TimedRangefinderPoint point{
   
            rotation * (first_echo * Eigen::Vector3f::UnitX()),
            i * msg.time_increment};
        point_cloud.points.push_back(point);
        if (msg.intensities.size() > 0) {
   
          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
          const auto& echo_intensities = msg.intensities[i];
          CHECK(HasEcho(echo_intensities));
          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
        } else {
   
          point_cloud.intensities.push_back(0.f);
        }
      }
    }
    angle += msg.angle_increment;
  }
  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  if (!point_cloud.points.empty()) {
   
    const double duration = point_cloud.points.back().time;
    timestamp += cartographer::common::FromSeconds(duration);
    for (auto& point : point_cloud.points) {
   
      point.time -= duration;
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

逐行解析

CHECK_GE(msg.range_min, 0.f);
CHECK_GE(msg.range_max, msg.range_min);

这两个检查的意思是 msg.range_min 一定要是大于0的
而 msg.range_max 一定是要大于 msg.range_min 的。

  if (msg.angle_increment > 0.f) {
   
    CHECK_GT(msg.angle_max, msg.angle_min);
  } else {
   
    CHECK_GT(msg.angle_min, msg.angle_max);
  }

如果点与点之间的角度增量是大于0的，那么msg.angle_max一定要大于msg.angle_min。
小于零，那么就是那么msg.angle_min一定要大于msg.angle_max。

其中，msg.angle_min指的是第一个点的角度，msg。angle_max是最后一个点的角度。

PointCloudWithIntensities point_cloud;

接着是定义了这个变量，一直忘这个变量里面填充东西。最后是把point_cloud带着时间戳给返回出去。

 float angle = msg.angle_min;

这是获得第一个点的角度。

 for (size_t i = 0; i < msg.ranges.size(); ++i) {
   
   ...
    angle += msg.angle_increment;
  }

这是遍历所有的点，操作完成之后，我们要操作下一个点，所以angle要增加一下。

 const auto& echoes = msg.ranges[i];

获得点的回波。

 if (HasEcho(echoes)) {
   
    ...
    }

确认是否有回波

  const float first_echo = GetFirstEcho(echoes);

一般默认的是获得第一个点的回波。

 if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
   
       ....
      }

确认回波是在我们可接受的范围内

 const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());    

定义一个旋转向量rotation，以Z轴为旋转轴，angle为旋转角度得到的旋转向量。

first_echo * Eigen::Vector3f::UnitX()

首先是一个测距值first_echo乘以一个X轴方向的单位长度的向量。得到一个沿着x轴方向长度为first_echo的向量，那么我们实际的向量可能不是在x轴上，需要一个角度去旋转到该点所在的向量方向上，首先可以确定的是旋转轴是z轴。只要乘以旋转向量就能获得旋转的x,y,z点的坐标。

 rotation * (first_echo * Eigen::Vector3f::UnitX())

const cartographer::sensor::TimedRangefinderPoint point{
   
            rotation * (first_echo * Eigen::Vector3f::