void ImuTracker::Advance(const common::Time time) 
{
   // 要求指定的时间要比当前时间晚或相等
  CHECK_LE(time_, time);
  // 求当前时间对指定时间的时间差
  const double delta_t = common::ToSeconds(time - time_);
  //假设为匀角速度运动的计算， 求得角度， 换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 在原朝向基础上继续旋转， 更新方位角
  // orientation_ 初始为 Eigen::Quaterniond::Identity()
  orientation_ = (orientation_ * rotation).normalized();

  /*  车体的旋转方向，跟重力向量的旋转方向是反向的，比如车体向上仰a角度，
  重力向量是往反方向即俯方向旋转a角度，
  conjugate 返回当前四元数的共轭四元数，就是逆旋转 */
  // gravity_vector_ 初始为 Eigen::Vector3d::UnitZ()  向量(0,0,1)
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

// 因为imu直接测得角速度，所以直接更新
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity)
{
  imu_angular_velocity_ = imu_angular_velocity;
}

// 使用线加速度更新重力向量，使用了一阶低通滤波， 防止加速度跳动剧烈
void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) 
{
  // 当前时刻离上一次时刻的差， 初值是inf
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;
  // 计算权重, alpha为滤波系数, delta_t越大, 当前的权重越大. delta为inf时，权重为1
  // imu_gravity_time_constant_为滤波器常数， 默认为10
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  // 一阶低通滤波， gravity_vector_ 为滤波输出值， imu_linear_acceleration为采样值
  // 初始的输出值是 imu_linear_acceleration，如果没有IMU就是 (0,0,1)
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

  // 根据新的重力向量，更新对应的旋转矩阵, FromTwoVectors返回一个表示从向量a旋转到向量b的四元数
      // rotation 看做线加速度对上次线加速度的校正
    // orientation_.conjugate() * Eigen::Vector3d::UnitZ()就是把这个四元数转换成向量的形式
    // 在这里我们只关注重力方向上的值，即z轴上的值
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ() );
  // 更新方位角
  orientation_ = (orientation_ * rotation).normalized();
  // 如果之前的计算正确，二者的乘积应当是（0,0,1）
  CHECK_GT((orientation_ * gravity_vector_).z(),  0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(),  0.99);
}

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                       ImuTracker* const imu_tracker)  const 
{
  CHECK_GE(time, imu_tracker->time());
  // 当没有IMU数据时，输入一个假的imu数据
  if (imu_data_.empty() || time < imu_data_.front().time) 
  {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    imu_tracker->Advance(time);  // 根据时间进行imu数据插值
    // 给定一个理论上的重力向量
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    //优先选择里程计得出的角速度给imu
    //当里程计的数据小于2个时，选择从pose；否则选择里程计。因为里程计的精度更高
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_ );
    return;   // 没有IMU可以return了
  }
  //有imu时，此处调用了imu数据队列，ImuTracker维护的时间早于IMU数据队列最早的时间
  if (imu_tracker->time() < imu_data_.front().time)
  {
    // 先把ImuTracker更新到IMU数据来临的那一刻
    imu_tracker->Advance(imu_data_.front().time);
  }
  // 依次取出IMU数据队列中的数据，更新ImuTracker，直到IMU数据的时间比指定时间time要晚
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });
  // 用imu数据队列进行imu-tracker的数据更新.
  // imu_tracker主要负责对角速度进行插值, 并且估计重力向量
  while (it != imu_data_.end() && it->time < time)
  {
      imu_tracker->Advance(it->time);
      imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
      imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
      ++it;
  }
  imu_tracker->Advance(time);
}

if(extrapolator_ != nullptr)
    return;
CHECK(!options_.pose_extrapolator_options().use_imu_based());
// 创建一个PoseExtrapolator类型的位姿推理器
// pose queue 用于速度估计，pose_queue_duration是估计所用的时间
extrapolator_ = absl::make_unique<PoseExtrapolator>(
    ::cartographer::common::FromSeconds(
    options_.pose_extrapolator_options().constant_velocity().pose_queue_duration() ),
    options_.pose_extrapolator_options().constant_velocity().imu_gravity_time_constant()  );
// 添加一个初始位姿，Rigid3d::Identity()对变换矩阵赋值，位移为0，旋转为1
extrapolator_->AddPose(time, transform::Rigid3d::Identity() );

common::Time PoseExtrapolator::GetLastPoseTime() const {
  if (timed_pose_queue_.empty()) {
    return common::Time::min();
  }
  return timed_pose_queue_.back().time;
}

sensor_bridges_[trajectory_id] = 
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          // 实际类型是 CollatedTrajectoryBuilder
          map_builder_->GetTrajectoryBuilder(trajectory_id)  );

void AddSensorData(
  const std::string& sensor_id,
  const sensor::TimedPointCloudData& timed_point_cloud_data) override
{
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}

void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data)
{
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  // Queue keys are a pair of trajectory ID and sensor identifier
  // OrderedMultiQueue  queue_;
  queue_.Add(std::move(queue_key), std::move(data));
}

key1(sensor_1): queue
key2(sensor_2): queue

struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;   //回调
    bool finished = false;
};

// CollatedTrajectoryBuilder 的构造函数调用该函数
// CollatedTrajectoryBuilder 的 HandleCollatedSensorData 作为回调函数callback传进来
// 添加轨迹以生成排序的传感器输出，每个topic设置一个回调函数
void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback)
{
  for (const auto& sensor_id : expected_sensor_ids) 
  {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    // 根据QueueKey，将对应的回调函数callback(CollatedTrajectoryBuilder::HandleCollatedSensorData)
    // 放入 queue_ （OrderedMultiQueue::AddQueue）里
    queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

// 参数sensor_id记录了产生数据的传感器， timed_point_cloud_data则是传感器产生的数据
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.";
    // Local SLAM的业务主线, 在该结果中同时记录了子图的更新信息
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);
    // 结果中包含了轨迹中节点的pose、点云和submap的相关信息
    if (matching_result == nullptr)
      // The range data 还没有fully accumulated yet.
        return;


    /* 将前端的输出结果喂给后端进行闭环检测和全局优化  */
    kLocalSlamMatchingResults->Increment();
     // 前端匹配的结果，即submap位置和submap
    std::unique_ptr<InsertionResult> insertion_result;
    // 判定前端成功的是否将传感器的数据插入到子图中
    if (matching_result->insertion_result != nullptr) 
    {
      kLocalSlamInsertionResults->Increment();
      // 将匹配后的结果加入图优化节点中
      // 把前端的输出结果喂给后端，即后端增加一个新的节点
      // 后端优化位姿图节点ID，记录在临时对象 node_id
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps );
      CHECK_EQ(node_id.trajectory_id, trajectory_id_);

      // 重新封装前端匹配的后的submap结果，并增加一个node_id
      insertion_result = absl::make_unique<InsertionResult>( InsertionResult{
          node_id, matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())  }  );
    }
    // 如果提供了回调函数，就调用之，并将前端的输出和刚刚构建的
    //insertion_result对象传参，也就是采用回调函数将前端匹配的结果信息传递出去
    if (local_slam_result_callback_) 
    {
      local_slam_result_callback_(
          trajectory_id_, matching_result->time, matching_result->local_pose,
          std::move(matching_result->range_data_in_local),
          std::move(insertion_result)  );
    }
}

struct InsertionResult
{
  // 有两个字段,分别记录了轨迹的索引(trajectory_id)和一个从零开始计数的节点编号(node_index)
  NodeId node_id;
  // 子图更新时在局部地图中的位姿，以及有传感器原始数据转换之后的点云信息
  std::shared_ptr<const TrajectoryNode::Data> constant_data;
  // 被更新的子图对象
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

struct InsertionResult
{
  // 插入的节点数据，TrajectoryNode::Data 包含了处理之后的点云数据
  std::shared_ptr<const TrajectoryNode::Data> constant_data;
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

void AddSensorData(const std::string& sensor_id,
                   const sensor::ImuData& imu_data) override
{
  // InitializeExtrapolator 和 extrapolator_->AddImuData
  if (local_trajectory_builder_)
    local_trajectory_builder_->AddImuData(imu_data);

  pose_graph_->AddImuData(trajectory_id_, imu_data);
}

void AddSensorData(const std::string& sensor_id,
                   const sensor::OdometryData& odometry_data) override 
{
  CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
  if (local_trajectory_builder_)
    // 实际有用的就一句 extrapolator_->AddOdometryData(odometry_data);
    local_trajectory_builder_->AddOdometryData(odometry_data);
  
  pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}

void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
{
  // 转换之后的点云数据和时间戳
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  // 将ROS的消息转换成点云数据，函数在 msg_conversion.cc
  // 只有一句 LaserScanToPointCloudWithIntensities(msg)
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty())
{
  const double duration = point_cloud.points.back()[3];
  // scan 最后一束激光的时间戳
  timestamp += cartographer::common::FromSeconds(duration);
  // 时间戳变成以最后一个测量数据为基准
  for (Eigen::Vector4f& point : point_cloud.points)
    point[3] -= duration;
}

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()[3], 0);  //最后一个点云数据的时间小于等于0
  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()[3];
    // 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 (Eigen::Vector4f& point : subdivision)
    {
      point[3] -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back()[3], 0);
    // 将分段数据喂给Cartographer
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  }
}

void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,  const carto::sensor::TimedPointCloud& ranges)
{
  // 查询在time时间，tracking_frame--->laser坐标系的变换，记录到对象 sensor_to_tracking
  // 这里有时会卡住
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  // 如果找到变换，通过trajectory_builder_ 添加传感器数据
  if (sensor_to_tracking != nullptr)
  {
    trajectory_builder_->AddSensorData(
    // 第一个转换是 TransformTimedPointCloud, 把数据从 sensor frame到tracking frame
    // 第二个转换是从 TimedPointCloud 转到了 TimedPointCloudData
    sensor_id, carto::sensor::TimedPointCloudData{
                   time, sensor_to_tracking->translation().cast<float>(),
                   carto::sensor::TransformTimedPointCloud(
                       ranges, sensor_to_tracking->cast<float>() )  }  );
  }
}

class TfBridge {
 public:
  TfBridge(const std::string& tracking_frame,
           double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
  ~TfBridge() {}

  TfBridge(const TfBridge&) = delete;
  TfBridge& operator=(const TfBridge&) = delete;

  // 查询在time时间，frame_id--->tracking_frame 的变换，如果存在的话
  // 也就是说 tracking_frame 是 target frame
  std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
      ::cartographer::common::Time time, const std::string& frame_id) const;

 private:
  const std::string tracking_frame_;
  const double lookup_transform_timeout_sec_;
  const tf2_ros::Buffer* const buffer_;
};

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

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const
{
  //裁剪数据，指定一定z轴范围内的数据
  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(options_.voxel_filter_size()).Filter(cropped.returns),
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)  };
}

// using PointCloud = std::vector<RangefinderPoint>;
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) 
{
  PointCloud results;
  for (const RangefinderPoint& point : point_cloud)
  {
    auto it_inserted =
        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
        // 该体素之前没有没占据过，则插入，否则不插入
    if (it_inserted.second)
        results.push_back(point);
  }
  return results;
}

const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
  sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
      .Filter(gravity_aligned_range_data.returns);

PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const 
{
  return AdaptivelyVoxelFiltered(
      options_, FilterByMaxRange(point_cloud, options_.max_range())  );
}

PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud)
{
  // 已经足够稀疏，默认200
  if (point_cloud.size() <= options.min_num_points()) {
    return point_cloud;
  }
  // 还是先调用VoxelFilter来滤波，参数 max_length
  PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
  // 按max_length进行滤波，还超过这个数，说明是这个参数能实现的最稀疏
  if (result.size() >= options.min_num_points()) {
    return result;
  }

  // 如果小于min_num_points，点云太稀疏，用于扫描匹配就不准了
  // 说明上面的 max_length太大了，现在要使用二分法找一个合适的参数
  // Search for a 'low_length' that is known to result in a sufficiently
  // dense point cloud. We give up and use the full 'point_cloud' if reducing
  // the edge length by a factor of 1e-2 is not enough.
  for (float high_length = options.max_length();
       high_length > 1e-2f * options.max_length(); 
          high_length /= 2.f)
  {
    float low_length = high_length / 2.f;
    result = VoxelFilter(low_length).Filter(point_cloud);
    if (result.size() >= options.min_num_points())
    {
      // 二分查找法找到合适的过滤器
      // low_length给出足够稠密的结果
      // 当 edge length is at most 10% off 时停止
      while ((high_length - low_length) / low_length > 1e-1f)
      {
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate =
            VoxelFilter(mid_length).Filter(point_cloud);
        // 点数又多了，增加边长，让 low_length 变大
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        }
        // 点数偏少，让high_length变小
        else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;
}

struct InsertionResult {
    // Node数据
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    //该Node数据插入的两个submap
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
  };

struct MatchingResult
{
    common::Time time;    // 扫描匹配发生的时间
    // tracking 坐标系在Local SLAM坐标系下的位姿, 需要乘以
    // Local SLAM坐标系到Global SLAM坐标系的转换 才能转到全局坐标系
    transform::Rigid3d local_pose;
    // tracking 坐标系下的点云（即LocalSLAM坐标系下的点云）
    sensor::RangeData range_data_in_local;
    // 如果被 motion filter 过滤，就是 nullptr
    std::unique_ptr<const InsertionResult> insertion_result;
};

// 把来自不同传感器的TimePointCloudData进行同步。TimePointCloudData是带时间的点云数据
// 3D情况下，前3个元素是点的坐标，第4个元素是测量到每个点的相对时间。
// 时间以s为单位，以最后一个点的捕获时间为0，越早捕获的点时间值越大。
// 对于2d情况，第3个元素始终为0，第4个元素同样表示时间
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data)
{
  // 把输入的索引和数据记录在range_data_collator_中，同时做了时间同步
  // RangeDataCollator::AddRangeData 
  // TimedPointCloudOriginData   range_data_collator_
  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;
  }
  // 记录下同步时间
  const common::Time& time = synchronized_data.time;
  // 如果不使用IMU,那么直接初始化 extrapolator
  // 取点云获取的时间为基准
  if (!options_.use_imu_data())
     InitializeExtrapolator(time);

  // 如果使用IMU进行估计，那么将在接收到IMU消息之后，再初始化 extrapolator
  if (extrapolator_ == nullptr)
  {
    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);
  // 获取第一个数据点的绝对时间，第一个点的时间就等于点云集获取的时间加上第一个点记录的相对时间
  // 将它与 extrapolator的时间对比，来判定 extrapolator 是否完成了初始化
  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;
  }

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();
  }
  // 根据每个点的时间戳，估计每个点对应的位置并进行缓存
  // 根据时间戳，计算 tracking坐标系在local坐标系下的位姿
  range_data_poses.push_back(
      extrapolator_->ExtrapolatePose(time_point).cast<float>()  );
}

// 如果还没有累积过扫描数据，就重置对象 accumulated_range_data_
// sensor::RangeData   accumulated_range_data_;
// 它就是后面插入子图的 scan 的来源
if (num_accumulated_ == 0)
    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) 
{
  const sensor::TimedRangefinderPoint& hit =
      synchronized_data.ranges[i].point_time;
  /*  下面两行是左乘机器人位姿，将tracking坐标系下的激光测量的hit点位姿
  和传感器位姿转换到 local map 的坐标系下*/
  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();   // 该向量的模

  // 完成 hit集合 和 miss集合
  // 所有低于min_range的hit点不考虑，但大于max_range的点做处理后放入miss集
  if (range >= options_.min_range() )
  {
    if (range <= options_.max_range())
      // 如果该向量的模在合理范围内，则压入returns点集
      accumulated_range_data_.returns.push_back(hit_in_local);
      // delta多了系数 missing_data_ray_length() / range 
    /* 这个公式的作用是在miss方向上距离原点 misses_data_ray_length长度处，
    选择一个点作为miss点加入miss集合，后面在更新submap的概率的时候，
    原点到return射线中间的点和到 misses 射线的点都是通过miss_table来查询概率。
    对超过最大距离的射线，选择一个中间的距离点，到这个中间距离点的区域，
    我们都认为是空白区域 */
    else
    {
      hit_in_local.position =
          origin_in_local + options_.missing_data_ray_length() / range * delta;
      accumulated_range_data_.misses.push_back(hit_in_local);
    }
  }
}

  // 有了一帧有效点云数据了
  ++num_accumulated_;
   // 每当累积的有效激光帧数 超过了配置值，一般为1
  if (num_accumulated_ >= options_.num_accumulated_range_data()  )
  {
      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;  // 计数清零
      // 获取重力的方向，后面将点云z轴和重力方向对齐
      const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
          extrapolator_->EstimateGravityOrientation(time) );

      // 计算origin，在插入子图时非常重要
      // 最后一个点的时间戳估计的坐标，作为原点
      accumulated_range_data_.origin = range_data_poses.back().translation();
      return AddAccumulatedRangeData(
          time,
          // 点云对齐到重力方向后，对return集合体素化滤波
          // 点云变换到原点处
          TransformToGravityAlignedFrameAndFilter(
              gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
              accumulated_range_data_),

              gravity_alignment, 
              sensor_duration  );
  }
  return nullptr;
}

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const
{
  const sensor::RangeData cropped =
      // 限定点云z坐标
      sensor::CropRangeData(sensor::TransformRangeData(
              range_data, transform_to_gravity_aligned_frame),
      options_.min_z(), options_.max_z());
      // 对 returns 和 misses点云做体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor:: (options_.voxel_filter_size()).Filter(cropped.returns),
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses) };
}

RangeData TransformRangeData(const RangeData& range_data,
                             const transform::Rigid3f& transform)
{
	return RangeData{
	  transform * range_data.origin,
	  TransformPointCloud(range_data.returns, transform),
	  TransformPointCloud(range_data.misses, transform),
	};
}
// 对点云中每一个点做变换
PointCloud TransformPointCloud(const PointCloud&  point_cloud,
                       const transform::Rigid3f&  transform)
{
	PointCloud result;
	result.reserve(point_cloud.size()  );
	for (const RangefinderPoint& point : point_cloud)
	{
		result.emplace_back(transform * point);
	}
	return result;
}

auto map_builder =
  cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
      node_options.map_builder_options );

const proto::MapBuilderOptions options_;	//MapBuilder的配置项
common::ThreadPool thread_pool_;  //线程池，应该是为每一条trajectory都单独开辟一个线程
// 一个PoseGraph的智能指针
std::unique_ptr<PoseGraph>  pose_graph_; 
//收集传感器数据的智能指针
unique_ptr<sensor::CollatorInterface> sensor_collator_;
//一个向量，管理所有的TrajectoryBuilderInterface;应该是每一个trajectory对应了该向量的一个元素
// 每一个TrajectoryBuilder对应了机器人运行了一圈
vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> trajectory_builders_;
//与每个 TrajectoryBuilderInterface 相对应的配置项
vector<proto::TrajectoryBuilderOptionsWithSensorIds> all_trajectory_builder_options_;

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) 
{
	// 加载配置，注意 optimization::OptimizationProblem2D 对象
  pose_graph_ = absl::make_unique<PoseGraph2D>(
    options_.pose_graph_options(),
    absl::make_unique<optimization::OptimizationProblem2D>(
        options_.pose_graph_options().optimization_problem_options() ),
    &thread_pool_);
  // 构建修正器
  if (options.collate_by_trajectory())
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  else
    sensor_collator_ = absl::make_unique<sensor::Collator>();
}

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>&  expected_sensor_ids,
    const proto::TrajectoryBuilderOptions&  trajectory_options,
    LocalSlamResultCallback   local_slam_result_callback)
{
  // 当有一个新的trajectory时，以向量的size值作为新的trajectory，加入到trajectory_builders_中
  const int trajectory_id = trajectory_builders_.size();
                                      // 省略 3D 部分
  /*LocalTrajectoryBuilder2D是不带Loop Closure的Local Slam, 包含了Pose Extrapolator, Scan Matching等
  这个类并没有继承TrajectoryBuilderInterface, 并不是一个TrajectoryBuilderInterface的实现
  而只是一个工具类，构造函数全是配置的赋值*/
  std::unique_ptr<LocalTrajectoryBuilder2D>  local_trajectory_builder;
  if (trajectory_options.has_trajectory_builder_2d_options() )
  {
    local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
        trajectory_options.trajectory_builder_2d_options(),
        // 找出expected_sensor_ids中的雷达的话题，比如scan1,scan2
        SelectRangeSensorIds(expected_sensor_ids) );
  }
  DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())  );
  /*  类CollatedTrajectoryBuilder   继承了接口 TrajectoryBuilderInterface，这个才是真正的
  轨迹跟踪器，使用 sensor::CollatorInterface 接口来收集传感器数据，具体的类在 MapBuilder 构造函数最后
   CollatedTrajectoryBuilder是 sensor_bridges_构造函数中的对应最后一个参数的类型  */
  trajectory_builders_.push_back( absl::make_unique<CollatedTrajectoryBuilder>(
      trajectory_options, sensor_collator_.get(), trajectory_id,
      expected_sensor_ids,
      /*  函数返回类型实际是 std::unique_ptr<GlobalTrajectoryBuilder>，它实际上是一个模板类
      其模板列表中的  LocalTrajectoryBuilder2D 和  PoseGraph2D  分别是前端和后端的两个核心类型
      local_slam_result_callback 前端数据更新后的回调函数  */

      // 将2D前端和位姿图打包传入 CollatedTrajectoryBuilder
      CreateGlobalTrajectoryBuilder2D(
          std::move(local_trajectory_builder), trajectory_id,
          static_cast<PoseGraph2D*>(pose_graph_.get()  ),
          local_slam_result_callback)  )  );

  // 如果开启了纯定位模式 
  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get());
  /*  如果开始建图之前已经有了初始位置，将初始位置提供给pose_graph_对象
  比如说，我们检测到了一个Landmark。那么这时，我们可以新增加一条trajectory
  然后根据机器人与Landmark之间的相对位姿推算机器人相对于世界坐标系的相对位姿
  以该位姿作为新增加的trajectory的初始位姿。这样在检测到Landmark时就能有效降低累积误差*/
  if (trajectory_options.has_initial_trajectory_pose())
  {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()) );
  }
  // 将轨迹跟踪器的配置信息和传感器配置信息保存到容器all_trajectory_builder_options_中
  proto::TrajectoryBuilderOptionsWithSensorIds  options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids)
  {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;

  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
  return trajectory_id;
}

const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
    expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
const int trajectory_id = 
    map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, topics, trajectory_id);
is_active_trajectory_[trajectory_id] = true;
for (const auto& sensor_id : expected_sensor_ids)
{  // unordered_set
  subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;

struct SensorId
{
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    SensorType type;
    std::string id;
}

cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
  cartographer_ros_msgs::SensorTopics topics;
  topics.laser_scan_topic = kLaserScanTopic;
  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
  topics.point_cloud2_topic = kPointCloud2Topic;
  topics.imu_topic = kImuTopic;
  topics.odometry_topic = kOdometryTopic;
  topics.nav_sat_fix_topic = kNavSatFixTopic;
  topics.landmark_topic = kLandmarkTopic;
  return topics;
}

// 添加一个轨迹跟踪器，同时将构建成功的索引返回
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids,  trajectory_options.trajectory_builder_options,
    // 这个参数相当于注册了一个回调函数 OnLocalSlamResult ,  用于响应map_builder_完成
    // 一个局部SLAM 或者 说是成功构建了一个子图的事件
    ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                ::std::placeholders::_1, ::std::placeholders::_2,
                ::std::placeholders::_3, ::std::placeholders::_4,
                ::std::placeholders::_5) );

LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

// std::unordered_map<int, std::unique_ptr<SensorBridge>>  sensor_bridges_;
// 检查 trajectory_id， 确保之前没有使用过
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// SensorBridge将ROS的消息转换成 Cartographer 中的传感器数据类型
// 前面都是参数，最后的参数是Cartographer的一个核心对象，TrajectoryBuilderInterface指针
// 通过sensor_bridge对象转换后的数据都是通过它喂给Cartographer的
sensor_bridges_[trajectory_id] = 
    cartographer::common::make_unique<SensorBridge>(
        trajectory_options.num_subdivisions_per_laser_scan,
        trajectory_options.tracking_frame,
        node_options_.lookup_transform_timeout_sec, tf_buffer_,
        // 实际类型是 CollatedTrajectoryBuilder
        map_builder_->GetTrajectoryBuilder(trajectory_id)  );
// 轨迹相关的配置保存到容器对象trajectory_options_中并检查后，返回刚刚生成的索引trajectory_id
auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id,
    const ::cartographer::common::Time time,  // 更新子图的时间
    const Rigid3d local_pose,       // 子图的参考位置
    ::cartographer::sensor::RangeData range_data_in_local,   // 参考位置下的扫描数据
    const std::unique_ptr<const ::cartographer::mapping::
                              TrajectoryBuilderInterface::InsertionResult> )
{
  // LocalSlamData是定义在 MapBuilderBridge 内部的结构体，用于记录局部SLAM反馈的状态
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)} );
  cartographer::common::MutexLocker lock(&mutex_);

  /* std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
      trajectory_state_data_  GUARDED_BY(mutex_);*/
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
// 省略一行：把lua文件的 imu_gravity_time_constant 参数赋给变量 gravity_time_constant
extrapolators_.emplace(
  std::piecewise_construct, std::forward_as_tuple(trajectory_id),
  std::forward_as_tuple(
      ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
      gravity_time_constant)  );

sensor_samplers_.emplace(
  std::piecewise_construct, std::forward_as_tuple(trajectory_id),
  std::forward_as_tuple(
      options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
      options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
      options.landmarks_sampling_ratio)  );