problem->AddResidualBlock(
    LandmarkCostFunction2D::CreateAutoDiffCostFunction(
        observation, prev->data, next->data),

    new ceres::HuberLoss(huber_scale), 
    prev_node_pose->data(),  next_node_pose->data(), 
    C_landmarks->at(landmark_id).rotation(),
    C_landmarks->at(landmark_id).translation() );

  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const LandmarkObservation& observation, const NodeSpec2D& prev_node,
      const NodeSpec2D& next_node)
{
    return new ceres::AutoDiffCostFunction<
        LandmarkCostFunction2D,
        6 /* residuals，operator() return的是6维向量 */,
                    // 所估计变量的维度
        3 /* previous node pose variables (x,y,theta) */, 
        3 /* next node pose variables  (x,y,theta)  */,
        4 /* landmark rotation variables  */,
        3 /* landmark translation variables */>(
        new LandmarkCostFunction2D(observation, prev_node, next_node));
}

  template <typename T>
  bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
                  const T* const landmark_rotation,
                  const T* const landmark_translation,
                  T* const e) const
{
    const std::tuple<std::array<T, 4>, std::array<T, 3>>
        interpolated_rotation_and_translation = InterpolateNodes2D(
            prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
            next_node_gravity_alignment_, interpolation_parameter_);

    const std::array<T, 6> error = ScaleError(
        ComputeUnscaledError(
            landmark_to_tracking_transform_,
            std::get<0>(interpolated_rotation_and_translation).data(),
            std::get<1>(interpolated_rotation_and_translation).data(),
            landmark_rotation, landmark_translation),
        translation_weight_, rotation_weight_);

    std::copy(std::begin(error), std::end(error), e);
    return true;
}

template <typename T>
static std::array<T, 6> ComputeUnscaledError(
    const transform::Rigid3d&  relative_pose, 
    const T* const start_rotation,  const T* const start_translation, 
    const T* const end_rotation,    const T* const end_translation )
{
	// 四元数共轭，在这里就是逆
  const Eigen::Quaternion<T> R_i_inverse( start_rotation[0],
                                         -start_rotation[1],
                                         -start_rotation[2],
                                         -start_rotation[3] );
    // 平移的差
  const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
                                     end_translation[1] - start_translation[1],
                                     end_translation[2] - start_translation[2]);
  const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;

  const Eigen::Quaternion<T> h_rotation_inverse =
      Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], 
                          -end_rotation[2], -end_rotation[3]) *
      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
                           start_rotation[2], start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> angle_axis_difference =
      transform::RotationQuaternionToAngleAxisVector(
          h_rotation_inverse * relative_pose.rotation().cast<T>());

  return {  {T(relative_pose.translation().x()) - h_translation[0],
             T(relative_pose.translation().y()) - h_translation[1],
             T(relative_pose.translation().z()) - h_translation[2],
           angle_axis_difference[0], 
           angle_axis_difference[1],
           angle_axis_difference[2]}  };
}

template <typename T>
std::array<T, 3> ScaleError( const std::array<T, 3>& error,
                             double translation_weight,
                             double rotation_weight )
{
  return {{
      error[0] * translation_weight,
      error[1] * translation_weight,
      error[2] * rotation_weight
  }};
}

struct LandmarkNode {
    struct LandmarkObservation {
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    // 同一时刻，可能观测到多个landmark数据
    std::vector<LandmarkObservation> landmark_observations;
    absl::optional<transform::Rigid3d> global_landmark_pose;
    bool frozen = false;
  };

void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data)
{
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      for (const auto& observation : landmark_data.landmark_observations)
      {
      	          			// initial landmark_nodes
          // const transform::Rigid3d global_pose =
          // observation.landmark_to_map_transform;
      #if 0
      data_.landmark_nodes的类型是 std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      #endif
        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
            PoseGraphInterface::LandmarkNode::LandmarkObservation{
                trajectory_id, landmark_data.time,
                observation.landmark_to_tracking_transform,
                observation.translation_weight, observation.rotation_weight} );

          // data_.landmark_nodes[observation.id].global_landmark_pose = global_pose;
      }
    }
    // optimization_problem_->new_landmark_add_ = true;
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

  std::set<int> frozen_trajectories;
  for (const auto& it : trajectories_state)
  {
    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN)
    {
      frozen_trajectories.insert(it.first);
    }
  }

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  // Set the starting point.  TODO(hrapp): Move ceres data into SubmapSpec.
  // ceres需要的是double指针，std::array 能转成原始指针的形式
  MapById<SubmapId, std::array<double, 3>> C_submaps;
  MapById<NodeId, std::array<double, 3>> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;
  bool first_submap = true;


  // 将需要优化的  子图位姿  设置为优化参数
  for (const auto& submap_id_data : submap_data_) {
    // submap_id的轨迹是否是冻结的
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
        // 将子图的global_pose放入 C_submaps
    C_submaps.Insert(submap_id_data.id,
                     FromPose(submap_id_data.data.global_pose));
    // c++11中，std::array::data()返回指向数组对象第一个元素的指针
    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);

    // 第一个子图或冻结，不优化子图位姿。 也就是不优化初值
    if (first_submap || frozen) {
      first_submap = false;
      // Fix the pose of the first submap or all submaps of a frozen
      // trajectory.
      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
    }
  }

  // 需要优化的  节点位姿  设置为优化参数，与上面的子图优化大致相同
  for (const auto& node_id_data : node_data_)
  {
    const bool frozen =
        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
    if (frozen)
    {
      // 这里的第一个节点也要参与优化，跟子图的不同了
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
    }
  }
				/* ...... */
// 第2种残差：landmark与landmark数据插值出来的节点相对位姿的差值
AddLandmarkCostFunctions(landmark_nodes, node_data_, 
						&C_nodes, &C_landmarks,
                        &problem, options_.huber_scale()  );

const std::string& landmark_id = landmark_node.first;
// 轨迹中第一个 node_data
const auto& begin_of_trajectory =
    node_data.BeginOfTrajectory(observation.trajectory_id);
// 如果 landmark observation was made before the trajectory was created
if (observation.time < begin_of_trajectory->data.time) {
    continue;
}
/* 以下语句Find the trajectory nodes before and after the landmark observation */

// 找到在landmark观测时间后的第一个节点
auto next = 
    node_data.lower_bound(observation.trajectory_id, observation.time);
/*  The landmark observation was made, but the next trajectory node has
    not been added yet. 即next已经是轨迹最后一个节点  */
if (next == node_data.EndOfTrajectory(observation.trajectory_id)  ) {
    continue;
}
// 如果是刚开始的node data
if (next == begin_of_trajectory) {
  next = std::next(next);
}
// 这里的pre配合next是为了获取两个位置，找到landmark观测时间的前一个节点
auto prev = std::prev(next);

// 根据两个索引，获取两个节点位姿
// Add parameter blocks for the landmark ID if they were not added before
std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);

// 如果landmark_id 不存在于 C_landmarks，
if (!C_landmarks->count(landmark_id)){
  // landmark如果已经优化，则 global_landmark_pose 有值
  // 否则根据时间插值计算一个位姿
  const transform::Rigid3d starting_point =
      landmark_node.second.global_landmark_pose.has_value()
          ? landmark_node.second.global_landmark_pose.value()
          : GetInitialLandmarkPose(observation, prev->data, next->data,
                                   *prev_node_pose, *next_node_pose);

  C_landmarks->emplace(
      landmark_id,
      // 四元数不支持广义加法，使用QuaternionParameterization 
      CeresPose(starting_point, nullptr /* translation_parametrization */,
                absl::make_unique<ceres::QuaternionParameterization>(),
                problem)  );

  // Set landmark constant if it is frozen. 不参与优化
  // 这里容易记不清，frozen 定义在  struct LandmarkNode
  if (landmark_node.second.frozen) {
    problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).translation());

    problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).rotation());
  }
}

problem->AddParameterBlock(data_->translation.data(), 3,
                         translation_parametrization.release());
problem->AddParameterBlock(data_->rotation.data(), 4,
                         rotation_parametrization.release());

problem->AddResidualBlock(
    LandmarkCostFunction2D::CreateAutoDiffCostFunction(
        observation, prev->data, next->data),

    new ceres::HuberLoss(huber_scale), 
    prev_node_pose->data(),  next_node_pose->data(), 
    C_landmarks->at(landmark_id).rotation(),
    C_landmarks->at(landmark_id).translation() );

for (const auto& C_landmark : C_landmarks) {
	// first 是 landmark_id
	landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}

// 遍历成员变量 landmark_data_，它在 Solve 函数的最后赋值
for (const auto& landmark : optimization_problem_->landmark_data() )
{
	 // first 是 landmark_id
  data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}

// landmark_poses_list 话题
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);

void Node::PublishLandmarkPosesList(const ::ros::WallTimerEvent& unused_timer_event)
{
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0)
  {
      absl::MutexLock lock(&mutex_);
      landmark_poses_list_publisher_.publish(map_builder_bridge_.GetLandmarkPosesList() );
  }
}

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;
  CHECK(msg->landmarks.size() != 0) << "No Landmarks !";

  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
}

struct LandmarkData {
  common::Time time;
  std::vector<LandmarkObservation> landmark_observations;
};

struct LandmarkObservation
{
  std::string id;
  transform::Rigid3d landmark_to_tracking_transform;
  double translation_weight;
  double rotation_weight;
};

void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg)
{
  auto landmark_data = ToLandmarkData(*msg);
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
  // 把相机对landmark的观测，转换到 tracking 坐标系
  if (tracking_from_landmark_sensor != nullptr)
  {
      for (auto& observation : landmark_data.landmark_observations)
      {
        observation.landmark_to_tracking_transform =
            *tracking_from_landmark_sensor *
            observation.landmark_to_tracking_transform;
      }
  }
  // 进入传感器数据融合阶段
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

LandmarkData ToLandmarkData(const LandmarkList& landmark_list)
{
    LandmarkData landmark_data;
    landmark_data.time = FromRos(landmark_list.header.stamp);
    for (const LandmarkEntry& entry : landmark_list.landmark)
    {
        landmark_data.landmark_observations.push_back(
        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
         entry.translation_weight, 10, entry.type});
         //LOG(INFO) << "find landmark x: " << entry.tracking_from_landmark_transform.position.x;
         //LOG(INFO) << "find landmark y: " << entry.tracking_from_landmark_transform.position.y;
    }
    return landmark_data;
}

void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
    if (collate_landmarks_)
    {
      AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
      return;
    }
    // wrapped_trajectory_builder_ 赋值在 CollatedTrajectoryBuilder 构造函数
    // 应当为map_builder_->GetTrajectoryBuilder(trajectory_id)
    wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

if (sensor_id.type == SensorId::SensorType::LANDMARK &&
    !collate_landmarks_)
{
    continue;
}

void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
   pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}

 // 至少长于 20 cm, 标定板不能距离激光超过2m, 标定板上的激光点超过 50 个
if(dist.head(2).norm() > 0.2
   && points.at(seg.id_start).head(2).norm() < 2
   && points.at(seg.id_end).head(2).norm() < 2
   && seg.id_end-seg.id_start > 50   ) 
{
    seg.dist = dist.head(2).norm();
    segs.push_back(seg);
}

#include <ros/ros.h>
#include <Eigen/Dense>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>

tf::TransformListener listener;

ros::Rate rate(10.0);
while (node.ok())
{
	//创建一个StampedTransform对象存储变换结果数据
	tf::StampedTransform ab;
	tf::StampedTransform bc;
	tf::StampedTransform tfAC;
	try{
	    listener.lookupTransform("A", "B",
	                             ros::Time(0), ab);
	    listener.lookupTransform("B", "C",
	                             ros::Time(0), bc);
	    listener.lookupTransform("A", "C",
	                             ros::Time(0), tfAC);
	    // 输出 lookupTransform 输出的 C--->A 变换
	    cout << "X: "<< tfAC.getOrigin().getX() << " Y: "<< tfAC.getOrigin().getY()
	            <<"  Z: "<< tfAC.getOrigin().getZ()<< endl;
	    cout << "quaternion: "<< tfAC.getRotation().x() <<" " << tfAC.getRotation().y()<<
	            " " << tfAC.getRotation().z() << " " << tfAC.getRotation().w() << endl;

    Eigen::Matrix4f AB, BC, AC;
    // 使用 pcl_ros 直接得到 4x4的齐次变换矩阵
    pcl_ros::transformAsMatrix(ab, AB);   
    pcl_ros::transformAsMatrix(bc, BC);
    std::cout << "AB:"  <<std::endl;
    std::cout << AB  <<std::endl;
    std::cout << "BC:"  <<std::endl;
    std::cout << BC  <<std::endl;
    std::cout << "AC:"  <<std::endl;
    AC = AB * BC;
    std::cout << AC  <<std::endl;
    cout << "-------------------------------------" << endl;
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
  }
}

// Converts an Eigen Quaternion into a tf Matrix3x3.
void  tf::matrixEigenToTF (const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
  // Converts a tf Matrix3x3 into an Eigen Quaternion.
void  tf::matrixTFToEigen (const tf::Matrix3x3 &t, Eigen::Matrix3d &e)

  // Converts an Eigen Affine3d into a tf Transform.
void  tf::poseEigenToTF (const Eigen::Affine3d &e, tf::Pose &t)
  // Converts a tf Pose into an Eigen Affine3d.
void  tf::poseTFToEigen (const tf::Pose &t, Eigen::Affine3d &e)
  
// Converts an Eigen Quaternion into a tf Quaternion.
void  tf::quaternionEigenToTF (const Eigen::Quaterniond &e, tf::Quaternion &t)
  // Converts a tf Quaternion into an Eigen Quaternion.
void  tf::quaternionTFToEigen (const tf::Quaternion &t, Eigen::Quaterniond &e)
  
  // Converts an Eigen Affine3d into a tf Transform.
void  tf::transformEigenToTF (const Eigen::Affine3d &e, tf::Transform &t)
  // Converts a tf Transform into an Eigen Affine3d.
void  tf::transformTFToEigen (const tf::Transform &t, Eigen::Affine3d &e)

  // Converts an Eigen Vector3d into a tf Vector3.
void  tf::vectorEigenToTF (const Eigen::Vector3d &e, tf::Vector3 &t)
   // Converts a tf Vector3 into an Eigen Vector3d.
void  tf::vectorTFToEigen (const tf::Vector3 &t, Eigen::Vector3d &e)

// 模板参数依次为： number of residuals,  first parameter的维度
class MyCostFun : public SizedCostFunction<1, 1, 1, 1>
{
    public:
		MyCostFun(double x, double y):
			m_x(x), m_y(y){}
   virtual bool Evaluate(double const* const* parameters,
		      	 double* residuals,
			 double** jacobians) const
   {
       double a = parameters[0][0];
       double b = parameters[0][1];
       double c = parameters[0][2];
       residuals[0] = m_y - exp(a*m_x*m_x + b*m_x + c);

		if (jacobians != NULL && jacobians[0] != NULL)
		{
		    jacobians[0][0] = -exp(a*m_x*m_x + b*m_x + c)* m_x * m_x;
		    jacobians[0][1] = -exp(a*m_x*m_x + b*m_x + c)* m_x;
		    jacobians[0][2] = -exp(a*m_x*m_x + b*m_x + c);

		}
		return true;
   }
   protected:
	double m_x;
    double m_y;
};

int main(int argv, char** argc)
{
    ros::init(argv, argc, "test_ceres");
    ros::NodeHandle nh;
    // 真实值
	double a = 1.0, b = 2.0, c = 1.0;

	// 生成带有噪声的模拟数据
	vector<double> x_data, y_data;
	int N = 240;  // 数据数量
	double w_sigma = 1.0;  // 高斯标准差
	cv::RNG  rng;
	for(unsigned int i =0; i<N; i++)
	{
		double x = i/100.0;
		x_data.push_back(x);
		y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
	}
	// 作为初值
	a = 0.0;
	b = 0.0;
	c = 0.0;
	Problem problem;
	for(unsigned int i=0; i<N; i++)
	{
		CostFunction* cost_func = new MyCostFun(x_data[i], y_data[i]);
		problem.AddResidualBlock(cost_func, nullptr, &a, &b, &c);
	}
	Solver::Options option;
	option.minimizer_progress_to_stdout = true;

	Solver::Summary summary;
	Solve(option, &problem, &summary);
	cout << summary.BriefReport() << endl <<endl;
	cout << "a: "<< a <<"  b: "<< b << "  c: "<< c <<endl;

    return 0;
}

set_num_residuals(kNumResiduals);
*mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};

class CERES_EXPORT QuaternionParameterization : public LocalParameterization
{
 public:
  virtual ~QuaternionParameterization() {}

  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;

  // 表示参数的自由度，也就是 Manifold 空间（四元数）是 4 维的
  // 即四元数本身的实际维数，如果是旋转矩阵，则为 9
  virtual int GlobalSize() const { return 4; }
  // 表示迭代增量所在的切空间(tangent space)是 3 维的。即旋转矢量的实际维数
  virtual int LocalSize() const { return 3; }
};

bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const
{
	/* -------- 将旋转矢量转换为四元数形式-------- */
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0)
  {
  	// 这里的 除以norm_delta 相当于归一化
  	const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
  	double q_delta[4];
  	q_delta[0] = cos(norm_delta);
  	q_delta[1] = sin_delta_by_delta * delta[0];
  	q_delta[2] = sin_delta_by_delta * delta[1];
  	q_delta[3] = sin_delta_by_delta * delta[2];

  	/* --------2.采用四元数乘法更新-------- */
  	// 注意这个乘法的顺序关乎求雅克比矩阵，具体说明参照 ComputeJacobian
  	x_plus_delta = q_delta * x;
  }
  else
  {
    for (int i = 0; i < 4; ++i)
        x_plus_delta[i] = x[i];
  }
  return true;
}

// x[0] 对应 w
bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const
{
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}

ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param) //重构参数，优化时实际使用的是3维的等效旋转矢量