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;
}
int main(int argc, char** argv) {
CommandArgs arg;
std::string outputFilename;
std::string inputFilename;
arg.param("o", outputFilename, "", "output file name");
arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
arg.parseArgs(argc, argv);
OptimizableGraph graph;
if (!graph.load(inputFilename.c_str())){
cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
return 0;
}
HyperGraph::EdgeSet removedEdges;
HyperGraph::VertexSet removedVertices;
for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
HyperGraph::Edge* e = *it;
EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
if (edgePointXY) {
VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
FeaturePointXYData * feature = new FeaturePointXYData();
feature->setPositionMeasurement(edgePointXY->measurement());
feature->setPositionInformation(edgePointXY->information());
pose->addUserData(feature);
removedEdges.insert(edgePointXY);
removedVertices.insert(landmark);
}
}
for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
graph.removeEdge(e);
}
for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
graph.removeVertex(v);
}
if (outputFilename.length()){
graph.save(outputFilename.c_str());
}
}
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;
}
