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

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;
}

  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;
  }

 HyperGraphElementAction* EdgeSE3DrawAction::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;
   
   EdgeSE3* e =  static_cast<EdgeSE3*>(element);
   VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
   VertexSE3* toEdge   = static_cast<VertexSE3*>(e->vertices()[1]);
   if (! fromEdge || ! toEdge)
     return this;
   glColor3f(POSE_EDGE_COLOR);
   glPushAttrib(GL_ENABLE_BIT);
   glDisable(GL_LIGHTING);
   glBegin(GL_LINES);
   glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),(float)fromEdge->estimate().translation().z());
   glVertex3f((float)toEdge->estimate().translation().x(),(float)toEdge->estimate().translation().y(),(float)toEdge->estimate().translation().z());
   glEnd();
   glPopAttrib();
   return this;
 }

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

    if (from_.count(from) > 0) {
        to->setEstimate(from->estimate() * _measurement);
    } else
        from->setEstimate(to->estimate() * _measurement.inverse());
    //cerr << "IE" << endl;
}

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

    Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
    
    if (from_.count(from) > 0) {
      to->setEstimate(from->estimate() * virtualMeasurement);
    } else
      from->setEstimate(to->estimate() * virtualMeasurement.inverse());
  }

  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);
 }

  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;
  }

  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;
  }

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;
 }

 HyperGraphElementAction* VertexSE3WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
   if (typeid(*element).name()!=_typeName)
     return 0;
   WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
   if (!params->os){
     std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid os specified" << std::endl;
     return 0;
   }
   
   VertexSE3* v =  static_cast<VertexSE3*>(element);
   *(params->os) << v->estimate().translation().x() << " " 
     << v->estimate().translation().y() << " " 
     << v->estimate().translation().z() << " ";
   *(params->os) << v->estimate().rotation().x() << " " 
     << v->estimate().rotation().y() << " " 
     << v->estimate().rotation().z() << " " << std::endl;
   return this;
 }

  void EdgeSE3Offset::linearizeOplus(){
    //BaseBinaryEdge<6, SE3Quat, VertexSE3, VertexSE3>::linearizeOplus();

    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& Pi=_cacheFrom->offsetParam()->offset();
    const Eigen::Isometry3d& Pj=_cacheTo->offsetParam()->offset();
    const Eigen::Isometry3d& Z=_measurement;
    // Matrix6d Ji, Jj;
    // computeSE3Gradient(E, Ji , Jj,
    //                    Z, Pi, Xi, Pj, Xj);
    // cerr  << "Ji:" << endl;
    // cerr << Ji-_jacobianOplusXi << endl;
    // cerr  << "Jj:" << endl;
    // cerr << Jj-_jacobianOplusXj << endl;
    internal::computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj, Pi, Pj);
  }

  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 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));
  }