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

  HyperGraphElementAction* EdgeSE2DrawAction::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;

    EdgeSE2* e =  static_cast<EdgeSE2*>(element);
    VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
    VertexSE2* toEdge   = static_cast<VertexSE2*>(e->vertex(1));
    glColor3f(0.5,0.5,0.8);
    glPushAttrib(GL_ENABLE_BIT);
    glDisable(GL_LIGHTING);
    glBegin(GL_LINES);
    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
    glEnd();
    glPopAttrib();
    return this;
  }

 void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /* to */)
 {
   VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
   VertexSE2* toEdge   = static_cast<VertexSE2*>(_vertices[1]);
   if (from.count(fromEdge) > 0)
     toEdge->setEstimate(fromEdge->estimate() * _measurement);
   else
     fromEdge->setEstimate(toEdge->estimate() * _inverseMeasurement);
 }

void EdgeSE2PlaceVicinity::initialEstimate(
		const OptimizableGraph::VertexSet& from,
		OptimizableGraph::Vertex* /*to*/) {
	VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
	VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);

	if (from.count(fromEdge) > 0)
		toEdge->setEstimate(fromEdge->estimate());
	else
		fromEdge->setEstimate(toEdge->estimate());
  }

void GraphSLAM::addData(SE2 currentOdom, RobotLaser* laser){
  boost::mutex::scoped_lock lockg(graphMutex);

  //Add current vertex
  VertexSE2 *v = new VertexSE2;

  SE2 displacement = _lastOdom.inverse() * currentOdom;
  SE2 currEst = _lastVertex->estimate() * displacement;

  v->setEstimate(currEst);
  v->setId(++_runningVertexId + idRobot() * baseId());
  //Add covariance information
  //VertexEllipse *ellipse = new VertexEllipse;
  //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
  //ellipse->setCovariance(cov);
  //v->setUserData(ellipse);
  v->addUserData(laser);

  std::cout << std::endl << 
    "Current vertex: " << v->id() << 
    " Estimate: "<< v->estimate().translation().x() << 
    " " << v->estimate().translation().y() << 
    " " << v->estimate().rotation().angle() << std::endl;

  _graph->addVertex(v);

  //Add current odometry edge
  EdgeSE2 *e = new EdgeSE2;
  e->setId(++_runningEdgeId + idRobot() * baseId());
  e->vertices()[0] = _lastVertex;
  e->vertices()[1] = v;
      
  e->setMeasurement(displacement);
  
  // //Computing covariances depending on the displacement
  // Vector3d dis = displacement.toVector();
  // dis.x() = fabs(dis.x());
  // dis.y() = fabs(dis.y());
  // dis.z() = fabs(dis.z());
  // dis += Vector3d(1e-3,1e-3,1e-2);  
  // Matrix3d dis2 = dis*dis.transpose();
  // Matrix3d newcov = dis2.cwiseProduct(_odomK);

  e->setInformation(_odominf);
  _graph->addEdge(e);

  _odomEdges.insert(e);

  _lastOdom = currentOdom;
  _lastVertex = v;
}

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

void EdgeSE2DistanceOrientation::initialEstimate(
		const OptimizableGraph::VertexSet& from,
		OptimizableGraph::Vertex* /*to*/) {
	VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
	VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);


	double dist = sqrt(_measurement[0] * _measurement[0] + _measurement[1] * _measurement[1]);
	double theta = _measurement[2];
	double x = dist * cos(theta), y = dist * sin(theta);
	SE2 tmpMeasurement(x,y,theta);
	SE2 inverseTmpMeasurement  = tmpMeasurement.inverse();
	if (from.count(fromEdge) > 0)
		toEdge->setEstimate(fromEdge->estimate() * tmpMeasurement);
	else
		fromEdge->setEstimate(toEdge->estimate() * inverseTmpMeasurement);
  }

 HyperGraphElementAction* EdgeSE2OdomDifferentialCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* )
 {
   if (typeid(*element).name()!=_typeName)
     return 0;
   EdgeSE2OdomDifferentialCalib* e = static_cast<EdgeSE2OdomDifferentialCalib*>(element);
   VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
   VertexSE2* toEdge   = static_cast<VertexSE2*>(e->vertex(1));
   glColor3f(0.5,0.5,0.5);
   glPushAttrib(GL_ENABLE_BIT);
   glDisable(GL_LIGHTING);
   glBegin(GL_LINES);
   glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
   glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
   glEnd();
   glPopAttrib();
   return this;
 }

void GraphSLAM::optimize(int nrunnings){
  boost::mutex::scoped_lock lockg(graphMutex);

  _graph->initializeOptimization();
  _graph->optimize(nrunnings);

  //////////////////////////////////////
  //Update laser data
  for (SparseOptimizer::VertexIDMap::const_iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) {
    VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
    RobotLaser* robotLaser = findLaserData(v);
    if (robotLaser) 
      robotLaser->setOdomPose(v->estimate());
  }
  
  //////////////////////////////////////
  //Update ellipse data
  //Covariance computation
  /*
  CovarianceEstimator ce(_graph);

  OptimizableGraph::VertexSet vset;
  for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
    OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) (it->second);
    vset.insert(v);
  }

  ce.setVertices(vset);
  ce.setGauge(_lastVertex);

  ce.compute();

  ///////////////////////////////////
  
  for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
    VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
    VertexEllipse* ellipse = findEllipseData(v);
    if (ellipse && (v != lastVertex())){
      MatrixXd PvX = ce.getCovariance(v);
      Matrix3d Pv = PvX;
      Matrix3f Pvf = Pv.cast<float>();
      ellipse->setCovariance(Pvf);
      ellipse->clearMatchingVertices();
    }else {
      if(ellipse && v == lastVertex()){
	ellipse->clearMatchingVertices();
	for (size_t i = 0; i<ellipse->matchingVerticesIDs().size(); i++){
	  int id = ellipse->matchingVerticesIDs()[i];
	  VertexSE2* vid = dynamic_cast<VertexSE2*>(_graph->vertex(id));
	  SE2 relativetransf = _lastVertex->estimate().inverse() * vid->estimate();
	  ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y());
	}
      }
    }
  }*/
  
}

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

    if (from.count(_vertices[0]) != 1)
      return;
    VertexSE2* vi     = static_cast<VertexSE2*>(_vertices[0]);
    VertexPointXY* vj = static_cast<VertexPointXY*>(_vertices[1]);
    vj->setEstimate(vi->estimate() * _measurement);
  }

void GraphRosPublisher::publishGraph(){

  assert(_graph && "Cannot publish: undefined graph");

  geometry_msgs::PoseArray traj;
  sensor_msgs::PointCloud pcloud;
  traj.poses.resize(_graph->vertices().size());
  pcloud.points.clear();
  int i = 0;
  for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
    VertexSE2* v = (VertexSE2*) (it->second);
    traj.poses[i].position.x = v->estimate().translation().x();
    traj.poses[i].position.y = v->estimate().translation().y();
    traj.poses[i].position.z = 0;
    traj.poses[i].orientation = tf::createQuaternionMsgFromYaw(v->estimate().rotation().angle());

    RobotLaser *laser = dynamic_cast<RobotLaser*>(v->userData());
    if (laser){
      RawLaser::Point2DVector vscan = laser->cartesian();
      SE2 trl = laser->laserParams().laserPose;
      SE2 transf = v->estimate() * trl;
      RawLaser::Point2DVector wscan;
      ScanMatcher::applyTransfToScan(transf, vscan, wscan);
	  
      size_t s= 0;
      while ( s<wscan.size()){
	geometry_msgs::Point32 point;
	point.x = wscan[s].x();
	point.y = wscan[s].y();
	pcloud.points.push_back(point);
	
	s = s+10;
      }
    }
    i++;
  }
  
  traj.header.frame_id = _fixedFrame;
  pcloud.header.frame_id = traj.header.frame_id;
  _publm.publish(pcloud);
  _pubtj.publish(traj);

}

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

    if (! _drawActions){
      _drawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
    }

    refreshPropertyPtrs(params_);
    if (! _previousParams)
      return this;


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

    glColor3f(0.5f,0.5f,0.8f);
    glPushAttrib(GL_ENABLE_BIT);
    glDisable(GL_LIGHTING);
    glPushMatrix();
    glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),0.f);
    glRotatef((float)RAD2DEG(that->estimate().rotation().angle()),0.f,0.f,1.f);
    if (_show && _show->value()) {
      float tx=0.1f, ty=0.05f;
      if (_triangleX && _triangleY){
	tx=_triangleX->value();
	ty=_triangleY->value();
      }
      glBegin(GL_TRIANGLE_FAN);
      glVertex3f( tx ,0.f ,0.f);
      glVertex3f(-tx ,-ty, 0.f);
      glVertex3f(-tx , ty, 0.f);
      glEnd();
    }
    OptimizableGraph::Data* d=that->userData();
    while (d && _drawActions ){
      (*_drawActions)(d, params_);
      d=d->next();
    }
    glPopMatrix();
    glPopAttrib();
    return this;
  }

  HyperGraphElementAction* EdgeSE2DistanceOrientationWriteGnuplotAction::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, on valid os specified" << std::endl;
      return 0;
    }

    EdgeSE2DistanceOrientation* e =  static_cast<EdgeSE2DistanceOrientation*>(element);
    VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
    VertexPointXY* toEdge   = static_cast<VertexPointXY*>(e->vertex(1));
    *(params->os) << fromEdge->estimate().translation().x() << " " << fromEdge->estimate().translation().y()
      << " " << fromEdge->estimate().rotation().angle() << std::endl;
    *(params->os) << toEdge->estimate().x() << " " << toEdge->estimate().y() << std::endl;
    *(params->os) << std::endl;
    return this;
  }

  HyperGraphElementAction* EdgeSE2DistanceOrientationDrawAction::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;

    EdgeSE2DistanceOrientation* e =  static_cast<EdgeSE2DistanceOrientation*>(element);
    VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0));
    VertexPointXY* to   = static_cast<VertexPointXY*>(e->vertex(1));
    if (! from)
      return this;
    double guessRange=5;
    double theta = e->measurement();
    Vector2D p(cos(theta)*guessRange, sin(theta)*guessRange);
    glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR);
    glDisable(GL_LIGHTING);
    if (!to){
      p=from->estimate()*p;
      glColor3f(LANDMARK_EDGE_GHOST_COLOR);
      glPushAttrib(GL_POINT_SIZE);
      glPointSize(3);
      glBegin(GL_POINTS);
      glVertex3f((float)p.x(),(float)p.y(),0.f);
      glEnd();
      glPopAttrib();
    } else {
      p=to->estimate();
      glColor3f(LANDMARK_EDGE_COLOR);
    }
    glBegin(GL_LINES);
    glVertex3f((float)from->estimate().translation().x(),(float)from->estimate().translation().y(),0.f);
    glVertex3f((float)p.x(),(float)p.y(),0.f);
    glEnd();
    glPopAttrib();
    return this;
  }

  HyperGraphElementAction* EdgeSE2DrawAction::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;

    EdgeSE2* e =  static_cast<EdgeSE2*>(element);
    VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0));
    VertexSE2* to   = static_cast<VertexSE2*>(e->vertex(1));
    if (! from && ! to)
      return this;
    SE2 fromTransform;
    SE2 toTransform;
    glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING | GL_COLOR);
    glDisable(GL_LIGHTING);
    if (! from) {
      glColor3f(POSE_EDGE_GHOST_COLOR);
      toTransform = to->estimate();
      fromTransform = to->estimate()*e->measurement().inverse();
      // DRAW THE FROM EDGE AS AN ARROW
      glPushMatrix();
      glTranslatef((float)fromTransform.translation().x(), (float)fromTransform.translation().y(),0.f);
      glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f);
      opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
      glPopMatrix();
    } else if (! to){
      glColor3f(POSE_EDGE_GHOST_COLOR);
      fromTransform = from->estimate();
      toTransform = from->estimate()*e->measurement();
      // DRAW THE TO EDGE AS AN ARROW
      glPushMatrix();
      glTranslatef(toTransform.translation().x(),toTransform.translation().y(),0.f);
      glRotatef((float)RAD2DEG(toTransform.rotation().angle()),0.f,0.f,1.f);
      opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
      glPopMatrix();
    } else {
      glColor3f(POSE_EDGE_COLOR);
      fromTransform = from->estimate();
      toTransform = to->estimate();
    }
    glBegin(GL_LINES);
    glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),0.f);
    glVertex3f((float)toTransform.translation().x(),(float)toTransform.translation().y(),0.f);
    glEnd();
    glPopAttrib();
    return this;
  }

void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
  ///////////////////////////////////
  // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one

  CovarianceEstimator ce(_graph);

  ce.setVertices(vset);
  ce.setGauge(_lastVertex);
  
  ce.compute();

  assert(!_lastVertex->fixed() && "last Vertex is fixed");
  assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
  
  OptimizableGraph::VertexSet tmpvset = vset;
  for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    
    MatrixXd Pv = ce.getCovariance(vertex);
    Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
    SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();	
    Vector2d hxy (delta.translation().x(), delta.translation().y());
    double perceptionRange =1;
    if (hxy.x()-perceptionRange>0) 
      hxy.x() -= perceptionRange;
    else if (hxy.x()+perceptionRange<0)
      hxy.x() += perceptionRange;
    else
      hxy.x() = 0;

    if (hxy.y()-perceptionRange>0) 
      hxy.y() -= perceptionRange;
    else if (hxy.y()+perceptionRange<0)
      hxy.y() += perceptionRange;
    else
      hxy.y() = 0;
    
    double d2 = hxy.transpose() * Pxy.inverse() * hxy;
    if (d2 > 5.99)
      vset.erase(*it);
 
  }
  
}