//==============================================================================
void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform)
{
  if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix())
    return;

  const Eigen::Isometry3d oldTransform = getRelativeTransform();

  FixedFrame::setRelativeTransform(transform);
  dirtyJacobian();
  dirtyJacobianDeriv();

  mRelativeTransformUpdatedSignal.raise(
        this, oldTransform, getRelativeTransform());
}

int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
                  const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
                  std::vector<Contact>* result)
{
  dVector3 halfSize0;
  dVector3 halfSize1;

  convVector(0.5 * size0, halfSize0);
  convVector(0.5 * size1, halfSize1);

  dMatrix3 R0, R1;

  convMatrix(T0, R0);
  convMatrix(T1, R1);

  dVector3 p0;
  dVector3 p1;

  convVector(T0.translation(), p0);
  convVector(T1.translation(), p1);

  return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, *result);
}

const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute pose change
    const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );

    //ds check point
    const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
    double dNorm( vecSamplePoint.norm( ) );
    const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
    dNorm = ( dNorm + vecDifference.norm( ) )/2;

    //ds return norm
    return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}

void VoEstimator::publishPose(int64_t utime, std::string channel, Eigen::Isometry3d pose,
  Eigen::Vector3d vel_lin, Eigen::Vector3d vel_ang){
  Eigen::Quaterniond r(pose.rotation());
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  pose_msg.orientation[0] =  r.w();
  pose_msg.orientation[1] =  r.x();
  pose_msg.orientation[2] =  r.y();
  pose_msg.orientation[3] =  r.z();
  pose_msg.vel[0] = vel_lin(0);
  pose_msg.vel[1] = vel_lin(1);
  pose_msg.vel[2] = vel_lin(2);
  pose_msg.rotation_rate[0] = vel_ang(0);
  pose_msg.rotation_rate[1] = vel_ang(1);
  pose_msg.rotation_rate[2] = vel_ang(2);
  pose_msg.accel[0]=0; // not estimated or filled in
  pose_msg.accel[1]=0;
  pose_msg.accel[2]=0;
  lcm_->publish( channel, &pose_msg);
}

bool pronto_vis::interpolateScan(const std::vector<float>& iRanges,
                const double iTheta0, const double iThetaStep,
                const Eigen::Isometry3d& iPose0,
                const Eigen::Isometry3d& iPose1,
                std::vector<Eigen::Vector3f>& oPoints) {
  const int n = iRanges.size();
  if (n < 2) return false;
  const double tStep = 1.0/(n-1);
  Eigen::Quaterniond q0(iPose0.linear());
  Eigen::Quaterniond q1(iPose1.linear());
  Eigen::Vector3d pos0(iPose0.translation());
  Eigen::Vector3d pos1(iPose1.translation());
  oPoints.resize(n);
  double t = 0;
  double theta = iTheta0;
  for (int i = 0; i < n; ++i, t += tStep, theta += iThetaStep) {
    Eigen::Quaterniond q = q0.slerp(t,q1);
    Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
    Eigen::Vector3d pt = iRanges[i]*Eigen::Vector3d(cos(theta), sin(theta), 0);
    oPoints[i] = (q*pt + pos).cast<float>();
  }
  return true;
}

Mapper::PointCloud::Ptr Mapper::generatePointCloud(const RGBDFrame::Ptr &frame)
{
    PointCloud::Ptr tmp( new PointCloud() );
    if ( frame->pointcloud == nullptr )
    {
        // point cloud is null ptr
        frame->pointcloud = boost::make_shared<PointCloud>();
#pragma omp parallel for
        for ( int m=0; m<frame->depth.rows; m+=3 )
        {
            for ( int n=0; n<frame->depth.cols; n+=3 )
            {
                ushort d = frame->depth.ptr<ushort>(m)[n];
                if (d == 0)
                    continue;
                if (d > max_distance * frame->camera.scale)
                    continue;
                PointT p;
                cv::Point3f p_cv = frame->project2dTo3d(n, m);
                p.b = frame->rgb.ptr<uchar>(m)[n*3];
                p.g = frame->rgb.ptr<uchar>(m)[n*3+1];
                p.r = frame->rgb.ptr<uchar>(m)[n*3+2];

                p.x = p_cv.x;
                p.y = p_cv.y;
                p.z = p_cv.z;

                frame->pointcloud->points.push_back( p );
            }
        }
    }

    Eigen::Isometry3d T = frame->getTransform().inverse();
    pcl::transformPointCloud( *frame->pointcloud, *tmp, T.matrix());
    tmp->is_dense = false;
    return tmp;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量 顶点(姿态) 是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N  高斯牛顿
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M                 
    g2o::SparseOptimizer optimizer;  
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    // 添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();//位姿
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );//旋转矩阵 和 平移向量
    pose->setId ( 0 );//id
    optimizer.addVertex ( pose );//添加顶点

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,//3D 位置
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray//相机内参数   灰度图
        );
        edge->setVertex ( 0, pose );//顶点
        edge->setMeasurement ( m.grayscale );//测量值为真是灰度值
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//误差 权重 信息矩阵
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"边的数量 edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();//优化初始化
    optimizer.optimize ( 30 );//最大优化次数
    Tcw = pose->estimate();// 变换矩阵
}

void compute_velocity(const Eigen::Vector3d& v_parent,
                      const Eigen::Vector3d& w_parent,
                      const Eigen::Vector3d& v_rel,
                      const Eigen::Vector3d& w_rel,
                      const Eigen::Vector3d& offset,
                      const Eigen::Isometry3d& tf_parent,
                      Eigen::Vector3d& v_child,
                      Eigen::Vector3d& w_child)
{
  const Eigen::Matrix3d& R = tf_parent.rotation();

  v_child = v_parent + R*v_rel + w_parent.cross(R*offset);

  w_child = w_parent + R*w_rel;
}

/**
 * @function transf2Params
 * @brief Fill par arguments for rot and trans from a _Tf
 */
void transf2Params( const Eigen::Isometry3d &_Tf,
		    SQ_params &_par ) {

    _par.px = _Tf.translation().x();
    _par.py = _Tf.translation().y();
    _par.pz = _Tf.translation().z();

    double r31, r11, r21, r32, r33;
    double r, p, y;

    r31 = _Tf.linear()(2,0);
    r11 = _Tf.linear()(0,0);
    r21 = _Tf.linear()(1,0);
    r32 = _Tf.linear()(2,1);
    r33 = _Tf.linear()(2,2);

    p = atan2( -r31, sqrt(r11*r11 + r21*r21) );
    y = atan2( r21 / cos(p), r11 / cos(p) );
    r = atan2( r32 / cos(p), r33 / cos(p) );

    _par.ra = r;
    _par.pa = p;
    _par.ya = y;
}

//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  const double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  const Eigen::Vector3d xAxis = pelvisXAxis.normalized();

  // Z-axis
  const Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}

int collideBoxBox(CollisionObject* o1, CollisionObject* o2,
                  const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
                  const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
                  CollisionResult& result)
{
  dVector3 halfSize0;
  dVector3 halfSize1;

  convVector(0.5 * size0, halfSize0);
  convVector(0.5 * size1, halfSize1);

  dMatrix3 R0, R1;

  convMatrix(T0, R0);
  convMatrix(T1, R1);

  dVector3 p0;
  dVector3 p1;

  convVector(T0.translation(), p0);
  convVector(T1.translation(), p1);

  return dBoxBox(o1, o2, p1, R1, halfSize1, p0, R0, halfSize0, result);
}

void FeatureTransformationEstimator::estimatePoseSVD(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d &T)
{
    pcl::TransformationFromCorrespondences tfc;
    for (int i = 0; i < P.cols(); i++) {
        Eigen::Vector3f p_i = P.col(i).cast<float>();
        Eigen::Vector3f q_i = Q.col(i).cast<float>();
        float inverse_weight = p_i(2)*p_i(2) + q_i(2)*q_i(2);
        float weight = 1;
        if (inverse_weight > 0) {
            weight = 1. / weight;
        }
        tfc.add(p_i, q_i, weight);
    }
    T.matrix() = tfc.getTransformation().matrix().cast<double>();
//    T.matrix() = Eigen::umeyama(P, Q, false);
}

PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
	PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
	
	// 合并点云
	PointCloud::Ptr output(new PointCloud());
	pcl::transformPointCloud(*original,*output,T.matrix());
	*newCloud += *output;

	// Voxel grid 滤波降采样
	static pcl::VoxelGrid<PointT> voxel;
	static ParameterReader pd;
	double gridsize = atof(pd.getData("voxel_grid").c_str());
	voxel.setLeafSize(gridsize,gridsize,gridsize);
	voxel.setInputCloud(newCloud);
	PointCloud::Ptr tmp(new PointCloud());
	voxel.filter(*tmp);
	return tmp;
}

void compute_acceleration(const Eigen::Vector3d& a_parent,
                          const Eigen::Vector3d& alpha_parent,
                          const Eigen::Vector3d& w_parent,
                          const Eigen::Vector3d& a_rel,
                          const Eigen::Vector3d& alpha_rel,
                          const Eigen::Vector3d& v_rel,
                          const Eigen::Vector3d& w_rel,
                          const Eigen::Vector3d& offset,
                          const Eigen::Isometry3d& tf_parent,
                          Eigen::Vector3d& a_child,
                          Eigen::Vector3d& alpha_child)
{
  const Eigen::Matrix3d& R = tf_parent.rotation();

  a_child = a_parent + R*a_rel
          + alpha_parent.cross(R*offset)
          + 2*w_parent.cross(R*v_rel)
          + w_parent.cross(w_parent.cross(R*offset));

  alpha_child = alpha_parent + R*alpha_rel + w_parent.cross(R*w_rel);
}

Eigen::Isometry3d SmoothEstimatePropagator::SmoothPropagateAction::
  exponencialInterpolation(const Eigen::Isometry3d& from, const Eigen::Isometry3d& to, double step) const
{
  Eigen::Isometry3d res;

  double maxdist = maxDistance-2;

  // step goes from 1 to maxDistance, we need x from 0 to 1 in a linear way.
  double x = 1 - ((maxdist - (step-1))/maxdist);
  // alpha in [0, inf) describes the explonential ramp "steepness"
  double alpha = 50;
  // exponential ramp from 0 to 1
  double exp_ramp = 1 - (x/(1+alpha*(1-x)));

  // using quaternion representation and slerp for interpolate between from and to isometry transformations
  res.linear() = (Eigen::Quaterniond(from.rotation()).slerp(exp_ramp, Eigen::Quaterniond(to.rotation()))).toRotationMatrix();
  res.translation() = (1 - exp_ramp) * from.translation() + exp_ramp * to.translation();

  return res;
}

int
main (int argc, char** argv)
{
  // 1. Parse arguments:
  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }  
  int mode=atoi(argv[1]);
  
  // 2 Construct the simulation method:
  int width = 640;
  int height = 480;
  simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
  
  // 3 Generate a series of simulation poses:
  // -0 100 fixed poses 
  // -1 a 'halo' camera 
  // -2 slerp between two different poses
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  if (mode==0){
    // Create a pose:
    Eigen::Isometry3d pose;
    pose.setIdentity();
    Matrix3d m;
    //ypr:
    m = AngleAxisd(-9.14989, Vector3d::UnitZ())     * AngleAxisd(0.20944, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());  
    pose *= m;
    Vector3d v;
    v << 1.31762, 0.382931, 1.89533;
    pose.translation() = v;  
    for (int i=0;i< 100;i++){ // duplicate the pose 100 times
      poses.push_back(pose); 
    }
  }else if(mode==1){
    Eigen::Vector3d focus_center(0,0,1.3);
    double halo_r = 4;
    double halo_dz = 2;
    int n_poses=16;
    generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
  }else if (mode==2){
    Eigen::Isometry3d pose1;
    pose1.setIdentity();
    pose1.translation() << 1,0.75,2;
    Eigen::Matrix3d rot1;
    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose1.rotate(rot1);  
  
    Eigen::Isometry3d pose2;
    pose2.setIdentity();
    pose2.translation() << 1,-1,3;
    Eigen::Matrix3d rot2;
    rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr    
    pose2.rotate(rot2);  
  
    int n_poses = 20;
    for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
      Eigen::Quaterniond rot3; 
      Eigen::Quaterniond r1(pose1.rotation());
      Eigen::Quaterniond r2(pose2.rotation());
      rot3 = r1.slerp(i,r2);
      Eigen::Isometry3d pose;
      pose.setIdentity();
      Eigen::Vector3d trans3 = (1-i)*pose1.translation() +  i*pose2.translation();
      pose.translation() <<  trans3[0] ,trans3[1] ,trans3[2];
      pose.rotate(rot3);
      poses.push_back(pose);
    }
  }

  // 4 Do the simulation and write the output:
  double tic_main = getTime();
  for (size_t i=0;i < poses.size();i++){
    std::stringstream ss;
    ss.precision(20);
    ss << "simcloud_" << i;// << ".pcd";
    double tic = getTime();
    simexample->doSim(poses[i]);
    
    write_sim_output(ss.str());
    cout << (getTime() -tic) << " sec\n";
  }
  cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
  cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
  
  return 0;
}

//.........这里部分代码省略.........
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx; 
        double y = ( pts1[i].y - cy ) * z / fy; 
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }
    
    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );
    
    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    
    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;
    
    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;
    
    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }
    
    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else 
        {
            inliers++;
        }
    }
    
    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}

int main(int /*argc*/, char** /*argv*/) {
  Line3D l0;
  std::cout << "l0: " << std::endl;
  printPlueckerLine(std::cout, l0);
  std::cout << std::endl;

  Vector6d v;
  v << 1.0, 1.0, -0.3, 0.5, 0.2, 0.3;
  Line3D l1(v);
  l1.normalize();
  std::cout << "v: ";
  printVector(std::cout, v);
  std::cout << std::endl;
  std::cout << "l1: " << std::endl;
  printPlueckerLine(std::cout, l1);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
  std::cout << std::endl;

  Line3D l2(l1);
  std::cout << "l2: " << std::endl;
  printPlueckerLine(std::cout, l2);
  std::cout << std::endl;

  Eigen::Vector4d mv = l2.ominus(l1);
  Eigen::Quaterniond q(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
  Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  double yaw = euler_angles[0];
  double pitch = euler_angles[1];
  double roll = euler_angles[2];  
  std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
  std::cout << std::endl;

  v << 0.0, 0.0, 1.0, 0.5, 0.5, 0.0;
  l1 = Line3D(v);
  l1.normalize();
  std::cout << "l1: " << std::endl;
  printPlueckerLine(std::cout, l1);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
  std::cout << std::endl;
  
  v << 0.0, 0.0, 1.0, 0.5, -0.5, 0.0;
  l2 = Line3D(v);
  l2.normalize();
  std::cout << "l2: " << std::endl;
  printPlueckerLine(std::cout, l2);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l2.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l2.d()) << std::endl;
  std::cout << std::endl;

  mv = l2.ominus(l1);
  q = Eigen::Quaterniond(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
  euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler_angles[0];
  pitch = euler_angles[1];
  roll = euler_angles[2];  
  std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
  std::cout << std::endl;
  
  Line3D l3 = Line3D(l1);
  std::cout << "l3: " << std::endl;
  printPlueckerLine(std::cout, l3);
  l3.oplus(mv);
  std::cout << "l3 oplus mv: " << std::endl;
  printPlueckerLine(std::cout, l3);
  std::cout << std::endl;

  std::vector<Line3D> l;
  v << 0.0, 0.0, 1.0, 1.0, 0.0, 0.0;
  Line3D ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);
  v << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0;
  ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);
  v << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0;
  ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);

  for(size_t i = 0; i < l.size(); ++i) {
    Line3D& line = l[i];
    std::cout << "line: "
	      << line.d()[0] << " " << line.d()[1] << " "  << line.d()[2] << " "
	      << line.w()[0] << " " << line.w()[1] << " "  << line.w()[2] << std::endl;
  }
  std::cout << std::endl;

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.translation().x() = 0.9;
  std::cout << "R: " << std::endl << T.linear() << std::endl;
  std::cout << "t: " << std::endl << T.translation() << std::endl;
  std::cout << std::endl;
  
  for(size_t i = 0; i < l.size(); ++i) {
    Line3D& line = l[i];
    line = T * line;
    std::cout << "line: "
//.........这里部分代码省略.........

void GraphicEnd::lostRecovery()
{
    //以present作为新的关键帧
    cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );

    //由于当前位置不知道,所以不添加与上一帧相关的边
    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
        edge->vertices()[1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = information(1, 1) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry);
        edge->setInformation( information );
        edge->setMeasurement( To );
        opt.addEdge( edge );
        _odo_last = _odo_this;
        _lost = 0;
        return;
    }
    //check loop closure
    for (int i=0; i<_keyframes.size() - 1; i++)
    {
        vector<PLANE>& p1 = _keyframes[ i ].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上，则在两个帧之间加一条边
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[i].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(1,1) = information(2,2) = 100; 
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }
    
}

int main()
{
  double euc_noise = 0.01;       // noise in position, m
  //  double outlier_ratio = 0.1;


  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // variable-size block solver
  BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
  BlockSolverX * solver_ptr = new BlockSolverX(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

  optimizer.setAlgorithm(solver);

  vector<Vector3d> true_points;
  for (size_t i=0;i<1000; ++i)
  {
    true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
                                   Sample::uniform()-0.5,
                                   Sample::uniform()+10));
  }


  // set up two poses
  int vertex_id = 0;
  for (size_t i=0; i<2; ++i)
  {
    // set up rotation and translation for this node
    Vector3d t(0,0,i);
    Quaterniond q;
    q.setIdentity();

    Eigen::Isometry3d cam; // camera pose
    cam = q;
    cam.translation() = t;

    // set up node
    VertexSE3 *vc = new VertexSE3();
    vc->setEstimate(cam);

    vc->setId(vertex_id);      // vertex id

    cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;

    // set first cam pose fixed
    if (i==0)
      vc->setFixed(true);

    // add to optimizer
    optimizer.addVertex(vc);

    vertex_id++;                
  }

  // set up point matches
  for (size_t i=0; i<true_points.size(); ++i)
  {
    // get two poses
    VertexSE3* vp0 = 
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
    VertexSE3* vp1 = 
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

    // calculate the relative 3D position of the point
    Vector3d pt0,pt1;
    pt0 = vp0->estimate().inverse() * true_points[i];
    pt1 = vp1->estimate().inverse() * true_points[i];

    // add in noise
    pt0 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    pt1 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    // form edge, with normals in varioius positions
    Vector3d nm0, nm1;
    nm0 << 0, i, 1;
    nm1 << 0, i, 1;
    nm0.normalize();
    nm1.normalize();

    Edge_V_V_GICP * e           // new edge with correct cohort for caching
        = new Edge_V_V_GICP(); 

    e->setVertex(0, vp0);      // first viewpoint

    e->setVertex(1, vp1);      // second viewpoint

    EdgeGICP meas;
    meas.pos0 = pt0;
    meas.pos1 = pt1;
    meas.normal0 = nm0;
    meas.normal1 = nm1;

    e->setMeasurement(meas);
//.........这里部分代码省略.........