void MainWindow::setRobustKernel()
{
SparseOptimizer* optimizer = viewer->graph;
bool robustKernel = cbRobustKernel->isChecked();
double huberWidth = leKernelWidth->text().toDouble();
//odometry edges are those whose node ids differ by 1
bool onlyLoop = cbOnlyLoop->isChecked();
if (robustKernel) {
QString strRobustKernel = coRobustKernel->currentText();
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
if (! creator) {
cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
return;
}
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (onlyLoop) {
if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
} else {
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
}
} else {
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(0);
}
}
}
void MainWindow::setRobustKernel()
{
SparseOptimizer* optimizer = viewer->graph;
bool robustKernel = cbRobustKernel->isChecked();
double huberWidth = leKernelWidth->text().toDouble();
if (robustKernel) {
QString strRobustKernel = coRobustKernel->currentText();
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
if (! creator) {
cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
return;
}
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
} else {
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(0);
}
}
}
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)
{
OptimizableGraph::initMultiThreading();
int maxIterations;
bool verbose;
string inputFilename;
string gnudump;
string outputfilename;
string solverProperties;
string strSolver;
string loadLookup;
bool initialGuess;
bool initialGuessOdometry;
bool marginalize;
bool listTypes;
bool listSolvers;
bool listRobustKernels;
bool incremental;
bool guiOut;
int gaugeId;
string robustKernel;
bool computeMarginals;
bool printSolverProperties;
double huberWidth;
double gain;
int maxIterationsWithGain;
//double lambdaInit;
int updateGraphEachN = 10;
string statsFile;
string summaryFile;
bool nonSequential;
// command line parsing
std::vector<int> gaugeList;
CommandArgs arg;
arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain");
arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)");
arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry");
arg.param("inc", incremental, false, "run incremetally");
arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes");
arg.param("guiout", guiOut, false, "gui output while running incrementally");
arg.param("marginalize", marginalize, false, "on or off");
arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver");
arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2");
arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
arg.param("robustKernel", robustKernel, "", "use this robust error function");
arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
arg.param("gaugeId", gaugeId, -1, "force the gauge");
arg.param("o", outputfilename, "", "output final version of the graph");
arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}");
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
string dummy;
arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
#endif
arg.param("stats", statsFile, "", "specify a file for the statistics");
arg.param("listTypes", listTypes, false, "list the registered types");
arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
arg.param("listSolvers", listSolvers, false, "list the available solvers");
arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP");
arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n e.g: 1,2,3,4,5 ");
arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries");
arg.parseArgs(argc, argv);
if (verbose) {
cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl;
}
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
// registering all the types from the libraries
DlWrapper dlTypesWrapper;
loadStandardTypes(dlTypesWrapper, argc, argv);
// register all the solvers
DlWrapper dlSolverWrapper;
loadStandardSolver(dlSolverWrapper, argc, argv);
#else
if (verbose)
cout << "# linked version of g2o" << endl;
#endif
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
if (listSolvers) {
solverFactory->listSolvers(cout);
}
if (listTypes) {
Factory::instance()->printRegisteredTypes(cout, true);
}
if (listRobustKernels) {
std::vector<std::string> kernels;
RobustKernelFactory::instance()->fillKnownKernels(kernels);
cout << "Robust Kernels:" << endl;
for (size_t i = 0; i < kernels.size(); ++i) {
cout << kernels[i] << endl;
}
}
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
SparseOptimizerTerminateAction* terminateAction = 0;
if (maxIterations < 0) {
cerr << "# setup termination criterion based on the gain of the iteration" << endl;
maxIterations = maxIterationsWithGain;
terminateAction = new SparseOptimizerTerminateAction;
terminateAction->setGainThreshold(gain);
terminateAction->setMaxIterations(maxIterationsWithGain);
optimizer.addPostIterationAction(terminateAction);
}
// allocating the desired solver + testing whether the solver is okay
OptimizationAlgorithmProperty solverProperty;
optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty));
if (! optimizer.solver()) {
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
return 0;
}
if (solverProperties.size() > 0) {
bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties);
if (! updateStatus) {
cerr << "Failure while updating the solver properties from the given string" << endl;
}
}
if (solverProperties.size() > 0 || printSolverProperties) {
optimizer.solver()->printProperties(cerr);
}
// Loading the input data
if (loadLookup.size() > 0) {
optimizer.setRenamedTypesFromString(loadLookup);
}
if (inputFilename.size() == 0) {
cerr << "No input data specified" << endl;
return 0;
} else if (inputFilename == "-") {
cerr << "Read input from stdin" << endl;
if (!optimizer.load(cin)) {
cerr << "Error loading graph" << endl;
return 2;
}
} else {
cerr << "Read input from " << inputFilename << endl;
ifstream ifs(inputFilename.c_str());
if (!ifs) {
cerr << "Failed to open file" << endl;
return 1;
}
if (!optimizer.load(ifs)) {
cerr << "Error loading graph" << endl;
return 2;
}
}
cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
if (optimizer.vertices().size() == 0) {
cerr << "Graph contains no vertices" << endl;
return 1;
}
set<int> vertexDimensions = optimizer.dimensions();
if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
return 3;
}
assert (optimizer.solver());
//optimizer.setMethod(str2method(strMethod));
//optimizer.setUserLambdaInit(lambdaInit);
// check for vertices to fix to remove DoF
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge=0;
if (gaugeList.size()){
cerr << "Fixing gauges: ";
for (size_t i=0; i<gaugeList.size(); i++){
int id=gaugeList[i];
OptimizableGraph::Vertex* v=optimizer.vertex(id);
if (!v){
cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
return -1;
} else {
if (i==0)
gauge = v;
cerr << v->id() << " ";
v->setFixed(1);
}
}
cerr << endl;
gaugeFreedom = false;
} else {
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 or already fixed vertex" << endl;
}
// if schur, we wanna marginalize the landmarks...
if (marginalize || solverProperty.requiresMarginalize) {
int maxDim = *vertexDimensions.rbegin();
int minDim = *vertexDimensions.begin();
if (maxDim != minDim) {
cerr << "# Preparing Marginalization of the Landmarks ... ";
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
v->setMarginalized(true);
}
}
cerr << "done." << endl;
}
}
if (robustKernel.size() > 0) {
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel);
cerr << "# Preparing robust error function ... ";
if (creator) {
if (nonSequential) {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
} else {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
cerr << "done." << endl;
} else {
cerr << "Unknown Robust Kernel: " << robustKernel << endl;
}
}
// 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 (incremental) {
cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl;
cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl;
int incIterations = maxIterations;
if (! arg.parsedParam("i")) {
cerr << "# Setting default number of iterations" << endl;
incIterations = 1;
}
int updateDisplayEveryN = updateGraphEachN;
int maxDim = 0;
cerr << "# incremental settings" << endl;
cerr << "#\t solve every " << updateGraphEachN << endl;
cerr << "#\t iterations " << incIterations << endl;
SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
maxDim = max(maxDim, v->dimension());
}
vector<SparseOptimizer::Edge*> edges;
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
edges.push_back(e);
}
optimizer.edges().clear();
optimizer.vertices().clear();
optimizer.setVerbose(false);
// sort the edges in a way that inserting them makes sense
sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
double cumTime = 0.;
int vertexCount=0;
int lastOptimizedVertexCount = 0;
int lastVisUpdateVertexCount = 0;
bool freshlyOptimized=false;
bool firstRound = true;
HyperGraph::VertexSet verticesAdded;
HyperGraph::EdgeSet edgesAdded;
for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
SparseOptimizer::Edge* e = *it;
int doInit = 0;
SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
if (! v1) {
SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
bool v1Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v1Added);
if (! v1Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 1;
if (v->dimension() == maxDim)
vertexCount++;
}
if (! v2) {
SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
bool v2Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v2Added);
if (! v2Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 2;
if (v->dimension() == maxDim)
vertexCount++;
}
// adding the edge and initialization of the vertices
{
//cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl;
if (! optimizer.addEdge(e)) {
cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
} else {
edgesAdded.insert(e);
}
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
//cerr << "init: "
//<< to->id() << "(" << to->dimension() << ") -> "
//<< from->id() << "(" << from->dimension() << ") " << endl;
e->initialEstimate(toSet, from);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
//cerr << "init: "
//<< from->id() << "(" << from->dimension() << ") -> "
//<< to->id() << "(" << to->dimension() << ") " << endl;
e->initialEstimate(fromSet, to);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
freshlyOptimized=false;
{
//cerr << "Optimize" << endl;
if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
if (firstRound) {
if (!optimizer.initializeOptimization()){
cerr << "initialization failed" << endl;
return 0;
}
} else {
if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) {
cerr << "updating initialization failed" << endl;
return 0;
}
}
verticesAdded.clear();
edgesAdded.clear();
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
firstRound = false;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
lastOptimizedVertexCount = vertexCount;
freshlyOptimized = true;
if (guiOut) {
if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
dumpEdges(cout, optimizer);
lastVisUpdateVertexCount = vertexCount;
}
}
}
if (! verbose)
cerr << ".";
}
} // for all edges
if (! freshlyOptimized) {
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
}
} else {
// BATCH optimization
if (statsFile!=""){
// allocate buffer for statistics;
optimizer.setComputeBatchStatistics(true);
}
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
double loadChi = optimizer.chi2();
cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
if (initialGuess) {
optimizer.computeInitialGuess();
} else if (initialGuessOdometry) {
EstimatePropagatorCostOdometry costFunction(&optimizer);
optimizer.computeInitialGuess(costFunction);
}
double initChi = optimizer.chi2();
signal(SIGINT, sigquit_handler);
int result=optimizer.optimize(maxIterations);
if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){
cerr << "Cholesky failed, result might be invalid" << endl;
} else if (computeMarginals){
std::vector<std::pair<int, int> > blockIndices;
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
if (v->hessianIndex()>=0){
blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
}
if (v->hessianIndex()>0){
blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
}
}
SparseBlockMatrix<MatrixXd> spinv;
if (optimizer.computeMarginals(spinv, blockIndices)) {
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
cerr << "Vertex id:" << v->id() << endl;
if (v->hessianIndex()>=0){
cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
cerr << endl;
}
if (v->hessianIndex()>0){
cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
cerr << endl;
}
}
}
}
optimizer.computeActiveErrors();
double finalChi=optimizer.chi2();
if (summaryFile!="") {
PropertyMap summary;
summary.makeProperty<StringProperty>("filename", inputFilename);
summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size());
int nLandmarks=0;
int nPoses=0;
int maxDim = *vertexDimensions.rbegin();
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
nLandmarks++;
} else
nPoses++;
}
set<string> edgeTypes;
for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
edgeTypes.insert(Factory::instance()->tag(*it));
}
stringstream edgeTypesString;
for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
edgeTypesString << *it << " ";
}
summary.makeProperty<IntProperty>("n_poses", nPoses);
summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
summary.makeProperty<DoubleProperty>("load_chi", loadChi);
summary.makeProperty<StringProperty>("solver", strSolver);
summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0);
summary.makeProperty<DoubleProperty>("init_chi", initChi);
summary.makeProperty<DoubleProperty>("final_chi", finalChi);
summary.makeProperty<IntProperty>("maxIterations", maxIterations);
summary.makeProperty<IntProperty>("realIterations", result);
ofstream os;
os.open(summaryFile.c_str(), ios::app);
summary.writeToCSV(os);
}
if (statsFile!=""){
cerr << "writing stats to file \"" << statsFile << "\" ... ";
ofstream os(statsFile.c_str());
const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
for (int i=0; i<maxIterations; i++) {
os << bsc[i] << endl;
}
cerr << "done." << endl;
}
}
// saving again
if (gnudump.size() > 0) {
bool gnuPlotStatus = saveGnuplot(gnudump, optimizer);
if (! gnuPlotStatus) {
cerr << "Error while writing gnuplot files" << endl;
}
}
if (outputfilename.size() > 0) {
if (outputfilename == "-") {
cerr << "saving to stdout";
optimizer.save(cout);
} else {
cerr << "saving " << outputfilename << " ... ";
optimizer.save(outputfilename.c_str());
}
cerr << "done." << endl;
}
// destroy all the singletons
//Factory::destroy();
//OptimizationAlgorithmFactory::destroy();
//HyperGraphActionLibrary::destroy();
return 0;
}
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;
}