C++ VertexSE3::setFixed方法代码示例

void GraphicEnd::init(SLAMEnd* pSLAMEnd)
{
    cout<<"Graphic end init..."<<endl;

    _pSLAMEnd = pSLAMEnd;
    _index = atoi( g_pParaReader->GetPara("start_index").c_str() );
    _rgbPath = g_pParaReader->GetPara("data_source")+string("/rgb_index/");
    _depPath = g_pParaReader->GetPara("data_source")+string("/dep_index/");
    _pclPath = g_pParaReader->GetPara("data_source")+string("/pcd/");
    _loops = 0;
    _success = false;
    _step_time = atoi(g_pParaReader->GetPara("step_time").c_str());
    _distance_threshold = atof( g_pParaReader->GetPara("distance_threshold").c_str() );
    _error_threshold = atof( g_pParaReader->GetPara("error_threshold").c_str() );
    _min_error_plane = atof( g_pParaReader->GetPara("min_error_plane").c_str() );
    _match_min_dist = atof( g_pParaReader->GetPara("match_min_dist").c_str() );
    _percent = atof( g_pParaReader->GetPara("plane_percent").c_str() );
    _max_pos_change = atof( g_pParaReader->GetPara("max_pos_change").c_str());
    _max_planes = atoi( g_pParaReader->GetPara("max_planes").c_str() );
    _loopclosure_frames = atoi( g_pParaReader->GetPara("loopclosure_frames").c_str() );
    _loop_closure_detection = (g_pParaReader->GetPara("loop_closure_detection") == string("yes"))?true:false;
    _loop_closure_error = atof(g_pParaReader->GetPara("loop_closure_error").c_str());
    _lost_frames = atoi( g_pParaReader->GetPara("lost_frames").c_str() );
    _robot = _kf_pos = Eigen::Isometry3d::Identity();
    _use_odometry = g_pParaReader->GetPara("use_odometry") == string("yes");
    _error_odometry = atof( g_pParaReader->GetPara("error_odometry").c_str() );
    _robot2camera = Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitX());
    if (_use_odometry)//some error happened! ty
    {
        cout<<"using odometry"<<endl;
        string fileaddr = g_pParaReader->GetPara("data_source")+string("/associate.txt");
		cout<<"fileaddr is: "<<fileaddr<<endl;
        ifstream fin(fileaddr.c_str());
        if (!fin)
        {
            cerr<<"cannot find associate.txt"<<endl;
            exit(0);
        }
		cout<<"found it"<<endl;
		string temp;
        while( !fin.eof())
        {
			getline(fin,temp);
			cout<<temp<<endl;
            _odometry.push_back( readOdometry(fin) );
        }
		cout<<"error??"<<endl;
        _odo_this = _odo_last = _odometry[ _index-1 ];
    }
    
    //读取首帧图像并处理
	cout<<"what happened?"<<endl;
    readimage();
    _lastRGB = _currRGB.clone();
    _currKF.id = 0;
    _currKF.frame_index = _index;
    /*
    _currKF.planes = extractPlanes( _currCloud ); //抓平面
    generateImageOnPlane( _currRGB, _currKF.planes, _currDep);
    */

    _currKF.planes = extractPlanesAndGenerateImage( _currCloud, _currRGB, _currDep );
    for ( size_t i=0; i<_currKF.planes.size(); i++ )
    {
        _currKF.planes[i].kp = extractKeypoints(_currKF.planes[i].image);
        _currKF.planes[i].desp = extractDescriptor( _currRGB, _currKF.planes[i].kp );
        compute3dPosition( _currKF.planes[i], _currDep );
    }
    _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( _robot );
    v->setFixed( true );
    opt.addVertex( v );
    _index ++;
    cout<<"********************"<<endl;
}

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);
//.........这里部分代码省略.........

void GraphicEnd2::init( SLAMEnd* pSLAMEnd)
{
    cout<<"Graphic end 2 init.."<<endl;
    
    _pSLAMEnd = pSLAMEnd;
    _index = atoi( g_pParaReader->GetPara("start_index").c_str() );
    _rgbPath = g_pParaReader->GetPara("data_source")+string("/rgb_index/");
    _depPath = g_pParaReader->GetPara("data_source")+string("/dep_index/");
    _pclPath = g_pParaReader->GetPara("data_source")+string("/pcd/");
    _loops = 0;
    _success = false;
    _step_time = atoi(g_pParaReader->GetPara("step_time").c_str());
    _distance_threshold = atof( g_pParaReader->GetPara("distance_threshold").c_str() );
    _error_threshold = atof( g_pParaReader->GetPara("error_threshold").c_str() );
    _min_error_plane = atof( g_pParaReader->GetPara("min_error_plane").c_str() );
    _match_min_dist = atof( g_pParaReader->GetPara("match_min_dist").c_str() );
    _percent = atof( g_pParaReader->GetPara("plane_percent").c_str() );
    _max_pos_change = atof( g_pParaReader->GetPara("max_pos_change").c_str());
    _max_planes = atoi( g_pParaReader->GetPara("max_planes").c_str() );
    _loopclosure_frames = atoi( g_pParaReader->GetPara("loopclosure_frames").c_str() );
    _loop_closure_detection = (g_pParaReader->GetPara("loop_closure_detection") == string("yes"))?true:false;
    _loop_closure_error = atof(g_pParaReader->GetPara("loop_closure_error").c_str());
    _lost_frames = atoi( g_pParaReader->GetPara("lost_frames").c_str() );
    _robot = _kf_pos = Eigen::Isometry3d::Identity();
    _use_odometry = g_pParaReader->GetPara("use_odometry") == string("yes");
    _error_odometry = atof( g_pParaReader->GetPara("error_odometry").c_str() );
    _robot2camera = Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitX());
    if (_use_odometry)
    {
        cout<<"using odometry"<<endl;
        string fileaddr = g_pParaReader->GetPara("data_source")+string("/associate.txt");
        ifstream fin(fileaddr.c_str());
        if (!fin)
        {
            cerr<<"cannot find associate.txt"<<endl;
            exit(0);
        }
        while( !fin.eof())
        {
            _odometry.push_back( readOdometry(fin) );
        }
        _odo_this = _odo_last = _odometry[ _index-1 ];
    }

    //处理首帧
    readimage();
    _lastRGB = _currRGB.clone();
    _currKF.id = 0;
    _currKF.frame_index = _index;
    _currKF.planes.push_back(extractKPandDesp(_currRGB, _currDep));

    _keyframes.push_back( _currKF );
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    else
        v->setEstimate( _robot );
    v->setFixed( true );
    opt.addVertex( v );
    _index ++;
    cout<<"********************"<<endl;
}