C++ VertexSE3类代码示例

  HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, 
                 HyperGraphElementAction::Parameters* params_){
    if (typeid(*element).name()!=_typeName)
      return 0;
    initializeDrawActionsCache();
    refreshPropertyPtrs(params_);

    if (! _previousParams)
      return this;
    
    if (_show && !_show->value())
      return this;

    VertexSE3* that = static_cast<VertexSE3*>(element);

    glColor3f(POSE_VERTEX_COLOR);
    glPushMatrix();
    glMultMatrixd(that->estimate().matrix().data());
    opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
    drawCache(that->cacheContainer(), params_);
    drawUserData(that->userData(), params_);

    if(_showId && _showId->value()){
      float scale = _idSize ? _idSize->value() : 1.f;
      drawId(std::to_string(that->id()), scale);
    }
    
    glPopMatrix();
    return this;
  }

bool EdgeSE3::setMeasurementFromState() {
    VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);
    Eigen::Isometry3d delta = from->estimate().inverse() * to->estimate();
    setMeasurement(delta);
    return true;
}

  bool extractAbsolutePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo, 
			    const DrawableFrame* current){
    VertexSE3* currentVertex =current->_vertex;
    ImuData* imuData = 0;
    OptimizableGraph::Data* d = currentVertex->userData();
    while(d) {
      ImuData* imuData_ = dynamic_cast<ImuData*>(d);
      if (imuData_){
	imuData = imuData_;
      }
      d=d->next();
    }
	
    if (imuData){
      Eigen::Matrix3d R=imuData->getOrientation().matrix();
      Eigen::Matrix3d Omega = imuData->getOrientationCovariance().inverse();
      priorMean.setIdentity();
      priorInfo.setZero();
      for (int c = 0; c<3; c++)
	for (int r = 0; r<3; r++)
	  priorMean.linear()(r,c)=R(r,c);
      
      for (int c = 0; c<3; c++)
	for (int r = 0; r<3; r++)
	  priorInfo(r+3,c+3)=Omega(r,c);
      return true;
    }
    return false;
  }

  HyperGraphElementAction* EdgeProjectDisparityDrawAction::operator()(HyperGraph::HyperGraphElement* element, 
                HyperGraphElementAction::Parameters*  params_ ){
  if (typeid(*element).name()!=_typeName)
      return 0;
    refreshPropertyPtrs(params_);
    if (! _previousParams)
      return this;

    if (_show && !_show->value())
      return this;
    EdgeSE3PointXYZDisparity* e =  static_cast<EdgeSE3PointXYZDisparity*>(element);
    VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
    VertexPointXYZ* toEdge   = static_cast<VertexPointXYZ*>(e->vertices()[1]);
    if (! fromEdge || ! toEdge)
      return this;
    Eigen::Isometry3d fromTransform=fromEdge->estimate() * e->cameraParameter()->offset();
    glColor3f(LANDMARK_EDGE_COLOR);
    glPushAttrib(GL_ENABLE_BIT);
    glDisable(GL_LIGHTING);
    glBegin(GL_LINES);
    glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),(float)fromTransform.translation().z());
    glVertex3f((float)toEdge->estimate().x(),(float)toEdge->estimate().y(),(float)toEdge->estimate().z());
    glEnd();
    glPopAttrib();
    return this;
  }

  HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, 
                 HyperGraphElementAction::Parameters* params_){
    if (typeid(*element).name()!=_typeName)
      return 0;
    if (! _cacheDrawActions){
      _cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
    }

    refreshPropertyPtrs(params_);
    if (! _previousParams)
      return this;
    
    if (_show && !_show->value())
      return this;

    VertexSE3* that = static_cast<VertexSE3*>(element);

    glColor3f(0.5,0.5,0.8);
    glPushMatrix();
    glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),that->estimate().translation().z());
    AngleAxisd aa(that->estimate().rotation());
    glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z());
    if (_triangleX && _triangleY){
      drawTriangle(_triangleX->value(), _triangleY->value());
    }
    CacheContainer* caches=that->cacheContainer();
    if (caches){
      for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
  Cache* c = it->second;
  (*_cacheDrawActions)(c, params_);
      }
    }
    glPopMatrix();
    return this;
  }

void GraphicEnd::generateKeyframe(Eigen::Isometry3d& T)
{
    cout<<"generating keyframes..."<<endl;
    currKF.id++;
    currKF.frame_index=index;
    currKF.frame.rgb=present.frame.rgb.clone();
    currKF.frame.dep=present.frame.dep.clone();
    //currKF.frame.cloud=present.frame.cloud;
    pcl::copyPointCloud(*(present.frame.cloud),*(currKF.frame.cloud));
    currKF.fullpose=present.fullpose;
    //keyframes.reserve(100);

    keyframes.push_back(currKF);
    //cout<<"keyframes capacity is: "<<keyframes.capacity()<<endl;
    
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    VertexSE3* v = new VertexSE3();
    v->setId( currKF.id );
    v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );
    cout<<"add vertex success!!!"<<endl;
    //edge
    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) = 100;
    information(1, 1) = 100;
    information(3,3) = information(4,4) = information(5,5) = 100; 
    edge->setInformation( information );
    edge->setMeasurement( T );
    opt.addEdge( edge );
    cout<<"add edge success!!!"<<endl;
}

void GraphicEnd::savepcd()
{
    cout<<"save final pointcloud"<<endl;
    SLAMEnd slam;
    slam.init(NULL);
    SparseOptimizer& opt = slam.globalOptimizer;
    opt.load("/home/lc/workspace/paper_related/Appolo/test/result/final_after.g2o");
    ifstream fin("/home/lc/workspace/paper_related/Appolo/test/result/key_frame.txt");
    PointCloud::Ptr output(new PointCloud());
    PointCloud::Ptr curr( new PointCloud());
    pcl::VoxelGrid<PointT> voxel;
    voxel.setLeafSize(0.01f, 0.01f, 0.01f );
    string pclPath ="/media/新加卷/dataset/dataset1/pcd/";

    pcl::PassThrough<PointT> pass;
    pass.setFilterFieldName("z");
    double z = 5.0;
    pass.setFilterLimits(0.0, z);
    
    while( !fin.eof() )
    {
        int frame, id;
        fin>>id>>frame;
        ss<<pclPath<<frame<<".pcd";
        
        string str;
        ss>>str;
        cout<<"loading "<<str<<endl;
        ss.clear();

        pcl::io::loadPCDFile( str.c_str(), *curr );
        //cout<<"curr cloud size is: "<<curr->points.size()<<endl;
        VertexSE3* pv = dynamic_cast<VertexSE3*> (opt.vertex( id ));
        if (pv == NULL)
            break;
        Eigen::Isometry3d pos = pv->estimate();

        cout<<pos.matrix()<<endl;
        voxel.setInputCloud( curr );
        PointCloud::Ptr tmp( new PointCloud());
        voxel.filter( *tmp );
        curr.swap( tmp );
        pass.setInputCloud( curr );
        pass.filter(*tmp);
        curr->swap( *tmp );
        //cout<<"tmp: "<<tmp->points.size()<<endl;
        pcl::transformPointCloud( *curr, *tmp, pos.matrix());
        *output += *tmp;
        //cout<<"output: "<<output->points.size()<<endl;
    }
    voxel.setInputCloud( output );
    PointCloud::Ptr output_filtered( new PointCloud );
    voxel.filter( *output_filtered );
    output->swap( *output_filtered );
    //cout<<output->points.size()<<endl;
    pcl::io::savePCDFile( "/home/lc/workspace/paper_related/Appolo/test/result/result.pcd", *output);
    cout<<"final result saved."<<endl;
}

 void EdgeSE3PointXYZCov::initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* /*to_*/)
 {
   VertexSE3*       from = static_cast<VertexSE3*>(_vertices[0]);
   VertexPointXYZCov* to = static_cast<VertexPointXYZCov*>(_vertices[1]);
   if (from_.count(from) > 0)
     to->setEstimate(from->estimate() * measurement());
   else
     std::cerr << __PRETTY_FUNCTION__ << std::endl;
 }

void GraphicEnd::generateKeyFrame( Eigen::Isometry3d T )
{
    cout<<BOLDGREEN<<"GraphicEnd::generateKeyFrame"<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    cout<<"add key frame: "<<_currKF.id<<endl;
    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    //v->setEstimate( _robot );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    //v->setEstimate( Eigen::Isometry3d::Identity() );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );
    //边
    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) = 100;
    information(1, 1) = 100;
    information(3,3) = information(4,4) = information(5,5) = 100; 
    edge->setInformation( information );
    edge->setMeasurement( T );
    opt.addEdge( edge );

    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        cout<<"odo last = "<<endl<<_odo_last.matrix()<<endl<<"odo this = "<<endl<<_odo_this.matrix()<<endl;
        cout<<"To = "<<endl<<To.matrix()<<endl;
        cout<<"Measurement = "<<endl<<T.matrix()<<endl;
        //waitKey( 0 );
        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(1,1) = information(2,2) = 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;
    }
    
}

  void EdgeSE3Prior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
    VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);

    SE3Quat newEstimate = _offsetParam->offset().inverse() * measurement();
    if (_information.block<3,3>(0,0).squaredNorm()==0){ // do not set translation
      newEstimate.setTranslation(v->estimate().translation());
    }
    if (_information.block<3,3>(3,3).squaredNorm()==0){ // do not set rotation
      newEstimate.setRotation(v->estimate().rotation());
    }
    v->setEstimate(newEstimate);
  }

 void EdgeSE3Prior::linearizeOplus(){
   VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
   Isometry3 E;
   Isometry3 Z, X, P;
   X=from->estimate().rotation().toRotationMatrix();
   X.translation()=from->estimate().translation();
   P=_cache->offsetParam()->offset().rotation().toRotationMatrix();
   P.translation()=_cache->offsetParam()->offset().translation();
   Z=_measurement.rotation().toRotationMatrix();
   Z.translation()=_measurement.translation();
   internal::computeEdgeSE3PriorGradient(E, _jacobianOplusXi, Z, X, P);
 }

  void EdgeSE3LotsOfXYZ::computeError(){
    VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);

    for(unsigned int i=0; i<_observedPoints; i++){
      VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
      Vector3D m = pose->estimate().inverse() * xyz->estimate();

      unsigned int index = 3*i;
      _error[index] = m[0] - _measurement[index];
      _error[index+1] = m[1] - _measurement[index+1];
      _error[index+2] = m[2] - _measurement[index+2];
    }
  }

void EdgeSE3::linearizeOplus() {

    // BaseBinaryEdge<6, Eigen::Isometry3d, VertexSE3, VertexSE3>::linearizeOplus();
    // return;

    VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);
    Eigen::Isometry3d E;
    const Eigen::Isometry3d& Xi=from->estimate();
    const Eigen::Isometry3d& Xj=to->estimate();
    const Eigen::Isometry3d& Z=_measurement;
    computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj);
}

  void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
    Eigen::Vector3d p;
    p(2) = _measurement(2);
    p.head<2>() = _measurement.head<2>()*p(2);
    p=invKcam*p;
    point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
  }

  void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    // SE3OffsetCache* vcache = (SE3OffsetCache* ) cam->getCache(_cacheIds[0]);
    // if (! vcache){
    //   cerr << "fatal error in retrieving cache" << endl;
    // }
    // SE3OffsetParameters* params=vcache->params;
    Eigen::Vector3d p=_measurement;
    point->setEstimate(cam->estimate() * (offsetParam->offset() * p));
  }