HyperGraphElementAction* RobotLaserDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_) {
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams) {
return this;
}
RobotLaser* that = static_cast<RobotLaser*>(element);
RawLaser::Point2DVector points=that->cartesian();
glPushMatrix();
const SE2& laserPose = that->laserParams().laserPose;
glTranslatef((float)laserPose.translation().x(), (float)laserPose.translation().y(), 0.f);
glRotatef((float)RAD2DEG(laserPose.rotation().angle()),0.f,0.f,1.f);
glBegin(GL_POINTS);
glColor4f(1.f,0.f,0.f,0.5f);
int step = 1;
if (_beamsDownsampling )
step = _beamsDownsampling->value();
if (_pointSize) {
glPointSize(_pointSize->value());
}
for (size_t i=0; i<points.size(); i+=step) {
glVertex3f((float)points[i].x(), (float)points[i].y(), 0.f);
}
glEnd();
glPopMatrix();
return this;
}
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());
}
}
}
}*/
}
HyperGraphElementAction* RobotLaserDrawAction::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;
RobotLaser* that = static_cast<RobotLaser*>(element);
RawLaser::Point2DVector points=that->cartesian();
if (_maxRange && _maxRange->value() >=0 ) {
// prune the cartesian points;
RawLaser::Point2DVector npoints(points.size());
int k = 0;
float r2=_maxRange->value();
r2 *= r2;
for (size_t i=0; i<points.size(); i++){
double x = points[i].x();
double y = points[i].y();
if (x*x + y*y < r2)
npoints[k++] = points[i];
}
points = npoints;
npoints.resize(k);
}
glPushMatrix();
const SE2& laserPose = that->laserParams().laserPose;
glTranslatef((float)laserPose.translation().x(), (float)laserPose.translation().y(), 0.f);
glRotatef((float)RAD2DEG(laserPose.rotation().angle()),0.f,0.f,1.f);
glColor4f(1.f,0.f,0.f,0.5f);
int step = 1;
if (_beamsDownsampling )
step = _beamsDownsampling->value();
if (_pointSize) {
glPointSize(_pointSize->value());
}
glBegin(GL_POINTS);
for (size_t i=0; i<points.size(); i+=step){
glVertex3f((float)points[i].x(), (float)points[i].y(), 0.f);
}
glEnd();
glPopMatrix();
return this;
}
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);
}
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());
}
}
}
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;
}
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::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);
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;
}
