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;
}
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.;
}
bool SolverSLAM2DLinear::solveOrientation()
{
assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph");
assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
VectorXD b, x; // will be used for theta and x/y update
b.setZero(_optimizer->indexMapping().size());
x.setZero(_optimizer->indexMapping().size());
typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;
ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
blockIndeces[i] = i+1;
SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size());
// building the structure, diagonal for each active vertex
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int poseIdx = v->hessianIndex();
ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
m->setZero();
}
HyperGraph::VertexSet fixedSet;
// off diagonal for each edge
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
# ifndef NDEBUG
EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
assert(e && "Active edges contain non-odometry edge"); //
# else
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
# endif
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
int ind1 = from->hessianIndex();
int ind2 = to->hessianIndex();
if (ind1 == -1 || ind2 == -1) {
if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices
if (ind2 == -1) fixedSet.insert(to);
continue;
}
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
ScalarMatrix* m = H.block(ind1, ind2, true);
m->setZero();
}
// walk along the Minimal Spanning Tree to compute the guess for the robot orientation
assert(fixedSet.size() == 1);
VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
VectorXD thetaGuess;
thetaGuess.setZero(_optimizer->indexMapping().size());
UniformCostFunction uniformCost;
HyperDijkstra hyperDijkstra(_optimizer);
hyperDijkstra.shortestPaths(root, &uniformCost);
HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
ThetaTreeAction thetaTreeAction(thetaGuess.data());
HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction);
// construct for the orientation
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]);
double omega = e->information()(2,2);
double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess);
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
double omega_r = - omega * error;
if (fromNotFixed) {
b(from->hessianIndex()) -= omega_r;
(*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega;
if (toNotFixed) {
if (from->hessianIndex() > to->hessianIndex())
(*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega;
else
(*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega;
}
}
if (toNotFixed ) {
b(to->hessianIndex()) += omega_r;
(*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega;
}
}
}
// solve orientation
typedef LinearSolverCSparse<ScalarMatrix> SystemSolver;
SystemSolver linearSystemSolver;
linearSystemSolver.init();
bool ok = linearSystemSolver.solve(H, x.data(), b.data());
if (!ok) {
cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
return false;
}
// update the orientation of the 2D poses and set translation to 0, GN shall solve that
root->setToOrigin();
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
int poseIdx = v->hessianIndex();
SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
v->setEstimate(poseUpdate);
}
return true;
}