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