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