LocalTrajectoryBuilder2D::AddRangeData

本文框架来自“Cartographer源码阅读12—LocalTrajectoryBuilder2D成员函数解读”。
range_data_in_local存储着odom坐标系下的点云，值格式是3D坐标。

一、LocalTrajectoryBuilder2D::AddRangeData

struct TimedRangefinderPoint {
  // 该点的3D坐标。2D点云时，z总是0，x、y满足sqrt(x*x + y*y)=R，tan(y/x)=Theta。
  Eigen::Vector3f position;
  // 该点的时间戳，单位秒。
  float time;
};

RangeData是AddRangeData常用的数据类型。

struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses;
};

origin值是range_data_poses.back().translation()。零时刻时base_footprint坐标系原点在odom坐标系的坐标，即base_footprint-->odom坐标变换中平移部分。
returns。点云中那些在处理能力范围内的点的坐标。PointCloud是class，可认为就是个std::vector，vector中单元是RangefinderPoint。这类型和之前的TimedPointCloudData、TimedPointCloudOriginData存储坐标值的方法是一样的。
misses。点云中那些超出处理能力范围的点的坐标。若*.lua的min_range、max_range配置值和slam传来的sensor_msgs::LaserScan的一样，那它是空。

AddRangeData是AddSensorData两任务的第一个：新收到一帧点云了，估计机器人现在局部位姿，并将点云画到概率图。

std::unique_ptr
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {

  // sensor::TimedPointCloudData ==> sensor::TimedPointCloudOriginData
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

synchronized_data类型是sensor::TimedPointCloudOriginData。点云：从LaserScan到TimedPointCloudOriginData已叙述了sensor::TimedPointCloudOriginData情况。接下要把TimedPointCloudOriginData进一步转换为sensor::RangeData。

sensor::RangeData是点云数据在(range_data_poses)坐标系下的坐标。

  // 取点云获取的时间为基准初始化PoseExtrapolator
  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  if (!options_.use_imu_data()) {
    // 对rose_ros，use_imu_data总是false，总是进这入口。
    InitializeExtrapolator(time);
  }

  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());
  // TODO(gaschler): Check if this can strictly be 0.
  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  // 第一个点的采集时间(time_first_point)等于零时刻(time)加上第一个点记录的相对时间
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  // 如果该时间比PoseExtrapolator的最新时间还要早，说明在第一个点被采集时，PoseExtrapolator还没初始化
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

synchronized_data.ranges.back().point_time类型是TimedRangefinderPoint，封装了点云最后那个有效点的坐标和时间。按赋值规则，point_time.time存储的是相对时间，最后一个点的值应该是0，前面点则是相对它偏移的秒数，因为时间上发生在前面，所以都是负数。如果最后一个点就是有效点，那ranges.back().point_time.time应该是0，否则是负数。总之得满足<=0。

第一个点的采集时间(time_first_point)等于零时刻(time)加上第一个点记录的相对时间。

接下遍历每个点，针对每个点，用PoseExtrapolator推算机器人在该点的Pose，放入range_data_poses这个向量。

  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()) {
      // 如果该时刻早于PoseExtrapolator的时间
      if (!warned) {
        LOG(ERROR)
            << "Timestamp of individual range data point jumps backwards from "
            << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
        warned = true;
      }
      // 时间设置为PoseExtrapolator的时间
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    // 推算time_point这个时间点的Pose
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast());
  }

range_data_poses用于记录零时刻时，各个扫描点所对应的、用于计算base_footprint-->odom坐标变化的位姿。它的长度等于点云中点数，而且索引一一对应，即range_data_poses的第一个位姿用于点云的第一个点，依次类推。为更直观理解，让看几种场景下range_data_poses实时值。

场景1：机器人没有移动过，即平移和旋转都是0。

range_data_poses[0/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.13299)
range_data_poses[1/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.13281)
range_data_poses[2/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.13262)
range_data_poses[3/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.13244)
range_data_poses[4/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.13225)
...
range_data_poses[657/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.00074)
range_data_poses[658/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.00055)
range_data_poses[659/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.00037)
range_data_poses[660/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(-0.00018)
range_data_poses[661/662]: { t: [0, 0, 0], q: [deg:0] }, time_point:637802439697160986(0)

场景2：机器人向东南移动，大概停在了(1米, -0.8米)、角度-57度。

range_data_poses[0/713]: { t: [0.990458, -0.810532, 0], q: [deg:-57.6755] }, time_point:637802440377018155(-0.13442)
range_data_poses[1/713]: { t: [0.990458, -0.810532, 0], q: [deg:-57.6752] }, time_point:637802440377018155(-0.13424)
range_data_poses[2/713]: { t: [0.990458, -0.810532, 0], q: [deg:-57.6748] }, time_point:637802440377018155(-0.13405)
range_data_poses[3/713]: { t: [0.990458, -0.810532, 0], q: [deg:-57.6744] }, time_point:637802440377018155(-0.13386)
range_data_poses[4/713]: { t: [0.990458, -0.810532, 0], q: [deg:-57.6741] }, time_point:637802440377018155(-0.13368)
...
range_data_poses[708/713]: { t: [0.990457, -0.810554, 0], q: [deg:-57.4169] }, time_point:637802440377018155(-0.00074)
range_data_poses[709/713]: { t: [0.990457, -0.810554, 0], q: [deg:-57.4166] }, time_point:637802440377018155(-0.00056)
range_data_poses[710/713]: { t: [0.990457, -0.810554, 0], q: [deg:-57.4162] }, time_point:637802440377018155(-0.00037)
range_data_poses[711/713]: { t: [0.990457, -0.810554, 0], q: [deg:-57.4159] }, time_point:637802440377018155(-0.00018)
range_data_poses[712/713]: { t: [0.990457, -0.810554, 0], q: [deg:-57.4155] }, time_point:637802440377018155(0)

[0/662]表示该点云有662个点，这是索引0处pose。time_point就是代码中的time_point，值等于“time + common::FromSeconds(range.point_time.time)”。从这两个示例可看到，经过相加后，所有扫描点的time_point都到了同一个时间点，即零时刻。后面()中的是加法中第二部分range.point_time.time，最后一个点的time是0，序号越小，time是越小的负数。综上所述，range_data_poses有以下特点。

所有位姿都计算在零时刻。
长度等于点云中点数，而且两者索引一一对应，即range_data_poses的第一个位姿用于点云的第一个点，依次类推。
用于计算base_footprint-->odom坐标变化的位姿。即它乘以点云后，就可得到点云在odom坐标系下的坐标。
当中所有位姿值基本一样大小。

  // 轨迹开始时刻。只在初始时执行一次。之后num_accumulated不再等于0，该语句就不执行了
  if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  // 遍历每一个点
  // 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) {
    // 处理点云中的第i个有效点，叫hit点。
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    // synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)是第i个有效点的原点，像(0, 0, 0.098)
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    // 将hit点坐标从base_footprint坐标系转换到odom(local)坐标系
    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();

delta是odom坐标系下由hit点到origin_in_local的射线。origin_in_local是“laser”坐标系原点odom下的3D直角坐标，当中值是先从laser下的(0, 0, 0)，转换到base_foorpint下的(0, 0, 0.098)，最后通过range_data_poses[i]这个tf转换到odom。range是delta这个向量的模，可近乎这么认为，它就是“laser”这个极坐标下，该点的极径r。

    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        // 如果该向量的模在合理范围内，则把他压入accumulated_range_data_点集中
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        // 否则，放入misses集中
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }
  }

[options_.min_range, options_.max_range]指示了catographer要处理的点坐标范围。举个例子，激光雷达报告的点范围是[0.15m, 12m]，考虑到效率，用options缩小一次点云处理范围到[0.08m, 8m]。对于此次超过8米的点，等待着机器人更靠近它时再处理。这样可缩小后面的概率图，从而降低整体cpu、内存消耗。

对[0.1m, 8m]内的点，它们是要处理的，于是放入accumulated_range_data_.returns，这很好理解。那为什么把(8m, 12m]的放在accumulated_range_data_.misses？首要要知道，放入misses的坐标依旧是个在[0.08m, 8m]范围内的点。从原点到miss点可形成一条射线，可以确定这整条射线涉及到点应该都没有障碍物，后面CastRays正是用这个结论把点云画到概论图，参考“概率栅格图(ProbabilityGrid)”。

限定了不能让放入misses集合的点坐标超出max_range。配置时，missing_data_ray_length要小于max_range，目前这个值是1.0米，小于max_range的8米。

  ++num_accumulated_;

  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    // 点云帧数大于1的话(num_accumulated_range_data在*.lua默认值是1)
    const common::Time current_sensor_time = synchronized_data.time;
    absl::optional 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;
    // 估计重力
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    accumulated_range_data_.origin = range_data_poses.back().translation();
    // 调用AddAccumulatedRangeData进行匹配、插入数据等。返回MatchingResult
    return AddAccumulatedRangeData(
        time,
        TransformToGravityAlignedFrameAndFilter( // 调用这个函数进行重力align
            gravity_alignment.cast() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);

解释下gravity_alignment.cast() * range_data_poses.back().inverse()。这是把odom坐标系下的点云(accumulated_range_data_)依次做两次坐标变换，第一次变换是range_data_poses.back().inverse()，第二次变换是gravity_alignment。由于range_data_poses.back().inverse()是最后一个点的位姿的逆，因而对最后一个点来说，经过第一次变化后，它完全恢复到base_footprint坐标系下的坐标，即等于synchronized_data中的值。其它点也变换到base_footprint坐标系，只是和synchronized_data中的值有一点偏差，毕竟range_data_poses[i]中的位姿并不完全等于range_data_poses.back()。gravity_alignment中平移总是0、只有旋转，可直观想象为是在第一次变化后的基础上逆时针旋转gravity_alignment.rotation度。经过这两次变化后的点云(gravity_aligned_range_data)将送入优化器

  }
  return nullptr;
}

二、LocalTrajectoryBuilder2D::AddAccumulatedRangeData

AddAccumulatedRangeData依次做三件事。

算出此帧点云后的local_pose。
用local_pose计算出ScanMatch优化后的点云，存储在变量range_data_in_local。
调用InsertIntoSubmap，把此帧点云画向概率图。

AddAccumulatedRangeData涉及两种点云数据：gravity_aligned_range_data和range_data_in_local，它们类型都是sensor::RangeData，都属odom坐标系。

time。零时刻。
gravity_aligned_range_data。经过transform_to_gravity_aligned_frame修正过的点云数据，即融合了位姿误差。有四个用途。1）以参数送入ScanMatch。2）ScanMatch后，用于恢复出ScanMatch优化后的点云range_data_in_local。3）作为参数送入InsertIntoSubmap。4）赋值到InsertionResult.constant_data(类型TrajectoryNode::Data).filtered_gravity_aligned_point_cloud字段。
gravity_alignment。此次点云后应该的imu_pose。

std::unique_ptr
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional& sensor_duration) {
  static int times = 0;
  static double total_cost = 0;
  trosverbose verbose("LocalTrajectoryBuilder2D::AddAccumulatedRangeData", times, total_cost);

  if (gravity_aligned_range_data.returns.empty()) {
    // 如果数据为空
    LOG(WARNING) << "Dropped empty horizontal range data.";
    return nullptr;
  }

  // Computes a gravity aligned pose prediction.
  // 计算经过重力aligned的Pose
  // 从PoseExtrapolator处获得未经重力aligned的Pose
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  // 重力align
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  // 生成一个自适应体素滤波器，对数据进行一下自适应体素滤波
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

  // local map frame <- gravity-aligned frame
  // 调用ScanMathc函数进行匹配，求取Scan插入Submap的最优Pose
  std::unique_ptr pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
  if (pose_estimate_2d == nullptr) {
    //如果为空，表示匹配为空
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }
  // 考虑重力方向，将2d的pose变成3d的pose
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 将新的pose添加到PoseExtrapolator的Pose队列中
  extrapolator_->AddPose(time, pose_estimate);

1/3：算出此帧点云后的local_pose。

外推出位姿差pose_prediction。
用ScanMatch进行优化，算出此帧点云后的local_pose，即变量pose_estimate。
AddPose加到位姿外推器的timed_pose_queue_。

  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast()));

2/3：用local_pose计算出odom坐标系下点云，存储在变量range_data_in_local。

  std::unique_ptr insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

3/3：调用InsertIntoSubmap，更新Submap，把此帧点云画向概率图，同时返回插入结果。

  // 统计一下数据累计的时间
  const auto wall_time = std::chrono::steady_clock::now();
  if (last_wall_time_.has_value()) {
    const auto wall_time_duration = wall_time - last_wall_time_.value();
    kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
    if (sensor_duration.has_value()) {
      kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
                                   common::ToSeconds(wall_time_duration));
    }
  }
  const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
  if (last_thread_cpu_time_seconds_.has_value()) {
    const double thread_cpu_duration_seconds =
        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value()) {
      kLocalSlamCpuRealTimeRatio->Set(
          common::ToSeconds(sensor_duration.value()) /
          thread_cpu_duration_seconds);
    }
  }
  last_wall_time_ = wall_time;
  last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
  // 返回匹配结果
  return absl::make_unique(
      MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                     std::move(insertion_result)});
}

三、点云：从TimedPointCloudOriginData到range_data_in_local

“点云：从LaserScan到TimedPointCloudOriginData”叙述了从雷达采到点云，一直到转成TimedPointCloudOriginData，即AddRangeData(...)中的synchronized_data。这里接着描述AddRangeData是如何把synchronized_data处理成表示点云在map坐标系下的坐标，并存储在range_data_in_local。

为直观，用一次点云的第一个和最后一个点作为示例，看它们的变化过程。

变量synchronized_data。点数：709
front: {(-1.17182, -0.02045, 0.09799), r: 1.17200 theta: -178.99998}
back: {(-1.18400, 0.00001, 0.09799), r: 1.18400 theta: 179.99947}

上面是这两个点的初始值，“sensor_msgs::LaserScan”有写，RPLIDAR A1M8在LaserScan报告的视场角是[-179, 180]，front是第一个扫描点，-178.99998度，近似起始角度-179，back是最后一个，则近似终止角度180。

步骤1、得到点云未优化的、在odom坐标系下的坐标，放在accumulated_range_data_

这个步骤的目标是计算出点云未优化的、在odom坐标系下坐标，计算方法是以下这条语句。

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

range_data_poses是个std::vector，长度709，存储着对应709个点的未优化local_pose。这个乘法是个标准的两坐标系间欧氏变换，具体是把base_footprint坐标系中的坐标hit变换到odom坐标系。

range_data_poses[front]: ({ t: [1.57805, 0.366646, 0], q: [25.9723] }) + {-178.99998}(-1.17182, -0.02045, 0.09799) = theta{-153.02765}
range_data_poses[back]: ({ t: [1.56143, 0.381865, 0], q: [26.3522] }) + {179.99947}(-1.18400, 0.00001, 0.09799) = theta{-153.64837}

附：图1对应代码

{
    Eigen::AngleAxisd rotation_vector(25.9723 * M_PI / 180, Eigen::Vector3d(0, 0, 1));

    Eigen::Vector3d v(-1.17182, -0.02045, 0.09799);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    SDL_Log("v_rotated: %.5f, %.5f, %.5f", v_rotated[0], v_rotated[1], v_rotated[2]);

    Eigen::Vector3d t_x(1.57805, 0, 0);
    Eigen::Vector3d v_result_x = v_rotated + t_x;
    SDL_Log("v_result_x: %.5f, %.5f, %.5f", v_result_x[0], v_result_x[1], v_result_x[2]);

    Eigen::Vector3d t_y(0, 0.366646, 0);
    Eigen::Vector3d v_result = v_result_x + t_y;

    SDL_Log("v_result: %.5f, %.5f, %.5f", v_result[0], v_result[1], v_result[2]);
}

图1描述了front点坐标变换。为直观，把变换拆成三个操作，先旋转25.9723度，再x方向平移，最后y方向平移。过程中雷达转了25.9723度，在雷达坐标系，f点最终角度是(-178.99998 + 25.9723)度。但对odom坐标系，f点最终角度是-17.17754度。

从图1可看出：平移不影响角度，这说法要正确，是针对坐标系#2，坐标系#1下不成立。

变量accumulated_range_data_.returns。点数：709
front: {(0.53353, -0.16492, 0.09799), r: 0.55844 theta: -17.17754}
back: {(0.50046, -0.14368, 0.09799), r: 0.52068 theta: -16.01924}

上面是经过步骤1，这两个点的值。accumulated_range_data_类型是sensor::RangeData，后面各步骤会把点云处理成不同数值，但存储点云的变量类型就不再变了，包括range_data_in_local。

步骤2、重力校正，放在gravity_aligned_range_data

接下要执行ScanMatch，ScanMatch需要点云作为参数，参数须要是经过重力校正后的点云。

gravity_alignment：{ t: [0, 0, 0], q: [24.65049] }

transform::Rigid3f transform_to_gravity_aligned_frame = gravity_alignment.cast<float>() * range_data_poses.back().inverse();
// 代入：{ t: [0, 0, 0], q: [24.65049] } * { t: [1.56143, 0.381865, 0], q: [26.3522] }
gravity_aligned_range_data = transform_to_gravity_aligned_frame * accumulated_range_data_;

transform_to_gravity_aligned_frame是修正因子，左乘上它，这是把odom坐标系下的点云(accumulated_range_data_)依次做两次坐标变换，第一次变换是range_data_poses.back().inverse()，第二次变换是gravity_alignment。由于range_data_poses.back().inverse()是最后一个点的位姿的逆，因而对最后一个点来说，经过第一次变化后，它完全恢复到base_footprint坐标系下的坐标，即等于synchronized_data中的值。其它点也变换到base_footprint坐标系，只是和synchronized_data中的值有一点偏差，毕竟range_data_poses[i]中的位姿并不完全等于range_data_poses.back()。gravity_alignment中平移总是0、只有旋转，可直观想象为是在第一次变化后的基础上逆时针旋转gravity_alignment.rotation度。经过这两次变化后的点云(gravity_aligned_range_data)将送入优化器。

变量：gravity_aligned_range_data。点数：437
front: {(-1.04368, -0.51602, 0.09799), r: 1.16428 theta: -153.69085}
back: {(-1.07610, -0.49381, 0.09799), r: 1.18400 theta: -155.35002}。根据对修正因子(transform_to_gravity_aligned_frame)解释，此刻的值应该和base_footprint下synchronized_data中的值有着一样极径r，角度则逆时针旋转了24.65049度：179.99947 + 24.65049 - 360 = -155.35004。注意，此刻和synchronized_data中的x、y是不一样的，x、y一样是发生在第一次变换后，第二次变换时旋转了一个非0角度，自然要改x、y。

步骤3、重力恢复，放在range_data_in_local

ScanMatch了算出优化后的local_pose，即pose_estimate_2d，接下是用它恢复出点云，存储优化过的、在odom坐标系下的坐标。

sensor::RangeData range_data_in_local =
  transform::Embed3D(pose_estimate_2d->cast<float>()) * gravity_aligned_range_data

pose_estimate_2d“应该”是“gravity_alignment.cast<float>() * range_data_poses.back().inverse()”的inverse，“应该”加引号，是因为当中有过优化。

pose_estimate/Embed3D(pose_estimate_2d)：{ t: [1.56621, 0.382067, 0], q: [25.8245]}
变量：range_data_in_local。点数：437
front: {(0.53331, -0.15523, 0.09799), r: 0.55545 theta: -16.22897}
back: {(0.50044, -0.13369, 0.09799), r: 0.51799 theta: -14.95712}

以上是经过重力恢复后，两个点在odom坐标系的坐标，应该和重力校正前“未优化的坐标”差别不大。

四、LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter

// 将RangeData转化成重力校正后的数据，并经过VoxelFilter
// 关于VoxelFilter和AdaptiveVoxelFilter，可参见如下两个参考链接：
// 源码解读：https://blog.csdn.net/learnmoreonce/article/details/76218136
// VoxelFilter的原理：https://blog.csdn.net/xiaoma_bk/article/details/81780066
// cartographer中的VoxelFilter是把空间分为很多个立方体的栅格，然后一个栅格内可能有很多点，
// 但只用一个点来代表整个栅格中的所有点
// 简单说，这些滤波就是对点云数据的预处理：去除不合理的范围的点、合并相同位置的点等，从而减少点云数量，提高点云质量
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
  //裁剪数据，指定一定z轴范围内的数据。在/src/cartographer/configuration_files/trajectory_builder_2d.lua中找到
  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())};
}

五、LocalTrajectoryBuilder2D::ScanMatch

// 扫描匹配。输入参数：
// 1. 时间,
// 2. 由PoseExtrapolator预测的初始位姿pose_prediction
// 3. 经过重力aligned的RangeData
// 输出是对该帧传感器数据的最优pose。采用Ceres Scan Matcher
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
  if (active_submaps_.submaps().empty()) {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  // 获取要进行扫描匹配的Submap的index
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  // Online Correlative Scan Matcher会refine初始Pose
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  if (options_.use_online_correlative_scan_matching()) {
    // 如果配置项设置使用OnlineCorrelativeScanMatching。但配置项里默认该项为false
    // 调用RealTimeCorrelativeScanMatcher2D的方法进行匹配。返回一个得分
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  // 调用Ceres库来实现匹配。匹配结果放到pose_observation中
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  // 统计残差
  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  // 返回优化后的pose
  return pose_observation;
}

六、LocalTrajectoryBuilder2D::InsertIntoSubmap

// 插入submap, 返回插入结果。这是在都已经完成匹配的情况下，调用该函数更新submap
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& range_data_in_local,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment) {
  // 如果没有被MotionFilter滤掉
  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }
  // 调用submap的工具函数将传感器数据插入，更新Submap。
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
  // 返回插入结果
  return absl::make_unique<InsertionResult>(InsertionResult{
      std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate}),
      std::move(insertion_submaps)});
}

LocalTrajectoryBuilder2D::AddRangeData

全部评论: 0 条