void GraphSLAM::optimize(int nrunnings){
boost::mutex::scoped_lock lockg(graphMutex);
_graph->initializeOptimization();
_graph->optimize(nrunnings);
//////////////////////////////////////
//Update laser data
for (SparseOptimizer::VertexIDMap::const_iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) {
VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
RobotLaser* robotLaser = findLaserData(v);
if (robotLaser)
robotLaser->setOdomPose(v->estimate());
}
//////////////////////////////////////
//Update ellipse data
//Covariance computation
/*
CovarianceEstimator ce(_graph);
OptimizableGraph::VertexSet vset;
for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) (it->second);
vset.insert(v);
}
ce.setVertices(vset);
ce.setGauge(_lastVertex);
ce.compute();
///////////////////////////////////
for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
VertexEllipse* ellipse = findEllipseData(v);
if (ellipse && (v != lastVertex())){
MatrixXd PvX = ce.getCovariance(v);
Matrix3d Pv = PvX;
Matrix3f Pvf = Pv.cast<float>();
ellipse->setCovariance(Pvf);
ellipse->clearMatchingVertices();
}else {
if(ellipse && v == lastVertex()){
ellipse->clearMatchingVertices();
for (size_t i = 0; i<ellipse->matchingVerticesIDs().size(); i++){
int id = ellipse->matchingVerticesIDs()[i];
VertexSE2* vid = dynamic_cast<VertexSE2*>(_graph->vertex(id));
SE2 relativetransf = _lastVertex->estimate().inverse() * vid->estimate();
ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y());
}
}
}
}*/
}
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* VertexSE2WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
if (!params || !params->os){
std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid output stream specified" << std::endl;
return 0;
}
VertexSE2* v = static_cast<VertexSE2*>(element);
*(params->os) << v->estimate().translation().x() << " " << v->estimate().translation().y()
<< " " << v->estimate().rotation().angle() << std::endl;
return this;
}
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::checkCovariance(OptimizableGraph::VertexSet& vset){
///////////////////////////////////
// we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one
CovarianceEstimator ce(_graph);
ce.setVertices(vset);
ce.setGauge(_lastVertex);
ce.compute();
assert(!_lastVertex->fixed() && "last Vertex is fixed");
assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
OptimizableGraph::VertexSet tmpvset = vset;
for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
MatrixXd Pv = ce.getCovariance(vertex);
Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();
Vector2d hxy (delta.translation().x(), delta.translation().y());
double perceptionRange =1;
if (hxy.x()-perceptionRange>0)
hxy.x() -= perceptionRange;
else if (hxy.x()+perceptionRange<0)
hxy.x() += perceptionRange;
else
hxy.x() = 0;
if (hxy.y()-perceptionRange>0)
hxy.y() -= perceptionRange;
else if (hxy.y()+perceptionRange<0)
hxy.y() += perceptionRange;
else
hxy.y() = 0;
double d2 = hxy.transpose() * Pxy.inverse() * hxy;
if (d2 > 5.99)
vset.erase(*it);
}
}
void GraphRosPublisher::publishGraph(){
assert(_graph && "Cannot publish: undefined graph");
geometry_msgs::PoseArray traj;
sensor_msgs::PointCloud pcloud;
traj.poses.resize(_graph->vertices().size());
pcloud.points.clear();
int i = 0;
for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
VertexSE2* v = (VertexSE2*) (it->second);
traj.poses[i].position.x = v->estimate().translation().x();
traj.poses[i].position.y = v->estimate().translation().y();
traj.poses[i].position.z = 0;
traj.poses[i].orientation = tf::createQuaternionMsgFromYaw(v->estimate().rotation().angle());
RobotLaser *laser = dynamic_cast<RobotLaser*>(v->userData());
if (laser){
RawLaser::Point2DVector vscan = laser->cartesian();
SE2 trl = laser->laserParams().laserPose;
SE2 transf = v->estimate() * trl;
RawLaser::Point2DVector wscan;
ScanMatcher::applyTransfToScan(transf, vscan, wscan);
size_t s= 0;
while ( s<wscan.size()){
geometry_msgs::Point32 point;
point.x = wscan[s].x();
point.y = wscan[s].y();
pcloud.points.push_back(point);
s = s+10;
}
}
i++;
}
traj.header.frame_id = _fixedFrame;
pcloud.header.frame_id = traj.header.frame_id;
_publm.publish(pcloud);
_pubtj.publish(traj);
}
HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _drawActions){
_drawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
VertexSE2* that = static_cast<VertexSE2*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glPushMatrix();
glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),0.f);
glRotatef((float)RAD2DEG(that->estimate().rotation().angle()),0.f,0.f,1.f);
if (_show && _show->value()) {
float tx=0.1f, ty=0.05f;
if (_triangleX && _triangleY){
tx=_triangleX->value();
ty=_triangleY->value();
}
glBegin(GL_TRIANGLE_FAN);
glVertex3f( tx ,0.f ,0.f);
glVertex3f(-tx ,-ty, 0.f);
glVertex3f(-tx , ty, 0.f);
glEnd();
}
OptimizableGraph::Data* d=that->userData();
while (d && _drawActions ){
(*_drawActions)(d, params_);
d=d->next();
}
glPopMatrix();
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;
}
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;
}
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::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;
}
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;
}
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;
}
void transformPointsFromVSet(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _referenceVertex, RawLaser::Point2DVector& scansInRefVertex){
VertexSE2* referenceVertex=dynamic_cast<VertexSE2*>(_referenceVertex);
scansInRefVertex.clear();
for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
if (laserv){
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex;
if (vertex->id() == referenceVertex->id()){
ScanMatcher::applyTransfToScan(trl, vscan, scanInRefVertex);
}else{
SE2 trel = referenceVertex->estimate().inverse() * vertex->estimate();
SE2 transf = trel * trl;
ScanMatcher::applyTransfToScan(transf, vscan, scanInRefVertex);
}
scansInRefVertex.insert(scansInRefVertex.end(), scanInRefVertex.begin(), scanInRefVertex.end());
}
}
}
void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /* to */)
{
VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);
if (from.count(fromEdge) > 0)
toEdge->setEstimate(fromEdge->estimate() * _measurement);
else
fromEdge->setEstimate(toEdge->estimate() * _inverseMeasurement);
}
void EdgeSE2PlaceVicinity::initialEstimate(
const OptimizableGraph::VertexSet& from,
OptimizableGraph::Vertex* /*to*/) {
VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);
if (from.count(fromEdge) > 0)
toEdge->setEstimate(fromEdge->estimate());
else
fromEdge->setEstimate(toEdge->estimate());
}
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.;
}
void EdgeSE2DistanceOrientation::initialEstimate(
const OptimizableGraph::VertexSet& from,
OptimizableGraph::Vertex* /*to*/) {
VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);
double dist = sqrt(_measurement[0] * _measurement[0] + _measurement[1] * _measurement[1]);
double theta = _measurement[2];
double x = dist * cos(theta), y = dist * sin(theta);
SE2 tmpMeasurement(x,y,theta);
SE2 inverseTmpMeasurement = tmpMeasurement.inverse();
if (from.count(fromEdge) > 0)
toEdge->setEstimate(fromEdge->estimate() * tmpMeasurement);
else
fromEdge->setEstimate(toEdge->estimate() * inverseTmpMeasurement);
}
HyperGraphElementAction* EdgeSE2OdomDifferentialCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* )
{
if (typeid(*element).name()!=_typeName)
return 0;
EdgeSE2OdomDifferentialCalib* e = static_cast<EdgeSE2OdomDifferentialCalib*>(element);
VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1));
glColor3f(0.5,0.5,0.5);
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;
}
int main(int argc, char **argv) {
/************************************************************************
* Input handling *
************************************************************************/
float rows, cols, gain, square_size;
float resolution, max_range, usable_range, angle, threshold;
string g2oFilename, mapFilename;
g2o::CommandArgs arg;
arg.param("resolution", resolution, 0.05f, "resolution of the map (how much is in meters a pixel)");
arg.param("threshold", threshold, -1.0f, "threshold to apply to the frequency map (values under the threshold are discarded)");
arg.param("rows", rows, 0, "impose the resulting map to have this number of rows");
arg.param("cols", cols, 0, "impose the resulting map to have this number of columns");
arg.param("max_range", max_range, -1.0f, "max laser range to consider for map building");
arg.param("usable_range", usable_range, -1.0f, "usable laser range for map building");
arg.param("gain", gain, 1, "gain to impose to the pixels of the map");
arg.param("square_size", square_size, 1, "square size of the region where increment the hits");
arg.param("angle", angle, 0, "rotate the map of x degrees");
arg.paramLeftOver("input_graph.g2o", g2oFilename, "", "input g2o graph to use to build the map", false);
arg.paramLeftOver("output_map", mapFilename, "", "output filename where to save the map (without extension)", false);
arg.parseArgs(argc, argv);
angle = angle*M_PI/180.0;
/************************************************************************
* Loading Graph *
************************************************************************/
// Load graph
typedef BlockSolver< BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
SlamLinearSolver *linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
OptimizationAlgorithmGaussNewton *solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
SparseOptimizer *graph = new SparseOptimizer();
graph->setAlgorithm(solverGauss);
graph->load(g2oFilename.c_str());
// Sort verteces
vector<int> vertexIds(graph->vertices().size());
int k = 0;
for(OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
vertexIds[k++] = (it->first);
}
sort(vertexIds.begin(), vertexIds.end());
/************************************************************************
* Compute map size *
************************************************************************/
// Check the entire graph to find map bounding box
Eigen::Matrix2d boundingBox = Eigen::Matrix2d::Zero();
std::vector<RobotLaser*> robotLasers;
std::vector<SE2> robotPoses;
double xmin=std::numeric_limits<double>::max();
double xmax=std::numeric_limits<double>::min();
double ymin=std::numeric_limits<double>::max();
double ymax=std::numeric_limits<double>::min();
SE2 baseTransform(0,0,angle);
for(size_t i = 0; i < vertexIds.size(); ++i) {
OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
if(!v) { continue; }
v->setEstimate(baseTransform*v->estimate());
OptimizableGraph::Data *d = v->userData();
while(d) {
RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
if(!robotLaser) {
d = d->next();
continue;
}
robotLasers.push_back(robotLaser);
robotPoses.push_back(v->estimate());
double x = v->estimate().translation().x();
double y = v->estimate().translation().y();
xmax = xmax > x+usable_range ? xmax : x+usable_range;
ymax = ymax > y+usable_range ? ymax : y+usable_range;
xmin = xmin < x-usable_range ? xmin : x-usable_range;
ymin = ymin < y-usable_range ? ymin : y-usable_range;
d = d->next();
}
}
boundingBox(0,0)=xmin;
boundingBox(0,1)=xmax;
boundingBox(1,0)=ymin;
boundingBox(1,1)=ymax;
std::cout << "Found " << robotLasers.size() << " laser scans"<< std::endl;
std::cout << "Bounding box: " << std::endl << boundingBox << std::endl;
if(robotLasers.size() == 0) {
std::cout << "No laser scans found ... quitting!" << std::endl;
return 0;
}
/************************************************************************
* Compute the map *
************************************************************************/
// Create the map
Eigen::Vector2i size;
if(rows != 0 && cols != 0) { size = Eigen::Vector2i(rows, cols); }
else {
size = Eigen::Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ resolution,
(boundingBox(1, 1) - boundingBox(1, 0))/ resolution);
}
std::cout << "Map size: " << size.transpose() << std::endl;
if(size.x() == 0 || size.y() == 0) {
std::cout << "Zero map size ... quitting!" << std::endl;
return 0;
}
//Eigen::Vector2f offset(-size.x() * resolution / 2.0f, -size.y() * resolution / 2.0f);
Eigen::Vector2f offset(boundingBox(0, 0),boundingBox(1, 0));
FrequencyMapCell unknownCell;
FrequencyMap map = FrequencyMap(resolution, offset, size, unknownCell);
for(size_t i = 0; i < vertexIds.size(); ++i) {
OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
if(!v) { continue; }
OptimizableGraph::Data *d = v->userData();
SE2 robotPose = v->estimate();
while(d) {
RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
if(!robotLaser) {
d = d->next();
continue;
}
map.integrateScan(robotLaser, robotPose, max_range, usable_range, gain, square_size);
d = d->next();
}
}
/************************************************************************
* Save map image *
************************************************************************/
cv::Mat mapImage(map.rows(), map.cols(), CV_8UC1);
mapImage.setTo(cv::Scalar(0));
for(int c = 0; c < map.cols(); c++) {
for(int r = 0; r < map.rows(); r++) {
if(map(r, c).misses() == 0 && map(r, c).hits() == 0) {
mapImage.at<unsigned char>(r, c) = 127;
} else {
float fraction = (float)map(r, c).hits()/(float)(map(r, c).hits()+map(r, c).misses());
if (threshold > 0 && fraction > threshold)
mapImage.at<unsigned char>(r, c) = 0;
else if (threshold > 0 && fraction <= threshold)
mapImage.at<unsigned char>(r, c) = 255;
else {
float val = 255*(1-fraction);
mapImage.at<unsigned char>(r, c) = (unsigned char)val;
}
}
// else if(map(r, c).hits() > threshold) {
// mapImage.at<unsigned char>(r, c) = 255;
// }
// else {
// mapImage.at<unsigned char>(r, c) = 0;
// }
}
}
cv::imwrite(mapFilename + ".png", mapImage);
/************************************************************************
* Write yaml file *
************************************************************************/
std::ofstream ofs(string(mapFilename + ".yaml").c_str());
Eigen::Vector3f origin(0.0f, 0.0f, 0.0f);
ofs << "image: " << mapFilename << ".png" << std::endl
<< "resolution: " << resolution << std::endl
<< "origin: [" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl
<< "negate: 0" << std::endl
<< "occupied_thresh: " << 0.65f << std::endl
<< "free_thresh: " << 0.2f << std::endl;
return 0;
}
bool ScanMatcher::verifyMatching(OptimizableGraph::VertexSet& vset1, OptimizableGraph::Vertex* _referenceVertex1,
OptimizableGraph::VertexSet& vset2, OptimizableGraph::Vertex* _referenceVertex2,
SE2 trel12, double *score){
VertexSE2* referenceVertex2=dynamic_cast<VertexSE2*>(_referenceVertex2);
resetGrid();
CharGrid auxGrid = _grid;
//Transform points from vset2 in the reference of referenceVertex1 using trel12
RawLaser::Point2DVector scansvset2inref1;
for (OptimizableGraph::VertexSet::iterator it = vset2.begin(); it != vset2.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex1;
if (vertex->id() == referenceVertex2->id()){
applyTransfToScan(trel12 * trl, vscan, scanInRefVertex1);
}else{
//Transform scans to the referenceVertex2 coordinates
SE2 tref2_v = referenceVertex2->estimate().inverse() * vertex->estimate();
applyTransfToScan(trel12 * tref2_v * trl, vscan, scanInRefVertex1);
}
scansvset2inref1.insert(scansvset2inref1.end(), scanInRefVertex1.begin(), scanInRefVertex1.end());
}
//Scans in vset1
RawLaser::Point2DVector scansvset1;
transformPointsFromVSet(vset1, _referenceVertex1, scansvset1);
//Add local map from vset2 into the grid
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset2inref1.begin(), scansvset2inref1.end(), _kernel);
//Find points from vset1 not explained by map vset2
RawLaser::Point2DVector nonmatchedpoints;
_grid.searchNonMatchedPoints(scansvset1, nonmatchedpoints, .3);
//Add those points to a grid to count them
auxGrid.addAndConvolvePoints<RawLaser::Point2DVector>(nonmatchedpoints.begin(), nonmatchedpoints.end(), _kernel);
// ofstream image1;
// std::stringstream filename1;
// filename1 << "map2.ppm";
// image1.open(filename1.str().c_str());
// _LCGrid.grid().saveAsPPM(image1, false);
// ofstream image2;
// std::stringstream filename2;
// filename2 << "mapnonmatched.ppm";
// image2.open(filename2.str().c_str());
// auxGrid.grid().saveAsPPM(image2, false);
// //Just for saving the image
// resetLCGrid();
// _LCGrid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset1.begin(), scansvset1.end(), _LCKernel);
// ofstream image3;
// std::stringstream filename3;
// filename3 << "map1.ppm";
// image3.open(filename3.str().c_str());
// _LCGrid.grid().saveAsPPM(image3, false);
//Counting points around trel12
Vector2f lower(-.3+trel12.translation().x(), -.3+trel12.translation().y());
Vector2f upper(+.3+trel12.translation().x(), +.3+trel12.translation().y());
auxGrid.countPoints(lower, upper, score);
cerr << "Score: " << *score << endl;
double threshold = 40.0;
if (*score <= threshold)
return true;
return false;
}
bool ScanMatcher::scanMatchingLChierarchical(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){
//cerr << "Loop Closing Scan Matching" << endl;
//cerr << "Size of Vset " << referenceVset.size() << endl;
VertexSE2* currentVertex=dynamic_cast<VertexSE2*>(_currentVertex);
VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex);
resetGrid();
trel.clear();
RawLaser::Point2DVector scansInRefVertex;
transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex);
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel);
RawLaser::Point2DVector scansInCurVertex;
transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex);
Vector2dVector reducedScans;
CharGrid::subsample(reducedScans, scansInCurVertex, 0.1);
//cerr << "subsampling: " << scansInCurVertex.size() << " -> " << reducedScans.size() << endl;
SE2 delta = referenceVertex->estimate().inverse() * currentVertex->estimate();
Vector3d initGuess(delta.translation().x(), delta.translation().y(), delta.rotation().angle());
Vector3f lower(-2.+initGuess.x(), -2.+initGuess.y(), -1.+initGuess.z());
Vector3f upper(+2.+initGuess.x(), 2.+initGuess.y(), 1.+initGuess.z());
RegionVector regions;
Region reg;
reg.lowerLeft = lower;
reg.upperRight = upper;
regions.push_back(reg);
std::vector<MatcherResult> mresvec;
double thetaRes = 0.025; // was 0.0125*.5
// clock_t t_ini, t_fin;
// double secs;
// t_ini = clock();
_grid.hierarchicalSearch(mresvec, reducedScans, regions, thetaRes, maxScore, 0.5, 0.5, 0.2, 3);
// t_fin = clock();
// secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
// printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
Vector3d adj=mresvec[0].transformation;
SE2 transf;
transf.setTranslation(Vector2d(adj.x(), adj.y()));
transf.setRotation(adj.z());
// cerr << " bestScore = " << mresvec[0].score << endl;
//cerr << "Found Loop Closure Edge. Transf: " << adj.x() << " " << adj.y() << " " << adj.z() << endl << endl;
trel.push_back(transf);
}
if (trel.size())
return true;
return false;
}
bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){
cerr << "Loop Closing Scan Matching" << endl;
//cerr << "Size of Vset " << referenceVset.size() << endl;
VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex);
resetGrid();
trel.clear();
RawLaser::Point2DVector scansInRefVertex;
transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex);
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel);
RawLaser::Point2DVector scansInCurVertex;
transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex);
Vector2dVector reducedScans;
CharGrid::subsample(reducedScans, scansInCurVertex, 0.1);
RegionVector regions;
RegionVector regionspi;
for (OptimizableGraph::VertexSet::iterator it = referenceVset.begin(); it != referenceVset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
Region reg;
SE2 relposv(.0, .0, .0);
if (vertex->id() != referenceVertex->id())
relposv = referenceVertex->estimate().inverse() * vertex->estimate();
Vector3f lower(-.5+relposv.translation().x(), -2.+relposv.translation().y(), -1.+relposv.rotation().angle());
Vector3f upper( .5+relposv.translation().x(), 2.+relposv.translation().y(), 1.+relposv.rotation().angle());
reg.lowerLeft = lower;
reg.upperRight = upper;
regions.push_back(reg);
lower[2] += M_PI;
upper[2] += M_PI;
reg.lowerLeft = lower;
reg.upperRight = upper;
regionspi.push_back(reg);
}
std::vector<MatcherResult> mresvec;
double thetaRes = 0.025; // was 0.0125*.5
//Results discretization
double dx = 0.5, dy = 0.5, dth = 0.2;
std::map<DiscreteTriplet, MatcherResult> resultsMap;
clock_t t_ini, t_fin;
double secs;
t_ini = clock();
_grid.greedySearch(mresvec, reducedScans, regions, thetaRes, maxScore, dx, dy, dth);
t_fin = clock();
secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
cerr << "Found Loop Closure Edge. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;
CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
}
t_ini = clock();
_grid.greedySearch(mresvec, reducedScans, regionspi, thetaRes, maxScore, dx, dy, dth);
t_fin = clock();
secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
cerr << "Found Loop Closure Edge PI. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;
CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
}
for (std::map<DiscreteTriplet, MatcherResult>::iterator it = resultsMap.begin(); it!= resultsMap.end(); it++){
MatcherResult res = it->second;
Vector3d adj=res.transformation;
SE2 transf;
transf.setTranslation(Vector2d(adj.x(), adj.y()));
transf.setRotation(normalize_theta(adj.z()));
trel.push_back(transf);
std::cerr << "Final result: " << transf.translation().x() << " " << transf.translation().y() << " " << transf.rotation().angle() << std::endl;
}
if (trel.size())
return true;
return false;
}
HyperGraphElementAction* EdgeSE2Landmark_malcolmDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
// std::cout << "DRAW up :D" << std::endl;exit(0);
if (typeid(*element).name()!=_typeName){
std::cout << "Wrong name :(" <<std::endl;;
return 0;
}
// refreshPropertyPtrs(params_);
// if (! _previousParams)
// return this;
//
// if (_show && !_show->value())
// return this;
//
// EdgeSE2Prior* e = static_cast<EdgeSE2Prior*>(element);
// VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
// VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1));
// // EdgeSE2PointXY* e = static_cast<EdgeSE2PointXY*>(element);
// // VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
// // VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertex(1));
// if (! fromEdge)
// return this;
// Vector3D p3d=e->measurement().toVector();
// Vector2D p;
// p << p3d(0), p3d(1);
// glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR);
// glDisable(GL_LIGHTING);
// if (!toEdge){
// p=fromEdge->estimate()/*.toVector()*/*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=toEdge->estimate().toVector();
// glColor3f(LANDMARK_EDGE_COLOR);
// }
// glBegin(GL_LINES);
// glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),0.f);
// glVertex3f((float)p.x(),(float)p.y(),0.f);
// glEnd();
// glPopAttrib();
// return this;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE2Landmark_malcolm* e = static_cast<EdgeSE2Landmark_malcolm*>(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_LANDMARK_MALCOLM_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_LANDMARK_MALCOLM_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_LANDMARK_MALCOLM_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;
}
int main(int argc, char** argv)
{
string rawFilename;
bool use_viso = false;
bool use_cfs = false;
double init_x, init_y;
cout << endl
<< "\033[32mA demo implementing the method of stereo-odo calibration.\033[0m"
<< endl << "* * * Author: \033[32mDoom @ ZJU.\033[0m"
<< endl << "Usage: sclam_odo_stereo_cal USE_VISO INPUT_FILENAME USE_CLOSEFORM" << endl << endl;
if(argc < 2){
cout << "\033[33m[Warning]:No input directory name, using the default one : /home/doom/CSO/data/params.yaml\033[0m" << endl << endl;
// Read in our param file
CYaml::get().parseParamFile("/home/doom/CSO/data/params.yaml");
// Read in params
use_viso = CYaml::get().general()["use_viso"].as<bool>();
rawFilename = CYaml::get().general()["data_folder"].as<std::string>();
use_cfs = CYaml::get().general()["use_closed_form"].as<bool>();
init_x = CYaml::get().general()["init_x"].as<double>();
init_y = CYaml::get().general()["init_y"].as<double>();
}
else if(argc == 2){
cout << "\033[31mInput params file directory: \033[0m" << argv[1] << endl << endl;
// Read in our param file
CYaml::get().parseParamFile(argv[1]);
// Read in params
use_viso = CYaml::get().general()["use_viso"].as<bool>();
rawFilename = CYaml::get().general()["data_folder"].as<std::string>();
use_cfs = CYaml::get().general()["use_closed_form"].as<bool>();
init_x = CYaml::get().general()["init_x"].as<double>();
init_y = CYaml::get().general()["init_y"].as<double>();
}
else
{
cerr << "\033[31m[FATAL]Bad parameters.\033[0m" << endl;
return 1;
}
// construct a new optimizer
SparseOptimizer optimizer;
initOptimizer(optimizer);
// read the times.txt, to determine how many pics in this directory.
stringstream ss;
ss << rawFilename << "/votimes.txt";
ifstream in(ss.str().c_str());
int Num = 0;
vector<double> timeque;
if(in.fail())
{
cerr << "\033[1;31m[ERROR]Wrong foldername or no times.txt in this directory.\033[0m" << endl;
return 1;
}
else
{
string stemp;
getline(in, stemp, '\n');
while(in.good()){
Num++;
getline(in, stemp, '\n');
}
cout << "There are " << Num << " pics in this direcotory." << endl << endl;
in.close();
in.open(ss.str().c_str());
double temp;
for(int i = 0; i < Num; i++)
{
in >> temp;
timeque.push_back(temp);
}
in.close();
}
// loading odom data
cerr << "\033[31mNow loading odometry data.\033[0m" << endl;
string odomName = rawFilename + "/newodom.txt";
DataQueue odometryQueue;
DataQueue stereovoQueue;
SE2 init_offset;
int numOdom = Gm2dlIO::readRobotOdom(odomName, odometryQueue);
if (numOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Done...Read " << numOdom << " odom readings from file" << endl << endl;
// initial first stereo frame pose
SE2 initialStereoPose;
bool first = true;
int numVo = 0;
if(use_viso)
{
cout << "\033[31mUsing libviso...\033[0m" << rawFilename << endl;
RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second);
// init a libviso2
StereoVo viso(rawFilename, Num, ro, timeque);
// get initial odometry pose in robot frame and stereo vo.
viso.getInitStereoPose(initialStereoPose);
numVo = viso.getMotion(stereovoQueue);
cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
}
else
{
cerr << "\033[31mLoading pose file...\033[0m" << endl;
ss.str("");
ss << rawFilename << "/CameraTrajectory.txt";
in.open(ss.str().c_str());
int numVo = 0;
Matrix4D posemat;
for(int i = 0; i < Num; i++)
{
for(int j = 0; j < 4; j++)
{
for(int k = 0; k < 4; k++)
in >> posemat(j, k);
}
if(first){
init_offset.setTranslation(Eigen::Vector2d(init_x, init_y));
init_offset.setRotation(Eigen::Rotation2Dd::Identity());
// SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
// RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second);
initialStereoPose = init_offset;
first = false;
}
// save stereo vo as robotOdom node.
RobotOdom* tempVo = new RobotOdom;
tempVo->setTimestamp(timeque[i]);
SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
tempVo->setOdomPose(init_offset*x);
stereovoQueue.add(tempVo);
numVo++;
}
in.close();
cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
}
// adding the measurements
vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
{
std::map<double, RobotData*>::const_iterator it = stereovoQueue.buffer().begin();
std::map<double, RobotData*>::const_iterator prevIt = it++;
for (; it != stereovoQueue.buffer().end(); ++it) {
MotionInformation mi;
RobotOdom* prevVo = dynamic_cast<RobotOdom*>(prevIt->second);
RobotOdom* curVo = dynamic_cast<RobotOdom*>(it->second);
mi.stereoMotion = prevVo->odomPose().inverse() * curVo->odomPose();
// get the motion of the robot in that time interval
RobotOdom* prevOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(prevVo->timestamp()));
RobotOdom* curOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(curVo->timestamp()));
mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
prevIt = it;
motions.push_back(mi);
}
}
// Construct the graph.
cerr << "\033[31mConstruct the graph...\033[0m" << endl;
// Vertex offset
addVertexSE2(optimizer, initialStereoPose, 0);
for(int i = 0; i < motions.size(); i++)
{
const SE2& odomMotion = motions[i].odomMotion;
const SE2& stereoMotion = motions[i].stereoMotion;
// add the edge
// stereo vo and odom measurement.
OdomAndStereoMotion meas;
meas.StereoMotion = stereoMotion;
meas.odomMotion = odomMotion;
addEdgeCalib(optimizer, 0, meas, Eigen::Matrix3d::Identity());
}
cerr << "Done..." << endl << endl;
// if you want check the component of your graph, uncomment the CHECK_GRAPH
#ifdef CHECK_GRAPH
for(auto it:optimizer.edges())
{
VertexSE2* v = static_cast<VertexSE2*>(it->vertex(0));
cout << v->id() << endl;
}
#endif
// optimize.
optimize(optimizer, 10);
Eigen::Vector3d result = getEstimation(optimizer);
cerr << "\033[1;32mCalibrated stereo offset (x, y, theta):\033[0m" << result[0] << " " << result [1] << " " << result[2] << endl << endl;
// If you want see how's its closed form solution, uncomment the CLOSED_FORM
if(use_cfs){
cerr << "\033[31mPerforming the closed form solution...\033[0m" << endl;
SE2 closedFormStereo;
ClosedFormCalibration::calibrate(motions, closedFormStereo);
cerr << "\033[1;32mDone... closed form Calibrated stereo offset (x, y, theta):\033[0m" << closedFormStereo.toVector().transpose() << endl;
}
return 0;
}
void Graph2occupancy::computeMap(){
// Sort verteces
vector<int> vertexIds(_graph->vertices().size());
int k = 0;
for(OptimizableGraph::VertexIDMap::iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) {
vertexIds[k++] = (it->first);
}
sort(vertexIds.begin(), vertexIds.end());
/************************************************************************
* Compute map size *
************************************************************************/
// Check the entire graph to find map bounding box
Matrix2d boundingBox = Matrix2d::Zero();
std::vector<RobotLaser*> robotLasers;
std::vector<SE2> robotPoses;
double xmin=std::numeric_limits<double>::max();
double xmax=std::numeric_limits<double>::min();
double ymin=std::numeric_limits<double>::max();
double ymax=std::numeric_limits<double>::min();
SE2 baseTransform(0,0,_angle);
bool initialPoseFound = false;
SE2 initialPose;
for(size_t i = 0; i < vertexIds.size(); ++i) {
OptimizableGraph::Vertex *_v = _graph->vertex(vertexIds[i]);
VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
if(!v) { continue; }
if (v->fixed() && !initialPoseFound){
initialPoseFound = true;
initialPose = baseTransform*v->estimate();
}
OptimizableGraph::Data *d = v->userData();
while(d) {
RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
if(!robotLaser) {
d = d->next();
continue;
}
robotLasers.push_back(robotLaser);
SE2 transformed_estimate = baseTransform*v->estimate();
robotPoses.push_back(transformed_estimate);
double x = transformed_estimate.translation().x();
double y = transformed_estimate.translation().y();
xmax = xmax > x+_usableRange ? xmax : x + _usableRange;
ymax = ymax > y+_usableRange ? ymax : y + _usableRange;
xmin = xmin < x-_usableRange ? xmin : x - _usableRange;
ymin = ymin < y-_usableRange ? ymin : y - _usableRange;
d = d->next();
}
}
boundingBox(0,0)=xmin;
boundingBox(1,0)=ymin;
boundingBox(0,1)=xmax;
boundingBox(1,1)=ymax;
if(robotLasers.size() == 0) {
std::cout << "No laser scans found ... quitting!" << std::endl;
return;
}
/************************************************************************
* Compute the map *
************************************************************************/
// Create the map
Vector2i size;
Vector2f offset;
if(_rows != 0 && _cols != 0) {
size = Vector2i(_rows, _cols);
}
else {
size = Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ _resolution,
(boundingBox(1, 1) - boundingBox(1, 0))/ _resolution);
}
if(size.x() == 0 || size.y() == 0) {
std::cout << "Zero map size ... quitting!" << std::endl;
return;
}
offset = Eigen::Vector2f(boundingBox(0, 0),boundingBox(1, 0));
FrequencyMapCell unknownCell;
_map = FrequencyMap(_resolution, offset, size, unknownCell);
for (size_t i = 0; i < robotPoses.size(); ++i) {
_map.integrateScan(robotLasers[i], robotPoses[i], _maxRange, _usableRange, _infinityFillingRange, _gain, _squareSize);
}
/************************************************************************
* Convert frequency map into int[8] *
************************************************************************/
*_mapImage = cv::Mat(_map.rows(), _map.cols(), CV_8UC1);
_mapImage->setTo(cv::Scalar(0));
for(int c = 0; c < _map.cols(); c++) {
for(int r = 0; r < _map.rows(); r++) {
if(_map(r, c).misses() == 0 && _map(r, c).hits() == 0) {
_mapImage->at<unsigned char>(r, c) = _unknownColor; }
else {
float fraction = (float)_map(r, c).hits()/(float)(_map(r, c).hits()+_map(r, c).misses());
if (_freeThreshold && fraction < _freeThreshold){
_mapImage->at<unsigned char>(r, c) = _freeColor; }
else if (_threshold && fraction > _threshold){
_mapImage->at<unsigned char>(r, c) = _occupiedColor; }
else {
_mapImage->at<unsigned char>(r, c) = _unknownColor; }
}
}
}
Eigen::Vector2f origin(0.0f, 0.0f);
if (initialPoseFound){
Eigen::Vector2i originMap = _map.world2map(Eigen::Vector2f(initialPose.translation().x(),
initialPose.translation().y()));
origin = Eigen::Vector2f(((-_resolution * originMap.y())+initialPose.translation().y()),
-(_resolution * (_mapImage->rows-originMap.x()) +initialPose.translation().x()));
}
_mapCenter = origin;
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
allocateSolverForSclam(optimizer);
// loading
if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
cerr << "Error while loading gm2dl file" << endl;
}
DataQueue robotLaserQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge = optimizer.findGauge();
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors" << endl;
}
addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge, &f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
if (1)
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (d.visited().count(v) == 0) {
cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
v->setFixed(true);
}
}
}
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->fixed()) {
cerr << "\t fixed vertex " << it->first << endl;
}
}
VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
signal(SIGINT, sigquit_handler);
cerr << "Doing full estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
int i=optimizer.optimize(maxIterations);
if (maxIterations > 0 && !i){
cerr << "optimize failed, result might be invalid" << endl;
}
if (laserOffset) {
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
}
if (odomParamsVertex) {
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
}
cerr << "vertices: " << optimizer.vertices().size() << endl;
cerr << "edges: " << optimizer.edges().size() << endl;
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a seperate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
const Vector3d& odomCalib = odomParamsVertex->estimate();
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurment
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
if (outputfilename.size() > 0) {
Gm2dlIO::updateLaserData(optimizer);
cerr << "Writing " << outputfilename << " ... ";
bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
cerr << (writeStatus ? "done." : "failed") << endl;
}
return 0;
}
bool ScanMatcher::closeScanMatching(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _originVertex, OptimizableGraph::Vertex* _currentVertex, SE2 *trel, double maxScore){
VertexSE2* currentVertex=dynamic_cast<VertexSE2*>(_currentVertex);
VertexSE2* originVertex =dynamic_cast<VertexSE2*>(_originVertex);
resetGrid();
RawLaser::Point2DVector scansInRefVertex;
transformPointsFromVSet(vset, _originVertex, scansInRefVertex);
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel);
//Current vertex
RobotLaser* lasercv = dynamic_cast<RobotLaser*>(currentVertex->userData());
if (!lasercv)
return false;
RawLaser::Point2DVector cvscan = lasercv->cartesian();
SE2 laserPoseCV = lasercv->laserParams().laserPose;
RawLaser::Point2DVector cvScanRobot;
applyTransfToScan(laserPoseCV, cvscan, cvScanRobot);
SE2 delta = originVertex->estimate().inverse() * currentVertex->estimate();
Vector3d initGuess(delta.translation().x(), delta.translation().y(), delta.rotation().angle());
std::vector<MatcherResult> mresvec;
clock_t t_ini, t_fin;
double secs;
t_ini = clock();
double thetaRes = 0.0125*.5; // was 0.01
Vector3f lower(-.3+initGuess.x(), -.3+initGuess.y(), -0.2+initGuess.z());
Vector3f upper(+.3+initGuess.x(), .3+initGuess.y(), 0.2+initGuess.z());
_grid.greedySearch(mresvec, cvScanRobot, lower, upper, thetaRes, maxScore, 0.5, 0.5, 0.2);
t_fin = clock();
secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
printf("Greedy search: %.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
Vector3d adj=mresvec[0].transformation;
trel->setTranslation(Vector2d(adj.x(), adj.y()));
trel->setRotation(adj.z());
//cerr << "bestScore = " << mresvec[0].score << endl << endl;
// if (currentVertex->id() > 120 && currentVertex->id() < 200){
// CharMatcher auxGrid = _grid;
// Vector2dVector transformedScan;
// transformedScan.resize(cvScanRobot.size());
// for (unsigned int i = 0; i<cvScanRobot.size(); i++){
// SE2 point;
// point.setTranslation(cvScanRobot[i]);
// SE2 transformedpoint = *trel * point;
// transformedScan[i] = transformedpoint.translation();
// }
// auxGrid.addPoints<RawLaser::Point2DVector>(transformedScan.begin(), transformedScan.end());
// ofstream image;
// std::stringstream filename;
// filename << "matchedmap" << currentVertex->id() << "_" << mresvec[0].score << ".ppm";
// image.open(filename.str().c_str());
// auxGrid.grid().saveAsPPM(image, false);
// }
return true;
}
cerr << endl;
return false;
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
allocateSolverForSclam(optimizer);
// loading
DataQueue odometryQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
Eigen::Vector3d odomCalib(1., 1., 1.);
SE2 initialLaserPose;
DataQueue robotLaserQueue;
int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue);
if (numRobotLaser == 0) {
cerr << "No robot laser read" << endl;
return 0;
} else {
RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second);
initialLaserPose = rl->odomPose().inverse() * rl->laserPose();
cerr << PVAR(initialLaserPose.toVector().transpose()) << endl;
}
// adding the measurements
vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
{
std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin();
std::map<double, RobotData*>::const_iterator prevIt = it++;
for (; it != robotLaserQueue.buffer().end(); ++it) {
MotionInformation mi;
RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second);
RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second);
mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose();
// get the motion of the robot in that time interval
RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp()));
RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp()));
mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
prevIt = it;
motions.push_back(mi);
}
}
if (1) {
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.));
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming full non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
optimizer.clear();
}
// linear least squares for some parameters
{
Eigen::MatrixXd A(motions.size(), 2);
Eigen::VectorXd x(motions.size());
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
A(i, 0) = velMeas.vl() * timeInterval;
A(i, 1) = velMeas.vr() * timeInterval;
x(i) = laserMotion.rotation().angle();
}
//linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
linearSolution = A.colPivHouseholderQr().solve(x);
//cout << PVAR(linearSolution.transpose()) << endl;
}
//constructing non-linear least squares
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexBaseline* odomParamsVertex = new VertexBaseline;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(1.);
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeCalib* calibEdge = new EdgeCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming partial non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate();
odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate();
odomCalib(2) = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomCalib.transpose() << endl;
{
SE2 closedFormLaser;
Eigen::Vector3d closedFormOdom;
ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom);
cerr << "\nObtaining closed form solution" << endl;
cerr << "Calibrated laser offset (x, y, theta):" << closedFormLaser.toVector().transpose() << endl;
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << closedFormOdom.transpose() << endl;
}
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a separate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurement
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
return 0;
}
void Localization::InitG2OGraph()
{
optimizer.clear();
LandmarkCount = 0;
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, 0, 0));
l->setFixed(true);
l->setId(CenterL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(A2, 0, 0));
l->setFixed(true);
l->setId(FrontL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(-A2, 0, 0));
l->setFixed(true);
l->setId(BackL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, -B2, 0));
l->setFixed(true);
l->setId(RightL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, B2, 0));
l->setFixed(true);
l->setId(LeftL);
optimizer.addVertex(l);
LandmarkCount++;
}
PreviousVertexId = -1;
CurrentVertexId = LandmarkCount;
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(
Eigen::Vector3d(location.x, location.y, 0));
l->setFixed(false);
l->setId(LandmarkCount);
optimizer.addVertex(l);
}
}
