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

::ros::init(argc, argv, "cartographer_node");
::ros::start();

cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();

constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
// 从lua文件加载配置
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
    LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

// 核心变量： Cartographer的地图构建器
auto map_builder =
    cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
        node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer);
// 加载现有地图的情况
if (!FLAGS_load_state_filename.empty()) {
  node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
  // 核心函数
if (FLAGS_start_trajectory_with_default_topics) {
  node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();

node.FinishAllTrajectories();
node.RunFinalOptimization();

if (!FLAGS_save_state_filename.empty()) {
  node.SerializeState(FLAGS_save_state_filename);
}

Publisher ros::NodeHandle::advertise(const std::string& topic,
	uint32_t 	queue_size,
	const  SubscriberStatusCallback & 	connect_cb,
	const  SubscriberStatusCallback & 	disconnect_cb = SubscriberStatusCallback(),
	const  VoidConstPtr & 	tracked_object = VoidConstPtr(),
	bool   latch = false 
)

void connectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("Subscriber Name: %s", singlePub.getSubscriberName().c_str());
}
void disconnectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("disconnectCb");
}
......
  ros::Publisher pub = nh.advertise<std_msgs::Int8>("topic", 50, connectCb,disconnectCb);

pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, 
	boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), 
	boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1)
	);

if (!sub_ && pub_.getNumSubscribers() > 0)
{
	ROS_DEBUG("Connecting to depth topic.");
	image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
	sub_ = it_.subscribeCamera("/camera/depth/image_raw", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
}

sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg);
    pub_.publish(scan_msg);

void Node::HandleLandmarkMessage(
    const int trajectory_id,  const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr&  msg)
{
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse())
      return;

  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
  // 添加下面代码
  geometry_msgs::TransformStamped stamped_transform;

  stamped_transform.header.stamp = ros::Time::now();
  stamped_transform.header.frame_id = "base_link";
  stamped_transform.child_frame_id = "QR_Code";
  stamped_transform.transform =  ToGeometryMsgTransform(ToRigid3d(msg->landmarks[0].tracking_from_landmark_transform).inverse() );

  tf_broadcaster_.sendTransform(stamped_transform);
}

void Node::PublishLandmarkPosesList(
    const ::ros::WallTimerEvent& unused_timer_event)
{
   // landmark话题必须有订阅者，一般是rviz
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0)
  {
    absl::MutexLock lock(&mutex_);
    landmark_poses_list_publisher_.publish(
        map_builder_bridge_.GetLandmarkPosesList() );
  }
}

visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList()
{
  visualization_msgs::MarkerArray landmark_poses_list;
  const std::map<std::string, Rigid3d> landmark_poses =
      map_builder_->pose_graph()->GetLandmarkPoses();
  LOG(INFO)<<" landmark pose size: "<< landmark_poses.size();

  for (const auto& id_to_pose : landmark_poses) {
    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
        id_to_pose.second, node_options_.map_frame));
  }
  return landmark_poses_list;
}

// #include <ros/ros.h>
// #include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>

ros::init(argc, argv, "tf_node");
ros::NodeHandle nh;
tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
ros::Publisher pub = nh.advertise<geometry_msgs::TransformStamped>("transform",10);
ros::Rate rate(400);
while(nh.ok())
{
    geometry_msgs::TransformStamped trans;
    try {
        ros::Time now = ros::Time::now();
       // if(buff.canTransform("base_footprint","imu",now,
        // ros::Duration(0.03),NULL)
        // 30毫秒应该足够了
        trans = buff.lookupTransform("base_footprint",
                         "imu", now, 
                         ros::Duration(0.03) );
       // else
           // ROS_WARN("no transform in buffer");
    } catch (tf2::TransformException & ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    pub.publish(trans);
    rate.sleep();
}

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ros::init(argc, argv, "test_tf");
ros::NodeHandle nh;

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
geometry_msgs::TransformStamped transform =
        buff.lookupTransform("parent","child",ros::Time(0),ros::Duration(1));

ROS_INFO("transform from parent to child  x: %f",transform.transform.translation.x);
ROS_INFO("transform from parent to child  y: %f",transform.transform.translation.y);
ROS_INFO("transform from parent to child  z: %f\n",transform.transform.translation.z);

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
// 在 child 坐标系中的点
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

ROS_INFO("point in child: 2, 1, 4\n");
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ROS_INFO("point int parent, x: %f",point_out.point.x);
ROS_INFO("point int parent, y: %f",point_out.point.y);
ROS_INFO("point int parent, z: %f\n",point_out.point.z);

cout<<fixed<<setprecision(2);

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();
cout << "rotation matrix: " <<endl<< rotation <<endl<<endl;

Eigen::Matrix4d m;
m.block(0,0,3,3) = rotation;

Eigen::Vector4d Vcol, Vrow;
Vcol << transform.transform.translation.x,
        transform.transform.translation.y,
        transform.transform.translation.z, 1;
m.col(3) = Vcol;

Vrow << 0,0,0,1;
m.row(3) = Vrow.transpose();
cout << "transform matrix: "<<endl<< m<<endl<<endl;

Eigen::Vector4d point;
point << point_in.point.x,
         point_in.point.y,
         point_in.point.z, 1;

Eigen::Vector4d transformed_point = m * point;
cout << "transformed point: "<<endl<<transformed_point.head(3) << endl;

void tf2::Transform::setIdentity()
{
    // Matrix3x3 m_basis;
    m_basis.setIdentity();   // 单位矩阵
    // Vector3   m_origin;
    m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
}

Transform::operator*(const Transform& t) const
{
    return Transform(m_basis * t.m_basis, 
               (*this)(t.m_origin)  );
}

// 2D的IMU可选，3D的IMU必须有
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) 
{
  expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
}

const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();

costant_velocity = {
  imu_gravity_time_constant = 10
  pose_queue_duration = 0.001
}

double variety_distance = (pose_estimate_2d->translation() - pose_prediction.translation()).norm();