[cartographer疑问系列] 8. lidar数据类型解析 + 多lidar数据时间对齐操作+运动畸变去除

一、lidar数据类型解析

1. bag包发布的lidar数据

数据的类型是：ros中激光雷达的消息类型 sensor_msgs/LaserScan Message

header.timestamp :  该帧激光的time; 是获取第一束激光的时刻。但是cartographer中，设置为获取最后一束激光的时刻。

然后转为所需的数据结构：
 

{

   time; // 该帧激光数据的时间；

   PointCloudWithIntensities;//点云数据

}

// 以下是类型分解

// 位于 point_cloud.h
using TimedPointCloud = std::vector<TimedRangefinderPoint>;
struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};

// 位于 rangefinder_point.h
struct TimedRangefinderPoint {
  Eigen::Vector3f position; // 每个激光点的测距值；即：sensor_msgs/LaserScan 中的 ranges 信息
  float time; // 相对于origin的时间差；从origin到该束激光的时间，为负值；由sensor_msgs/LaserScan 中的 time_increament 以及该束激光的id 计算得到
};

// 位于cartographer_ros msg_conversion.cc 
// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
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);
}

 此外，还将点云转移到odom系表示。odom系是cartographer的系统坐标系，其他所有的传感器数据都要转到odom系。

TimedPointCloudData

/*
// Stores 3D position of a point with its relative measurement time.
// See point_cloud.h for more details.
struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};

struct TimedPointCloudData {
  common::Time time;//最后一束激光的获取时间
  Eigen::Vector3f origin;//最后一束激光的激光器位置作为该帧的原点，其坐标在lidar系应该是(0,0,0)，但是现在传感器数据都表示在了odom系，因此这个值是lidar系和odom系的外参
  TimedPointCloud ranges;// using TimedPointCloud = std::vector<TimedRangefinderPoint>;
};

// 不同激光器件的数据集合
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;
    size_t origin_index;
  };
  common::Time time;
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges;
};
*/

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);
  }
  // 获得 lidar_to_odom 的 外参
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

  // 将 点云转移到 odom 系表示
  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>())});
  }
}

二、 多lidar数据时间对齐操作

该操作位于local_trajectory_builder_2d.cc 

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
        const std::string& sensor_id,
        const sensor::TimedPointCloudData& unsynchronized_data)
{
    //进行多个传感器的数据同步,只有一个传感器的时候,可以直接忽略(这里的传感器指激光雷达，即有几个lidar dzh)
       // 返回类型是 TimedPointCloudOriginData  表示多个激光器件的对齐数据集合
       // 不过底层的数据类型都是TimedRangefinderPoint
       // unsynchronized_data 是表示在 当前帧所在时刻的odom系
    auto synchronized_data =
            range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

......

}

  关于这个对齐函数就不展开说了，简单总结如下：

  有4个激光器，但是其获取的4帧激光肯定不是同一时刻的，我们对齐的目的就是，将其联合成一帧激光数据。前提是这4个激光器的时间戳是同步的。

    比如4帧激光分别获取的时间段如下：

          [10, 100]   ,  [12, 100] , [15, 102], [5, 90]

  统一成一帧激光，就需要定义这帧激光的最后一束激光。

  1）确定合成帧的时间段 [10, 102]

  2) 最后一束激光为第三帧的最后一束激光，则合成帧的 time = 这束激光的time

  3）将四帧激光中，在[10, 102]范围内的激光加入进来，注意要改变每束激光的deltatime。因为deltatime是相对于该帧激光的第一束激光的时间差，现在合成帧中，其第一束激光改变了，因此时间得重新计算。

  4）origin，每个激光器的位置不同，和 odom 的外参不同，因此origin也不同，因此保存四个激光的origin。

       至此，合成完毕。

           

三、运动畸变去除

1. 先获得每个激光点在 local系 下的坐标

    for (size_t i = 0; i < synchronized_data.ranges.size(); ++i)
    {
        const sensor::TimedRangefinderPoint& hit =
                synchronized_data.ranges[i].point_time;

        // 什么是运动畸变？
        // 假设 hit[i] 在 t时刻获取，hit[i] 表示在 t 时刻 的 odom系
        // 这一帧激光数据一共100束，历时100ms.该帧激光的 time 为最后一束激光的 time K. 在这100ms内，机器人前行了10米。
        // 拿到这帧激光后，我们往往习惯默认100束激光都是在 K 时刻获取的。
        // 我们要做的事情是什么呢？已知 i 时刻激光器的全局pose Twi, 利用Twi将100束激光变换到w系下，完成点云图。
        // 显然，我们的习惯与实际不符，因此会产生畸变。直的墙会变斜。
        // 如何去畸变呢？
        // 核心目的是获取每个激光束在w系下的表示，那么，得到每个激光束时刻的pose Twi 不就可以了吗？是的。这就是去畸变

        //原点在local系的pose
        // 每一束激光采集时刻，其origin在当前时刻的 odom系的位置是固定的，即外参。但在local系的位置，就是变化的了
        // 和当前时刻_to_local pose 有关 
        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())// min_range=0
        {
            if (range <= options_.max_range())// max_range=30m
            {
                accumulated_range_data_.returns.push_back(hit_in_local);
            }
            else
            {
                // miss 点的距离设定为 5m 
                hit_in_local.position =
                        origin_in_local +
                        options_.missing_data_ray_length() / range * delta;
                accumulated_range_data_.misses.push_back(hit_in_local);
            }
        }
    }

2. 将激光点变换到origin系表示

 

accumulated_range_data_ = range_data_poses.back().inverse() × accumulated_range_data_

至此，运动畸变去除完成了。

注意：这里的origin系 是 origin时刻的 odom 系 ， range_data_poses 保存的也是 odom系到local系的变换。

所用变量取自如下代码中。

        return AddAccumulatedRangeData(
                    time,
                    TransformToGravityAlignedFrameAndFilter(// 这个操作完成后，返回的点云还是表示在local系，只不过是在 gravity_alignment pose下的
                        gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
                        accumulated_range_data_),// accumulated_range_data_ 表示在local系
                    gravity_alignment, sensor_duration);