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;
}
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::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;
}
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.;
}
HyperGraphElementAction* EdgeSE2WriteGnuplotAction::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;
}
EdgeSE2* e = static_cast<EdgeSE2*>(element);
VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1));
*(params->os) << fromEdge->estimate().translation().x() << " " << fromEdge->estimate().translation().y()
<< " " << fromEdge->estimate().rotation().angle() << std::endl;
*(params->os) << toEdge->estimate().translation().x() << " " << toEdge->estimate().translation().y()
<< " " << toEdge->estimate().rotation().angle() << std::endl;
*(params->os) << std::endl;
return this;
}
inline bool addObservation(Point2d observation, double xFasher,
double yFasher, LandmarkType type)
{
{
EdgeSE2 * e = new EdgeSE2;
e->vertices()[0] = optimizer.vertex(type);
e->vertices()[1] = optimizer.vertex(CurrentVertexId);
switch (type)
{
case RightL:
observation.y += B2;
break;
case FrontL:
observation.x -= A2;
break;
case LeftL:
observation.y -= B2;
break;
case BackL:
observation.x += A2;
break;
default:
break;
}
e->setMeasurement(SE2(observation.x, observation.y, 0));
Matrix3d information;
information.fill(0.);
information(0, 0) = xFasher;
information(1, 1) = yFasher;
information(2, 2) = 1;
e->setInformation(information);
g2o::RobustKernelCauchy* rk = new g2o::RobustKernelCauchy;
e->setRobustKernel(rk);
optimizer.addEdge(e);
}
atLeastOneObservation = true;
return true;
}
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);
}
}
}
void GraphSLAM::checkClosures(){
if (_closures.checkList(windowLoopClosure)){
cout << endl << "Loop Closure Checking." << endl;
// for(std::list<VertexTime>::iterator it = _closures.vertexList().begin(); it!= _closures.vertexList().end(); it++){
// VertexTime vt = *it;
// cout << "In list: Vertex: " << vt.v->id() << " time: " << vt.time << endl;
// }
lcc.init(_closures.vertices(), _closures.edgeSet(), inlierThreshold);
lcc.check();
// for (LoopClosureChecker::EdgeDoubleMap::iterator it = lcc.closures().begin(); it!= lcc.closures().end(); it++){
// EdgeSE2* e = (EdgeSE2*) (it->first);
// VertexSE2* vfrom=dynamic_cast<VertexSE2*>(e->vertices()[0]);
// VertexSE2* vto=dynamic_cast<VertexSE2*>(e->vertices()[1]);
// cerr << "Edge from: " << vfrom->id() << " " << vto->id()
// << " Estimate: " << e->measurement().translation().x() << " " << e->measurement().translation().y() << " " << e->measurement().rotation().angle()
// << " Chi2 = " << it->second << endl;
// }
cout << "Best Chi2 = " << lcc.chi2() << endl;
cout << "Inliers = " << lcc.inliers() << endl;
if (lcc.inliers() >= minInliers){
LoopClosureChecker::EdgeDoubleMap results = lcc.closures();
cout << "Results:" << endl;
for (LoopClosureChecker::EdgeDoubleMap::iterator it= results.begin(); it!= results.end(); it++){
EdgeSE2* e = (EdgeSE2*) (it->first);
cout << "Edge from: " << e->vertices()[0]->id() << " to: " << e->vertices()[1]->id() << ". Chi2 = " << it->second << endl;
if (it->second < inlierThreshold){
cout << "Is an inlier. Adding to Graph" << endl;
_graph->addEdge(e);
}
}
}
}
}
void GraphSLAM::findConstraints(){
boost::mutex::scoped_lock lockg(graphMutex);
//graph is quickly optimized first so last added edge is satisfied
_graph->initializeOptimization();
_graph->optimize(1);
OptimizableGraph::VertexSet vset;
_vf.findVerticesScanMatching( _lastVertex, vset);
checkCovariance(vset);
addNeighboringVertices(vset, 8);
checkHaveLaser(vset);
std::set<OptimizableGraph::VertexSet> setOfVSet;
_vf.findSetsOfVertices(vset, setOfVSet);
OptimizableGraph::EdgeSet loopClosingEdges;
for (std::set<OptimizableGraph::VertexSet>::iterator it = setOfVSet.begin(); it != setOfVSet.end(); it++) {
OptimizableGraph::VertexSet myvset = *it;
OptimizableGraph::Vertex* closestV = _vf.findClosestVertex(myvset, _lastVertex);
if (closestV->id() == _lastVertex->id() - 1) //Already have this edge
continue;
SE2 transf;
if (!isMyVertex(closestV) || (isMyVertex(closestV) && abs(_lastVertex->id() - closestV->id()) > 10)){
/*VertexEllipse* ellipse = findEllipseData(_lastVertex);
if (ellipse){
for (OptimizableGraph::VertexSet::iterator itv = myvset.begin(); itv != myvset.end(); itv++){
VertexSE2 *vertex = (VertexSE2*) *itv;
SE2 relativetransf = _lastVertex->estimate().inverse() * vertex->estimate();
ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y());
ellipse->addMatchingVertexID(vertex->id());
}
}*/
std::vector<SE2> results;
/*OptimizableGraph::VertexSet referenceVset;
referenceVset.insert(_lastVertex);
int j = 1;
int comm_gap = 5;
while (j <= comm_gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
referenceVset.insert(vj);
else
break;
j++;
}*/
//Loop Closing Edge
bool shouldIAdd = _LCMatcher.scanMatchingLC(myvset, closestV, _lastVertex, results, maxScore);
//bool shouldIAdd = _mf.scanMatchingLC(myvset, closestV, referenceVset, _lastVertex, results, maxScore);
if (shouldIAdd){
for (unsigned int i =0; i< results.size(); i++){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(results[i]);
ne->setInformation(_SMinf);
loopClosingEdges.insert(ne);
_SMEdges.insert(ne);
}
}else {
std::cout << "Rejecting LC edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}else{
//Edge between close vertices
bool shouldIAdd = _closeMatcher.closeScanMatching(myvset, closestV, _lastVertex, &transf, maxScore);
if (shouldIAdd){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(transf);
ne->setInformation(_SMinf);
_graph->addEdge(ne);
_SMEdges.insert(ne);
}else {
std::cout << "Rejecting edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}
}
if (loopClosingEdges.size())
addClosures(loopClosingEdges);
checkClosures();
updateClosures();
}
void GraphSLAM::addDataSM(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 << 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;
OptimizableGraph::VertexSet vset;
vset.insert(_lastVertex);
int j = 1;
int gap = 5;
while (j <= gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
vset.insert(vj);
else
break;
j++;
}
SE2 transf;
bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v, &transf, maxScore);
if (shouldIAdd){
e->setMeasurement(transf);
e->setInformation(_SMinf);
}else{ //Trust the odometry
e->setMeasurement(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(newcov.inverse());
e->setInformation(_odominf);
}
_graph->addEdge(e);
_lastOdom = currentOdom;
_lastVertex = v;
}
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;
}