// pose_estimate_2d 是 scanMatch返回的
 // ceres计算的位姿需要再跟重力方向对齐，才是LocalSLAM的位姿
 const transform::Rigid3d pose_estimate = transform::Embed3D(*pose_estimate_2d) *
 							gravity_alignment;
 // 对应流程图，反馈给 extrapolator_ 以修正估计值
 // 添加到PoseExtrapolator的timed_pose_queue_队列
 extrapolator_->AddPose(time, pose_estimate);

             // 下面使用的还是ceres返回的位姿
  // 把点云数据转化为在 local map 坐标系 中的点云数据
  /* 注意这里的点云 range_data_in_local 来自 gravity_aligned_range_data
   后者是没有经过voxel filter的点云，否则加入地图的点就太少了 */
 // gravity_aligned_range_data的 origin,return,miss都左乘 pose_estimate_2d
 sensor::RangeData  range_data_in_local =
     TransformRangeData( gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()) );
 // 更新Submap, 同时返回插入结果  将LocalSLAM坐标系下的点云插入 active_submap
 // filtered_gravity_aligned_point_cloud 实际没参与 插入子图，只作为insert result的成员返回
 std::unique_ptr<InsertionResult>  insertion_result = InsertIntoSubmap(
     time,  range_data_in_local,  filtered_gravity_aligned_point_cloud,
     pose_estimate,    gravity_alignment.rotation()  );
 // ......时间处理部分

 // 传入后端
 return absl::make_unique<MatchingResult>(
     MatchingResult{ time, pose_estimate, std::move(range_data_in_local),
            std::move(insertion_result)  }  );

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;

/* 将前端的输出结果喂给后端进行闭环检测和全局优化  */
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_);

  // 重新封装前端传来的 InsertionResult，增加了一个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()  )  }  );
}

NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
{
  // 将点云的原点坐标从local map坐标系转换为global map坐标系
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
  // 添加轨迹节点  子图节点
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
  // 查看旧子图是否插入完成（两个submap，front用于匹配和更新的）
  // 如果 finished，则需要进行闭环检测，完成会调用 Submap2D::Finish()
  const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();
   // 添加计算约束的任务，加入 work_queue 队列，实际在DrainWorkQueue函数执行
  AddWorkItem( [=]() LOCKS_EXCLUDED(mutex_)
  {
    return  ComputeConstraintsForNode(node_id, insertion_submaps,
                                      newly_finished_submap);
  }  );
  return node_id;
}

transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
    const int trajectory_id) const
{
  absl::MutexLock locker(&mutex_);
  
  return ComputeLocalToGlobalTransform(
    data_.global_submap_poses_2d,
    trajectory_id  );
}

transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
    const MapById<SubmapId, optimization::SubmapSpec2D>&  global_submap_poses,
    const int trajectory_id) const
{
  // 查找已经优化过的trajectory id中的submap的位置，如果有可返回存储的值
  auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
  auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
  if (begin_it == end_it)
  {
    const auto it = data_.initial_trajectory_poses.find(trajectory_id);
    // 位姿插值
    if (it != data_.initial_trajectory_poses.end())
    {
      return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *
             it->second.relative_pose;
    }
    // 仅是唯一个，表明为初始submap，转移矩阵为单位矩阵
    else
        return transform::Rigid3d::Identity();
  }
  const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
   // 如果不存在，则返回由上个优化后的 submap_id 的位置的相对位置换算出的位置
  return transform::Embed3D(
             global_submap_poses.at(last_optimized_submap_id).global_pose) *
         data_.submap_data.at(last_optimized_submap_id)
             .submap->local_pose()
             .inverse();
}

Vector translation_;
Quaternion rotation_;
// using Vector = Eigen::Matrix<FloatType, 3, 1>;
// using Quaternion = Eigen::Quaternion<FloatType>;
// using AngleAxis = Eigen::AngleAxis<FloatType>;

// 参见 SLAM 十四讲
Rigid3 inverse() const
{
  const Quaternion rotation = rotation_.conjugate();
  const Vector translation = -(rotation * translation_);
  return Rigid3(translation, rotation);
}

template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

struct NodeSpec2D
{
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment;
};

struct SubmapSpec2D
{
  transform::Rigid2d global_pose;
};
// optimization_problem_->submap_data()  就是 SubmapSpec2D
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;

struct PoseGraphData
{
  /* struct InternalSubmapData
  {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;

  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
    std::set<NodeId>  node_ids;
  }; */

  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.
  MapById<SubmapId, InternalSubmapData> submap_data;

  // Global submap poses currently used for displaying data.
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

  // Data that are currently being shown.
  MapById<NodeId, TrajectoryNode> trajectory_nodes;

  // Global landmark poses with all observations.
  std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      landmark_nodes;
  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;
  int num_trajectory_nodes = 0;
  std::map<int, InternalTrajectoryState> trajectories_state;

  // Set of all initial trajectory poses.
  std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

  std::vector<PoseGraphInterface::Constraint> constraints;
};

struct InternalSubmapData
{
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;
  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
  std::set<NodeId> node_ids;
};

struct Constraint {
  struct Pose {
    transform::Rigid3d zbar_ij;
    double translation_weight;
    double rotation_weight;
  };
  SubmapId submap_id;  // 'i' in the paper.
  NodeId node_id;      // 'j' in the paper.
  // Pose of the node 'j' relative to submap 'i'.
  Pose pose;
/*  Differentiates between intra-submap (where node 'j' was inserted into
  submap 'i') and inter-submap constraints (where node 'j' was not inserted
  into submap 'i').*/
  enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};

class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d>
{
public:
    void Solve(
      const std::vector<Constraint>& constraints,
      const std::map<int, PoseGraphInterface::TrajectoryState>&
          trajectories_state,
      const std::map<std::string, LandmarkNode>& landmark_nodes) override;
    // 省略很多函数
private:
  optimization::proto::OptimizationProblemOptions  options_;
  MapById<NodeId, NodeSpec2D>  node_data_;
  MapById<SubmapId, SubmapSpec2D> submap_data_;
  sensor::MapByTime<sensor::ImuData> imu_data_;
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;

  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
  std::map<std::string, transform::Rigid3d> landmark_data_;
};

// 结果、时间、函数
struct WorkItem
{
  enum class Result {
    kDoNotRunOptimization,
    kRunOptimization,
  };

  std::chrono::steady_clock::time_point time;
  std::function<Result()> task;
};

using WorkQueue = std::deque<WorkItem>;

std::deque<TimedPose> timed_pose_queue_;
std::deque<sensor::ImuData> imu_data_;
std::deque<sensor::OdometryData> odometry_data_;

// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance
// It is assumed that between the 'origin' and 'misses' is free space
struct RangeData {
  Eigen::Vector3f  origin;
  PointCloud  returns;
  PointCloud  misses;
};

using PointCloud = std::vector<RangefinderPoint>;
// Stores 3D position of a point observed by a rangefinder sensor.
struct RangefinderPoint
{
	Eigen::Vector3f position;
};

struct TimedPointCloudData {
  common::Time time;
  Eigen::Vector3f origin;
  TimedPointCloud ranges;
};

struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;
    size_t origin_index;
  };
  common::Time time;
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges;
};

// Like 'RangeData', but with 'TimedPointClouds'.
struct TimedRangeData {
  Eigen::Vector3f origin;
  TimedPointCloud returns;
  TimedPointCloud misses;
};

using TimedPointCloud = std::vector<TimedRangefinderPoint>;

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

struct SubmapPose
{
	int version;
	transform::Rigid3d  pose;
};

struct SubmapData
{
	std::shared_ptr<const Submap> submap;
	transform::Rigid3d  pose;
};

struct MatchingResult
{
    common::Time time;    // 扫描匹配发生的时间
    // tracking 坐标系下的位姿(Local SLAM坐标系下的位姿), 需要乘以
    // tracking frame in map的转换(Local SLAM坐标系到Global SLAM坐标系的转换)才能转到全局坐标系
    transform::Rigid3d local_pose;
    // tracking 坐标系下的点云（即LocalSLAM坐标系下的点云）
    sensor::RangeData range_data_in_local;
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
};

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

struct TrajectoryNode
{
  struct Data
  {
    common::Time  time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond  gravity_alignment;
    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud  filtered_gravity_aligned_point_cloud;
    // 省略 3D.

    // The node pose in the local SLAM frame.
    transform::Rigid3d   local_pose;
  };
  common::Time time() const { return constant_data->time; }
  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;
  // The node pose in the global SLAM frame.
  transform::Rigid3d  global_pose;
};

// An individual submap, which has a 'local_pose' in the local map frame, 
// keeps track of how many range data were inserted into it
// and sets 'insertion_finished' when the map no longer changes 
// and is ready for loop closing
class Submap
{
public:
	Submap(const transform::Rigid3d& local_submap_pose)
	  : local_pose_(local_submap_pose) {}

private:
	// Pose of this submap in the local map frame
	const transform::Rigid3d  local_pose_;
	// Number of RangeData inserted.
	int num_range_data_ = 0;
	bool insertion_finished_ = false;
};

// 对点云中每一个点做变换， 左乘 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;
}

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

  // 插入集合
sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data);

private:
  // 融合
  sensor::TimedPointCloudOriginData CropAndMerge();
  // 期望处理传感器类型清单
  const std::set<std::string> expected_sensor_ids_;
  // 同步和融合后集合，每种传感器至多一帧点云数据
  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;

sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& timed_point_cloud_data)
{
  // 检测该点云数据 sensor_id 是否在期望的sensor_ids里面
  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
  // TODO(gaschler): These two cases can probably be one.
  // 此传感器类型数据已有
  if (id_to_pending_data_.count(sensor_id) != 0) 
  {
    current_start_ = current_end_;
    // When we have two messages of the same sensor, move forward the older of
    // the two (do not send out current).
    /* 如果当前融合时间段内融合结果中已包含同一ID的传感器数据，则应采用最新的点云数据进行替换
    但是结束的时间戳仍然采用原来的时间刻度，开始进行数据截取合并CropAndMerge()并返回 */
    // std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
    current_end_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();
    id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
    return result;
  }
  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
  // 若现在收到的数据类型未全，即期望收到种类未全，直接退出，无需融合
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }
  current_start_ = current_end_;
  // We have messages from all sensors, move forward to oldest.
  common::Time oldest_timestamp = common::Time::max();
  // 找传感器数据中最早的时间戳
  for (const auto& pair : id_to_pending_data_) {
    oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
  }
    /*  current_end_是下次融合的开始时间，是本次融合的最后时间刻度
  但其实是此次融合所有传感器中最早的时间戳*/
  current_end_ = oldest_timestamp;
  return CropAndMerge();
}

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);
// 省略:  统计残差， 观测值 - 预测值
return  pose_observation;

/*
 input:
 1. 上一个 scan 的位姿 target_translation
 2. 经过 RealTimeCorrelativeScanMatcher2D 处理过的
     当前的 scan 的位姿的初始估计  initial_pose_estimate
 3. 当前 scan 点云（2D）  point_cloud
 4. local map 概率分布栅格图   grid

 output:  更新的位姿估计  pose_estimate
*/
void CeresScanMatcher2D::Match(
   const Eigen::Vector2d&   target_translation,
   const transform::Rigid2d&  initial_pose_estimate,
   const sensor::PointCloud&  point_cloud,
   const Grid2D&  grid,
   transform::Rigid2d* const  pose_estimate,
   ceres::Solver::Summary* const  summary) const
{
	// 优化的变量（3维）
  double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                   initial_pose_estimate.translation().y(),
                                   initial_pose_estimate.rotation().angle() };
  ceres::Problem  problem;
  CHECK_GT(options_.occupied_space_weight(), 0.);
    // 点云匹配残差块
  problem.AddResidualBlock(
      CreateOccupiedSpaceCostFunction2D(
          options_.occupied_space_weight() /
              std::sqrt(static_cast<double>(point_cloud.size())),
          point_cloud,   grid ),
      nullptr /* loss function */, ceres_pose_estimate );

    // 省略 GridType::TSDF
  CHECK_GT(options_.translation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单   AutoDiff
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation),
      nullptr, ceres_pose_estimate);

  CHECK_GT(options_.rotation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单
  problem.AddResidualBlock(
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.rotation_weight(), ceres_pose_estimate[2]),
      nullptr, ceres_pose_estimate);
   // ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
  ceres::Solve(ceres_solver_options_, &problem, summary);

  *pose_estimate = transform::Rigid2d(
      {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
  {
    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
                             2 /* residuals */,
                             3 /* pose variables */>(
        new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
  }
  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[0] - x_);
    residual[1] = scaling_factor_ * (pose[1] - y_);
    return true;
  }
 private:
  // Constructs a new TranslationDeltaCostFunctor2D  from the given
  // 'target_translation' (x, y).
  explicit TranslationDeltaCostFunctor2D(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
      : scaling_factor_(scaling_factor),
        x_(target_translation.x()),
        y_(target_translation.y()) {}
  // 省略: 禁止拷贝构造函数 和 赋值运算符
  const double scaling_factor_;
  const double x_;
  const double y_;
};

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

if (gravity_aligned_range_data.returns.empty())
{
  LOG(WARNING) << "Dropped empty horizontal range data.";
  return nullptr;
}
const sensor::PointCloud&  filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);

if (filtered_gravity_aligned_point_cloud.empty())
    return nullptr;

const transform::Rigid3d  non_gravity_aligned_pose_prediction =
  extrapolator_->ExtrapolatePose(time);
  // 经过重力方向计算投影后的2d位置
const transform::Rigid2d   pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse() );

pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

transform::Rigid3d pose_estimate =
     transform::Embed3D(*pose_estimate_2d) * gravity_alignment;

// local map frame <--- gravity-aligned frame
// 求 Scan 插入Submap的最优 Pose
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
    ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr)
{
  LOG(WARNING) << "Scan matching failed.";
  return nullptr;
}

// 输出是对该帧传感器数据的最优pose
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time,  const transform::Rigid2d&  pose_prediction,
    const sensor::PointCloud&  filtered_gravity_aligned_point_cloud)
{
  // 如果还没有子图，返回 PoseExtrapolator 的预测值
  if (active_submaps_.submaps().empty() )
    return absl::make_unique<transform::Rigid2d>(pose_prediction);

  // 永远是头部的子图做匹配   SCAN-to-MAP
  std::shared_ptr<const Submap2D> matching_submap =  
      active_submaps_.submaps().front();

  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 函数最终更新 initial_ceres_pose，返回匹配的最高分数
  if (options_.use_online_correlative_scan_matching())
  {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction,   filtered_gravity_aligned_point_cloud,
        // Grid2D类型
        *matching_submap->grid(),  &initial_ceres_pose );
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }
  // ceres scan matcher 部分
}

// 更新 pose_estimate， 返回最高的分数
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d&  initial_pose_estimate,
    const sensor::PointCloud&  point_cloud, 
    const Grid2D&  grid,
    transform::Rigid2d*  pose_estimate  ) const
{
  CHECK(pose_estimate != nullptr);
  // 预测位姿的旋转量
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   /*  左乘 initial_rotation， 将当前点云转换为
  初始角度坐标系下点云，这里只有 旋转  */
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), 
          Eigen::Vector3f::UnitZ()  )   )
      );

  const SearchParameters search_parameters(
      options_.linear_search_window(),  options_.angular_search_window(),
      rotated_point_cloud,   grid.limits().resolution()  );
	// 获取搜索窗口下机器人朝向各个方向角时的点云数据

  // 先通过角度切片，然后在xy方向上暴力搜索，为了减少计算量
  // 切片数目 num_scans , 按照搜索角度/步长，以初始值为基础，生成一些列的旋转后的点云
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

     // DiscreteScan2D 就是 std::vector<Eigen::Array2i>
     /* 把切片后的旋转点云坐标，左乘initial_pose_estimate.translation
       转化到地图坐标，这里只有 平移
    最终通过GetCellIndex返回一系列的  激光点return所在的cell index*/
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(),   rotated_scans,
      Eigen::Translation2f( initial_pose_estimate.translation().x(),
      initial_pose_estimate.translation().y()  )
      );
   /*  获得candidates, size是 search_parameters.num_scans * 
  (linear_bounds.max_x - inear_bounds.min_x) * (linear_bounds.max_y - inear_bounds.min_y)*/

    /*  这里是num_scans循环，x和y循环，没有 theta循环了
  成员是 scan_index, x_index_offset(逐步增加), y_index_offset(逐步增加), 
  search_parameters意思其实是 每个激光点附近的矩形区域*/
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

  // 计算当前帧在theta、x、y三层窗口中，每个状态在grid地图中的匹配的评分
  // search_parameters 实际没用
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
  // 取最好的候选
  const Candidate2D&   best_candidate =
      *std::max_element(candidates.begin(), candidates.end());

  // 最后的结果是预测值加候选位姿
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y },
       // 前端观测，所以右乘
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation) );

  return best_candidate.score;
}

SearchParameters::SearchParameters(
               const double linear_search_window,
               const double angular_search_window,
               const sensor::PointCloud& point_cloud,
               const double resolution)
    : resolution(resolution)
{
  // We set this value to something on the order of 
  // resolution to make sure that the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }
  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  // 这两者的计算不同
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  // 切片数目，由于resolution不修改，实际取决于 max_scan_range 和 angular_search_window
  num_scans = 2 * num_angular_perturbations + 1;
  /*  Linear search window in pixel offsets; bounds are inclusive.
  struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };
  std::vector<LinearBounds> linear_bounds;   Per rotated scans  */
  linear_bounds.reserve(num_scans);
  // 对同一帧scan, linear_bounds的元素都相同，不同帧就不同了
  for (int i = 0; i != num_scans; ++i) {
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations} );
  }
}

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud&  point_cloud,
    const SearchParameters&  search_parameters)
{
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);
  // 旋转角度步长
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  //  个数 num_scans
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
       delta_theta += search_parameters.angular_perturbation_step_size)
  {
    // 左乘旋转向量，旋转轴为z轴
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()) ) )  );
  }
  return rotated_scans;
}

void ScoreCandidates(
    const Grid2D& grid,  const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters&  search_parameters,
    std::vector<Candidate2D>*  const candidates) const
{
  for (Candidate2D& candidate : *candidates)
  {
    candidate.score = ComputeCandidateScore(
        static_cast<const ProbabilityGrid&>(grid),
        discrete_scans[candidate.scan_index],
        candidate.x_index_offset,
        candidate.y_index_offset);
    break;
      // 省略 GridType::TSDF
      // 进一步做权重处理
    candidate.score *=
        std::exp(
          -common::Pow2(
            std::hypot(candidate.x, candidate.y) *
            options_.translation_delta_cost_weight() 
                        +
            std::abs(candidate.orientation) *
            options_.rotation_delta_cost_weight()
                       )
          );
  }
}


// 其中用到的函数， 计算每个候选的得分
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
             const DiscreteScan2D&  discrete_scan,
             int x_index_offset,  int y_index_offset)
{
  float candidate_score = 0.f;
  // 这里就用到前面获取的x y 索引队列
  for (const Eigen::Array2i& xy_index : discrete_scan)
  {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset  );
    // Returns probability of the cell with  'cell_index'
    // 这个就是占据概率
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  // 返回的是一帧点云所有激光点得分的平均值
  candidate_score /= static_cast<float>(discrete_scan.size());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh)
{
  std::string model_name; 
  if (!nh.getParam("footprint_model/type", model_name))
  {
	// 如果未指定类型，则使用点类型
    return boost::make_shared<PointRobotFootprint>();
  }
  // point  
  if (model_name.compare("point") == 0)
  {
      ROS_INFO("Footprint model 'point' loaded for trajectory optimization.");
      return  boost::make_shared<PointRobotFootprint>();
  }
  // circular   省略
  if (model_name.compare("circular") == 0)
  // line  省略
  if (model_name.compare("line") == 0)
  // two circles 省略
  if (model_name.compare("two_circles") == 0)

  // polygon  只对 Point2dContainer vertices_; 赋值
  if (model_name.compare("polygon") == 0)
  {
    // check parameters
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) )
    {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for 
      	trajectory optimization, since param '" << nh.getNamespace() << 
      	"/footprint_model/vertices' does not exist. Using point-model instead");
        return boost::make_shared<PointRobotFootprint>();
    }
    // get vertices
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      try
      {
        Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, 
        	"/footprint_model/vertices");
        ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization.");
        return boost::make_shared<PolygonRobotFootprint>(polygon);
      } 
      catch(const std::exception& ex)
      {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: "
        	<< ex.what() << ". Using point-model instead.");
        return boost::make_shared<PointRobotFootprint>();
      }
    }
    else
    {
      ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, 
      	since param '" << nh.getNamespace() << "/footprint_model/vertices' does not 
      	define an array of coordinates. Using point-model instead.");
      return boost::make_shared<PointRobotFootprint>();
    }
  }
  return boost::make_shared<PointRobotFootprint>();
}

// TebLocalPlannerROS::initialize中的调用
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, 
    double costmap_inscribed_radius, double min_obst_dist)
{}

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons

void workerCallback(const ros::TimerEvent&)
{
  updateCostmap2D();
  compute();
}


void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
	occupied_cells_.clear();
	if (!costmap_->getMutex())
	{
		ROS_ERROR("Cannot update costmap since the mutex pointer is null");
		return;
	}
	int idx = 0;
	costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
	    
	// get indices of obstacle cells
	for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
	{
	for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
	{
	  int value = costmap_->getCost(i,j);
	  if(value >= costmap_2d::LETHAL_OBSTACLE)
	  {
	    double x, y;
	    costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
	    occupied_cells_.push_back( KeyPoint( x, y ) );
	  }
	  ++idx;
	}
	}
}