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; }
void Slam2DViewer::draw() { if (! graph) return; // drawing the graph glColor4f(0.00f, 0.67f, 1.00f, 1.f); glBegin(GL_TRIANGLES); for (SparseOptimizer::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) { VertexSE2* v = dynamic_cast<VertexSE2*>(it->second); if (v) { drawSE2(v); } } glEnd(); glColor4f(1.00f, 0.67f, 0.00f, 1.f); glPointSize(2.f); glBegin(GL_POINTS); for (SparseOptimizer::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) { VertexPointXY* v = dynamic_cast<VertexPointXY*>(it->second); if (v) { glVertex3f(v->estimate()(0), v->estimate()(1), 0.f); } } glEnd(); glPointSize(1.f); if (drawCovariance) { for (SparseOptimizer::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) { VertexSE2* v = dynamic_cast<VertexSE2*>(it->second); if (v) { // TODO //drawCov(v->estimate().translation(), v->uncertainty()); } } } }
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; }