void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed) { for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it); v->setFixed(fixed); } }
bool G2oSlamInterface::fixNode(const std::vector<int>& nodes) { for (size_t i = 0; i < nodes.size(); ++i) { OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]); if (v) v->setFixed(true); } return true; }
bool OptimizableGraph::load(istream& is, bool createEdges) { // scna for the paramers in the whole file if (!_parameters.read(is,&_renamedTypesLookup)) return false; #ifndef NDEBUG cerr << "Loaded " << _parameters.size() << " parameters" << endl; #endif is.clear(); is.seekg(ios_base::beg); set<string> warnedUnknownTypes; stringstream currentLine; string token; Factory* factory = Factory::instance(); HyperGraph::GraphElemBitset elemBitset; elemBitset[HyperGraph::HGET_PARAMETER] = 1; elemBitset.flip(); Vertex* previousVertex = 0; Data* previousData = 0; while (1) { int bytesRead = readLine(is, currentLine); if (bytesRead == -1) break; currentLine >> token; //cerr << "Token=" << token << endl; if (bytesRead == 0 || token.size() == 0 || token[0] == '#') continue; // handle commands encoded in the file bool handledCommand = false; if (token == "FIX") { handledCommand = true; int id; while (currentLine >> id) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id)); if (v) { # ifndef NDEBUG cerr << "Fixing vertex " << v->id() << endl; # endif v->setFixed(true); } else { cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl; } } } if (handledCommand) continue; // do the mapping to an internal type if it matches if (_renamedTypesLookup.size() > 0) { map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token); if (foundIt != _renamedTypesLookup.end()) { token = foundIt->second; } } if (! factory->knowsTag(token)) { if (warnedUnknownTypes.count(token) != 1) { warnedUnknownTypes.insert(token); cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl; } continue; } HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); if (dynamic_cast<Vertex*>(element)) { // it's a vertex type //cerr << "it is a vertex" << endl; previousData = 0; Vertex* v = static_cast<Vertex*>(element); int id; currentLine >> id; bool r = v->read(currentLine); if (! r) cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl; v->setId(id); if (!addVertex(v)) { cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl; delete v; } else { previousVertex = v; } }
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; }
int main (int argc , char ** argv){ int maxIterations; bool verbose; bool robustKernel; double lambdaInit; CommandArgs arg; bool fixSensor; bool fixPlanes; bool fixFirstPose; bool fixTrajectory; bool planarMotion; bool listSolvers; string strSolver; cerr << "graph" << endl; arg.param("i", maxIterations, 5, "perform n iterations"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.param("solver", strSolver, "lm_var", "select one specific solver"); arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg"); arg.param("robustKernel", robustKernel, false, "use robust error functions"); arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot"); arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory"); arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose"); arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)"); arg.param("planarMotion", planarMotion, false, "robot moves on a plane"); arg.param("listSolvers", listSolvers, false, "list the solvers"); arg.parseArgs(argc, argv); SparseOptimizer* g=new SparseOptimizer(); ParameterSE3Offset* odomOffset=new ParameterSE3Offset(); odomOffset->setId(0); g->addParameter(odomOffset); OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance(); OptimizationAlgorithmProperty solverProperty; OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty); g->setAlgorithm(solver); if (listSolvers){ solverFactory->listSolvers(cerr); return 0; } if (! g->solver()){ cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl; cerr << "available solvers: " << endl; solverFactory->listSolvers(cerr); cerr << "--------------" << endl; return 0; } cerr << "sim" << endl; Simulator* sim = new Simulator(g); cerr << "robot" << endl; Robot* r=new Robot(g); cerr << "planeSensor" << endl; Matrix3d R=Matrix3d::Identity(); R << 0, 0, 1, -1, 0, 0, 0, -1, 0; Isometry3d sensorPose=Isometry3d::Identity(); sensorPose.matrix().block<3,3>(0,0) = R; sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2); PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose); ps->_nplane << 0.03, 0.03, 0.005; r->_sensors.push_back(ps); sim->_robots.push_back(r); cerr << "p1" << endl; Plane3D plane; PlaneItem* pi =new PlaneItem(g,1); plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.)); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.)); pi =new PlaneItem(g,2); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); cerr << "p2" << endl; pi =new PlaneItem(g,3); plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.)); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); Quaterniond q, iq; if (planarMotion) { r->_planarMotion = true; r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025; q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix()); iq = Quaterniond(AngleAxisd(-0.2, Vector3d::UnitZ()).toRotationMatrix()); } else { r->_planarMotion = false; //r->_nmovecov << 0.1, 0.005, 1e-9, 0.05, 0.001, 0.001; r->_nmovecov << 0.1, 0.005, 1e-9, 0.001, 0.001, 0.05; q = Quaterniond((AngleAxisd(M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix()); iq = Quaterniond((AngleAxisd(-M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix()); } Isometry3d delta=Isometry3d::Identity(); sim->_lastVertexId=4; Isometry3d startPose=Isometry3d::Identity(); startPose.matrix().block<3,3>(0,0) = AngleAxisd(-0.75*M_PI, Vector3d::UnitZ()).toRotationMatrix(); sim->move(0,startPose); int k =20; int l = 2; double delta_t = 0.2; for (int j=0; j<l; j++) { Vector3d tr(1.,0.,0.); delta.matrix().block<3,3>(0,0) = q.toRotationMatrix(); if (j==(l-1)){ delta.matrix().block<3,3>(0,0) = Matrix3d::Identity(); } delta.translation()=tr*(delta_t*j); Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; i<k; i++){ cerr << "m"; if (a==0) sim->relativeMove(0,delta); else sim->relativeMove(0,iDelta); cerr << "s"; sim->sense(0); } } } for (int j=0; j<l; j++) { Vector3d tr(1.,0.,0.); delta.matrix().block<3,3>(0,0) = iq.toRotationMatrix(); if (j==l-1){ delta.matrix().block<3,3>(0,0) = Matrix3d::Identity(); } delta.translation()=tr*(delta_t*j); Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; i<k; i++){ cerr << "m"; if (a==0) sim->relativeMove(0,delta); else sim->relativeMove(0,iDelta); cerr << "s"; sim->sense(0); } } } ofstream os("test_gt.g2o"); g->save(os); if (fixSensor) { ps->_offsetVertex->setFixed(true); } else { Vector6d noffcov; noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5; ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov)); ps->_offsetVertex->setFixed(false); } if (fixFirstPose){ OptimizableGraph::Vertex* gauge = g->vertex(4); if (gauge) gauge->setFixed(true); } // else { // // multiply all vertices of the robot by this standard quantity // Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix()); // Vector3d tr(1,0,0); // Isometry3d delta; // delta.matrix().block<3,3>(0,0)=q.toRotationMatrix(); // delta.translation()=tr; // for (size_t i=0; i< g->vertices().size(); i++){ // VertexSE3 *v = dynamic_cast<VertexSE3 *>(g->vertex(i)); // if (v && v->id()>0){ // v->setEstimate (v->estimate()*delta); // } // } // } ofstream osp("test_preopt.g2o"); g->save(osp); //g->setMethod(SparseOptimizer::LevenbergMarquardt); g->initializeOptimization(); g->setVerbose(verbose); g->optimize(maxIterations); if (! fixSensor ){ SparseBlockMatrix<MatrixXd> spinv; std::pair<int, int> indexParams; indexParams.first = ps->_offsetVertex->hessianIndex(); indexParams.second = ps->_offsetVertex->hessianIndex(); std::vector<std::pair <int, int> > blockIndices; blockIndices.push_back(indexParams); if (!g->computeMarginals(spinv, blockIndices)){ cerr << "error in computing the covariance" << endl; } else { MatrixXd m = *spinv.block(ps->_offsetVertex->hessianIndex(), ps->_offsetVertex->hessianIndex()); cerr << "Param covariance" << endl; cerr << m << endl; cerr << "OffsetVertex: " << endl; ps->_offsetVertex->write(cerr); cerr << endl; cerr << "rotationDeterminant: " << m.block<3,3>(0,0).determinant() << endl; cerr << "translationDeterminant: " << m.block<3,3>(3,3).determinant() << endl; cerr << endl; } } ofstream os1("test_postOpt.g2o"); g->save(os1); }