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