• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    迪恩网络公众号

C++ VertexSE2类代码示例

原作者: [db:作者] 来自: [db:来源] 收藏 邀请

本文整理汇总了C++中VertexSE2的典型用法代码示例。如果您正苦于以下问题:C++ VertexSE2类的具体用法?C++ VertexSE2怎么用?C++ VertexSE2使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。



在下文中一共展示了VertexSE2类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: lockg

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());
	}
      }
    }
  }*/
  
}
开发者ID:droter,项目名称:cg_mrslam,代码行数:57,代码来源:graph_slam.cpp


示例2: assert

  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);
  }
开发者ID:2maz,项目名称:g2o,代码行数:10,代码来源:edge_se2_pointxy_calib.cpp


示例3:

 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;
 }
开发者ID:Aerobota,项目名称:c2tam,代码行数:14,代码来源:vertex_se2.cpp


示例4: main

int main(int argc, char** argv) {
  CommandArgs arg;


  std::string outputFilename;
  std::string inputFilename;

  arg.param("o", outputFilename, "", "output file name"); 
  arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
  arg.parseArgs(argc, argv);
  OptimizableGraph graph;
  if (!graph.load(inputFilename.c_str())){
    cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
    return 0;
  }
  HyperGraph::EdgeSet removedEdges;
  HyperGraph::VertexSet removedVertices;
  for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
    HyperGraph::Edge* e = *it;
    EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
    if (edgePointXY) {
      VertexSE2* pose    = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
      VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
      FeaturePointXYData * feature = new FeaturePointXYData();
      feature->setPositionMeasurement(edgePointXY->measurement());
      feature->setPositionInformation(edgePointXY->information());
      pose->addUserData(feature); 
      removedEdges.insert(edgePointXY);
      removedVertices.insert(landmark);
    }
  }
  
  for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
    OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
    graph.removeEdge(e);
  }

  for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
    OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
    graph.removeVertex(v);
  }

  
  if (outputFilename.length()){
    graph.save(outputFilename.c_str());
  }
 
}
开发者ID:9578577,项目名称:g2o_frontend,代码行数:48,代码来源:g2o_anonymize_observations.cpp


示例5: ce

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);
 
  }
  
}
开发者ID:droter,项目名称:cg_mrslam,代码行数:44,代码来源:graph_slam.cpp


示例6: assert

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

}
开发者ID:droter,项目名称:cg_mrslam,代码行数:43,代码来源:graph_ros_publisher.cpp


示例7: refreshPropertyPtrs

  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;
  }
开发者ID:Aerobota,项目名称:c2tam,代码行数:43,代码来源:vertex_se2.cpp


示例8: refreshPropertyPtrs

  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;
  }
开发者ID:pangfumin,项目名称:g2o,代码行数:53,代码来源:edge_se2.cpp


示例9:

  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;
  }
开发者ID:jachu51,项目名称:DiamentowyGrant,代码行数:19,代码来源:edge_se2_distanceOrientation.cpp


示例10: updateVertexIdx

	inline void updateVertexIdx()
	{
		if ((ros::Time::now() - lastSavedNodeTime).toSec() >= 0.03)
		{
			nodeCounter++;
			lastSavedNodeTime = ros::Time::now();
			PreviousVertexId = CurrentVertexId;
			CurrentVertexId++;
			if (CurrentVertexId - LandmarkCount >= 100)
			{
				CurrentVertexId = LandmarkCount;
			}

			{
				VertexSE2 * r = new VertexSE2;
				r->setEstimate(Eigen::Vector3d(location.x, location.y, 0));
				r->setFixed(false);
				r->setId(CurrentVertexId);
				if (optimizer.vertex(CurrentVertexId) != NULL)
				{
					optimizer.removeVertex(optimizer.vertex(CurrentVertexId));
				}

				optimizer.addVertex(r);
			}

			{
				EdgeSE2 * e = new EdgeSE2;
				e->vertices()[0] = optimizer.vertex(PreviousVertexId);
				e->vertices()[1] = optimizer.vertex(CurrentVertexId);
				Point2d dead_reck = getOdometryFromLastGet();
				e->setMeasurement(SE2(dead_reck.x, dead_reck.y, 0));
				Matrix3d information;
				information.fill(0.);
				information(0, 0) = 200;
				information(1, 1) = 200;
				information(2, 2) = 1;
				e->setInformation(information);
				optimizer.addEdge(e);
			}
		}
	}
开发者ID:AIS-Bonn,项目名称:humanoid_op_ros,代码行数:42,代码来源:Localization.hpp


示例11: refreshPropertyPtrs

  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;
  }
开发者ID:jachu51,项目名称:DiamentowyGrant,代码行数:41,代码来源:edge_se2_distanceOrientation.cpp


示例12: refreshPropertyPtrs

  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;
  }
开发者ID:Florenc,项目名称:g2o,代码行数:25,代码来源:edge_se2.cpp


示例13: transformPointsFromVSet

void transformPointsFromVSet(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _referenceVertex, RawLaser::Point2DVector& scansInRefVertex){

  VertexSE2* referenceVertex=dynamic_cast<VertexSE2*>(_referenceVertex);
  scansInRefVertex.clear();
  for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
    if (laserv){
      RawLaser::Point2DVector vscan = laserv->cartesian();
      SE2 trl = laserv->laserParams().laserPose;
      RawLaser::Point2DVector scanInRefVertex;
      if (vertex->id() == referenceVertex->id()){
	ScanMatcher::applyTransfToScan(trl, vscan, scanInRefVertex);
      }else{
	SE2 trel = referenceVertex->estimate().inverse() * vertex->estimate();
	SE2 transf = trel * trl;
	ScanMatcher::applyTransfToScan(transf, vscan, scanInRefVertex);
      }
      scansInRefVertex.insert(scansInRefVertex.end(), scanInRefVertex.begin(), scanInRefVertex.end());
    }
  }
}
开发者ID:droter,项目名称:cg_mrslam,代码行数:22,代码来源:scan_matcher.cpp


示例14:

 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);
 }
开发者ID:pangfumin,项目名称:g2o,代码行数:9,代码来源:edge_se2.cpp


示例15:

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());
  }
开发者ID:jachu51,项目名称:DiamentowyGrant,代码行数:11,代码来源:edge_se2_placeVicinity.cpp


示例16: perform

 virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e)
 {
   if (! vParent)
     return 0.;
   EdgeSE2* odom    = static_cast<EdgeSE2*>(e);
   VertexSE2* from  = static_cast<VertexSE2*>(vParent);
   VertexSE2* to    = static_cast<VertexSE2*>(v);
   assert(to->hessianIndex() >= 0);
   double fromTheta = from->hessianIndex() < 0 ? 0. : _thetaGuess[from->hessianIndex()];
   bool direct      = odom->vertices()[0] == from;
   if (direct) 
     _thetaGuess[to->hessianIndex()] = fromTheta + odom->measurement().rotation().angle();
   else
     _thetaGuess[to->hessianIndex()] = fromTheta - odom->measurement().rotation().angle();
   return 1.;
 }
开发者ID:2maz,项目名称:g2o,代码行数:16,代码来源:solver_slam2d_linear.cpp


示例17: tmpMeasurement

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);
  }
开发者ID:jachu51,项目名称:DiamentowyGrant,代码行数:17,代码来源:edge_se2_distanceOrientation.cpp


示例18: glPushAttrib

 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;
 }
开发者ID:ericperko,项目名称:g2o,代码行数:17,代码来源:edge_se2_odom_differential_calib.cpp


示例19: main

int main(int argc, char **argv) {
  /************************************************************************
   *                          Input handling                              *
   ************************************************************************/
  float rows, cols, gain, square_size;
  float resolution, max_range, usable_range, angle, threshold;
  string g2oFilename, mapFilename;
  g2o::CommandArgs arg;
  arg.param("resolution", resolution, 0.05f, "resolution of the map (how much is in meters a pixel)");
  arg.param("threshold", threshold, -1.0f, "threshold to apply to the frequency map (values under the threshold are discarded)");
  arg.param("rows", rows, 0, "impose the resulting map to have this number of rows");
  arg.param("cols", cols, 0, "impose the resulting map to have this number of columns");
  arg.param("max_range", max_range, -1.0f, "max laser range to consider for map building");
  arg.param("usable_range", usable_range, -1.0f, "usable laser range for map building");
  arg.param("gain", gain, 1, "gain to impose to the pixels of the map");
  arg.param("square_size", square_size, 1, "square size of the region where increment the hits");
  arg.param("angle", angle, 0, "rotate the map of x degrees");
  arg.paramLeftOver("input_graph.g2o", g2oFilename, "", "input g2o graph to use to build the map", false);
  arg.paramLeftOver("output_map", mapFilename, "", "output filename where to save the map (without extension)", false);  
  arg.parseArgs(argc, argv);

  angle = angle*M_PI/180.0;

  /************************************************************************
   *                          Loading Graph                               *
   ************************************************************************/
  // Load graph
  typedef BlockSolver< BlockSolverTraits<-1, -1> >  SlamBlockSolver;
  typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
  SlamLinearSolver *linearSolver = new SlamLinearSolver();
  linearSolver->setBlockOrdering(false);
  SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
  OptimizationAlgorithmGaussNewton *solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
  SparseOptimizer *graph = new SparseOptimizer();
  graph->setAlgorithm(solverGauss);    
  graph->load(g2oFilename.c_str());
  
  // Sort verteces
  vector<int> vertexIds(graph->vertices().size());
  int k = 0;
  for(OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
    vertexIds[k++] = (it->first);
  }  
  sort(vertexIds.begin(), vertexIds.end());
  
  /************************************************************************
   *                          Compute map size                            *
   ************************************************************************/
  // Check the entire graph to find map bounding box
  Eigen::Matrix2d boundingBox = Eigen::Matrix2d::Zero();
  std::vector<RobotLaser*> robotLasers;
  std::vector<SE2> robotPoses;
  double xmin=std::numeric_limits<double>::max();
  double xmax=std::numeric_limits<double>::min();
  double ymin=std::numeric_limits<double>::max();
  double ymax=std::numeric_limits<double>::min();

  SE2 baseTransform(0,0,angle);

  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    v->setEstimate(baseTransform*v->estimate());
    OptimizableGraph::Data *d = v->userData();

    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      robotLasers.push_back(robotLaser);
      robotPoses.push_back(v->estimate());
      double x = v->estimate().translation().x();
      double y = v->estimate().translation().y();
      
      xmax = xmax > x+usable_range ? xmax : x+usable_range;
      ymax = ymax > y+usable_range ? ymax : y+usable_range;
      xmin = xmin < x-usable_range ? xmin : x-usable_range;
      ymin = ymin < y-usable_range ? ymin : y-usable_range;
 
      d = d->next();
    }
  }

  boundingBox(0,0)=xmin;
  boundingBox(0,1)=xmax;
  boundingBox(1,0)=ymin;
  boundingBox(1,1)=ymax;

  std::cout << "Found " << robotLasers.size() << " laser scans"<< std::endl;
  std::cout << "Bounding box: " << std::endl << boundingBox << std::endl; 
  if(robotLasers.size() == 0)  {
    std::cout << "No laser scans found ... quitting!" << std::endl;
    return 0;
  }

  /************************************************************************
   *                          Compute the map                             *
//.........这里部分代码省略.........
开发者ID:lfermin77,项目名称:g2o2ros,代码行数:101,代码来源:g2o2ros.cpp


示例20: resetGrid

bool ScanMatcher::verifyMatching(OptimizableGraph::VertexSet& vset1, OptimizableGraph::Vertex* _referenceVertex1, 
				 OptimizableGraph::VertexSet& vset2, OptimizableGraph::Vertex* _referenceVertex2, 
				 SE2 trel12, double *score){ 

  VertexSE2* referenceVertex2=dynamic_cast<VertexSE2*>(_referenceVertex2);

  resetGrid();
  CharGrid auxGrid = _grid;

  //Transform points from vset2 in the reference of referenceVertex1 using trel12
  RawLaser::Point2DVector scansvset2inref1;
  for (OptimizableGraph::VertexSet::iterator it = vset2.begin(); it != vset2.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
    RawLaser::Point2DVector vscan = laserv->cartesian();
    SE2 trl = laserv->laserParams().laserPose;

    RawLaser::Point2DVector scanInRefVertex1;
    if (vertex->id() == referenceVertex2->id()){
      applyTransfToScan(trel12 * trl, vscan, scanInRefVertex1);
    }else{
      //Transform scans to the referenceVertex2 coordinates
      SE2 tref2_v = referenceVertex2->estimate().inverse() * vertex->estimate();
      applyTransfToScan(trel12 * tref2_v * trl, vscan, scanInRefVertex1);
    }
    scansvset2inref1.insert(scansvset2inref1.end(), scanInRefVertex1.begin(), scanInRefVertex1.end());
  }

  //Scans in vset1
  RawLaser::Point2DVector scansvset1;
  transformPointsFromVSet(vset1, _referenceVertex1, scansvset1);

  //Add local map from vset2 into the grid
  _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset2inref1.begin(), scansvset2inref1.end(), _kernel);
  //Find points from vset1 not explained by map vset2
  RawLaser::Point2DVector nonmatchedpoints;
  _grid.searchNonMatchedPoints(scansvset1, nonmatchedpoints, .3);

  //Add those points to a grid to count them
  auxGrid.addAndConvolvePoints<RawLaser::Point2DVector>(nonmatchedpoints.begin(), nonmatchedpoints.end(), _kernel);

  // ofstream image1;
  // std::stringstream filename1;
  // filename1 << "map2.ppm";
  // image1.open(filename1.str().c_str());
  // _LCGrid.grid().saveAsPPM(image1, false);

  // ofstream image2;
  // std::stringstream filename2;
  // filename2 << "mapnonmatched.ppm";
  // image2.open(filename2.str().c_str());
  // auxGrid.grid().saveAsPPM(image2, false);

  // //Just for saving the image
  // resetLCGrid();
  // _LCGrid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset1.begin(), scansvset1.end(), _LCKernel);
  // ofstream image3;
  // std::stringstream filename3;
  // filename3 << "map1.ppm";
  // image3.open(filename3.str().c_str());
  // _LCGrid.grid().saveAsPPM(image3, false);


  //Counting points around trel12
  Vector2f lower(-.3+trel12.translation().x(), -.3+trel12.translation().y());
  Vector2f upper(+.3+trel12.translation().x(), +.3+trel12.translation().y()); 
  
  auxGrid.countPoints(lower, upper, score);
  cerr << "Score: " << *score << endl;
  double threshold = 40.0;
  if (*score <= threshold)
    return true;
  
  return false;

}
开发者ID:droter,项目名称:cg_mrslam,代码行数:76,代码来源:scan_matcher.cpp



注:本文中的VertexSE2类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。


鲜花

握手

雷人

路过

鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
上一篇:
C++ VertexSE3类代码示例发布时间:2022-05-31
下一篇:
C++ VertexRange类代码示例发布时间:2022-05-31
热门推荐
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服(服务时间 9:00~18:00)

在线QQ客服
地址:深圳市南山区西丽大学城创智工业园
电邮:jeky_zhao#qq.com
移动电话:139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap