const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
// calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) 计算机器人到障碍物的距离
// current_pose为当前机器人的位姿。
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);

if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
  _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, 
  										cfg_->optim.obstacle_cost_exponent);
}

double vel1 = dist1 / dt1->dt();
double vel2 = dist2 / dt2->dt();
const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon);

const double omega1 = angle_diff1 / dt1->dt();
const double omega2 = angle_diff2 / dt2->dt();
const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);

void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
	_measurement = &vel_start;
}

ROS_ASSERT_MSG(cfg_ && _measurement, 
"You must call setTebConfig() and setInitialVelocity() on EdgeAccelerationStart()");
vel1 = _measurement->linear.x;
omega1 = _measurement->angular.z;

if (start_vel)
  setVelocityStart(*start_vel);
if (free_goal_vel)
  setVelocityGoalFree();
else
  vel_goal_.first = true;

// std::pair<bool, geometry_msgs::Twist>  vel_start_;
// 注意vel_start_ 和 vel_start 不同
void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist& vel_start)
{
  vel_start_.first = true;
  vel_start_.second.linear.x = vel_start.linear.x;
  vel_start_.second.linear.y = vel_start.linear.y;
  vel_start_.second.angular.z = vel_start.angular.z;
}

if (vel_start_.first)
{
  EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
  acceleration_edge->setVertex(0,teb_.PoseVertex(0));
  acceleration_edge->setVertex(1,teb_.PoseVertex(1));
  acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
  // 这里就是上面的 setInitialVelocity, _measurement 就是 vel_start_.second
  acceleration_edge->setInitialVelocity(vel_start_.second);
  acceleration_edge->setInformation(information);
  acceleration_edge->setTebConfig(*cfg_);
  optimizer_->addEdge(acceleration_edge);
}

_jacobianOplus[0].resize(2,2); // conf1
_jacobianOplus[1].resize(2,2); // conf2
_jacobianOplus[2].resize(2,2); // conf3
_jacobianOplus[3].resize(2,1); // deltaT1
_jacobianOplus[4].resize(2,1); // deltaT2
_jacobianOplus[5].resize(2,1); // angle1
_jacobianOplus[6].resize(2,1); // angle2
_jacobianOplus[7].resize(2,1); // angle3

# 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下，作为空函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

_estimate.position() = position;
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;

virtual void setToOriginImpl()
{
    _estimate.setZero();
}

// double 也可以换成 number_t
virtual void oplusImpl( const double* update ) // 更新
{
     // update为增量△m 迭代的时候，估计值 m= m + △m, m是向量（a,b,c）
    _estimate += Eigen::Vector3d(update);  
}

virtual void oplusImpl(const double* update)
{
    _estimate.plus(update);
}

virtual void setToOriginImpl() {    // 重置
    _estimate = SE3Quat();
}

virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    //更新方式 类似于左扰动？
    setEstimate(SE3Quat::exp(update)*estimate());        
}

virtual void computeError() override
{
  // ...
    _error = _measurement - Something;
}
virtual void linearizeOplus() override
{
    _jacobianOplusXi(pos, pos) = something;
    // ...

    /*
    _jocobianOplusXj(pos, pos) = something;
    ...
    */
}      

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>

_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

// 相对 _jacobianOplus[2] 的另一种写法
_jacobianOplusXi(0,0) =
......
_jacobianOplusXj(0,0) = 
......

// vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
// 剔除一些误差大的边
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        optimizer.removeEdge(e12);
        optimizer.removeEdge(e21);
        vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
        vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
        nBad++;
    }
}

// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
if(e->chi2()>5.991 || !e->isDepthPositive())
{
    e->setLevel(1);// 不优化
}

class Parent
{
public:
  virtual std::string getName1(int x)
  {
    return "Parent";
  }

  virtual std::string getName2(int x)
  {
    return "Parent";
  }
};

class Child : public Parent
{
public:
  virtual std::string getName1(double x) override
  {
    return "Sub";
  }
  virtual std::string getName2(int x) const override
  {
    return "Sub";
  }
};

 // 至少长于 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)