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