Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "main_window.h"
00020
00021 #include "g2o/core/solver_property.h"
00022 #include "g2o/core/solver_factory.h"
00023 #include "g2o/core/graph_optimizer_sparse.h"
00024 #include "g2o/core/estimate_propagator.h"
00025
00026 #include <QFileDialog>
00027 #include <QStandardItemModel>
00028 #include <QDoubleValidator>
00029
00030 #include <fstream>
00031 #include <iostream>
00032 using namespace std;
00033 using namespace g2o;
00034
00035 MainWindow::MainWindow(QWidget * parent, Qt::WindowFlags flags) :
00036 QMainWindow(parent, flags),
00037 _lastSolver(-1)
00038 {
00039 setupUi(this);
00040 leLambda->setValidator(new QDoubleValidator(-numeric_limits<double>::max(), numeric_limits<double>::max(), 7, this));
00041 leKernelWidth->setValidator(new QDoubleValidator(-numeric_limits<double>::max(), numeric_limits<double>::max(), 7, this));
00042 plainTextEdit->setMaximumBlockCount(1000);
00043 btnForceStop->hide();
00044 }
00045
00046 MainWindow::~MainWindow()
00047 {
00048 }
00049
00050 void MainWindow::on_actionLoad_triggered(bool)
00051 {
00052 QString filename = QFileDialog::getOpenFileName(this, "Load g2o file", "", "g2o files (*.g2o);;All Files (*)");
00053 if (! filename.isNull()) {
00054 loadFromFile(filename);
00055 }
00056 }
00057
00058 void MainWindow::on_actionSave_triggered(bool)
00059 {
00060 QString filename = QFileDialog::getSaveFileName(this, "Save g2o file", "", "g2o files (*.g2o)");
00061 if (! filename.isNull()) {
00062 ofstream fout(filename.toStdString().c_str());
00063 viewer->graph->save(fout);
00064 if (fout.good())
00065 cerr << "Saved " << filename.toStdString() << endl;
00066 else
00067 cerr << "Error while saving file" << endl;
00068 }
00069 }
00070
00071 void MainWindow::on_btnOptimize_clicked()
00072 {
00073 if (viewer->graph->vertices().size() == 0 || viewer->graph->edges().size() == 0) {
00074 cerr << "Graph has no vertices / egdes" << endl;
00075 return;
00076 }
00077
00078 bool allocatedNewSolver;
00079 bool allocateStatus = allocateSolver(allocatedNewSolver);
00080 if (! allocateStatus) {
00081 cerr << "Error while allocating solver" << endl;
00082 return;
00083 }
00084 if (allocatedNewSolver)
00085 prepare();
00086 setRobustKernel();
00087
00088 if (rbGauss->isChecked())
00089 viewer->graph->setMethod(g2o::SparseOptimizer::GaussNewton);
00090 else if (rbLevenberg->isChecked()) {
00091 double lambdaInit = leLambda->text().toDouble();
00092 if (lambdaInit > 0) {
00093 cerr << "Using initial damping of " << lambdaInit << endl;
00094 viewer->graph->setUserLambdaInit(lambdaInit);
00095 } else
00096 viewer->graph->setUserLambdaInit(0.);
00097 viewer->graph->setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
00098 } else
00099 viewer->graph->setMethod(g2o::SparseOptimizer::GaussNewton);
00100
00101 btnOptimize->hide();
00102 btnForceStop->show();
00103
00104 _forceStopFlag = false;
00105 viewer->graph->setForceStopFlag(&_forceStopFlag);
00106
00107 int maxIterations = spIterations->value();
00108 int iter = viewer->graph->optimize(maxIterations);
00109 if (maxIterations > 0 && !iter){
00110 cerr << "Optimization failed, result might be invalid" << endl;
00111 }
00112
00113 btnOptimize->show();
00114 btnForceStop->hide();
00115
00116 viewer->setUpdateDisplay(true);
00117 viewer->updateGL();
00118 _forceStopFlag = false;
00119 }
00120
00121 void MainWindow::on_btnInitialGuess_clicked()
00122 {
00123 if (viewer->graph->activeEdges().size() == 0)
00124 viewer->graph->initializeOptimization();
00125
00126 viewer->graph->computeInitialGuess();
00127 viewer->setUpdateDisplay(true);
00128 viewer->updateGL();
00129 }
00130
00131 void MainWindow::fixGraph()
00132 {
00133 if (viewer->graph->vertices().size() == 0 || viewer->graph->edges().size() == 0) {
00134 return;
00135 }
00136
00137
00138 bool gaugeFreedom = viewer->graph->gaugeFreedom();
00139 g2o::OptimizableGraph::Vertex* gauge = viewer->graph->findGauge();
00140 if (gaugeFreedom) {
00141 if (! gauge) {
00142 cerr << "cannot find a vertex to fix in this thing" << endl;
00143 return;
00144 } else {
00145 cerr << "graph is fixed by node " << gauge->id() << endl;
00146 gauge->setFixed(true);
00147 }
00148 } else {
00149 cerr << "graph is fixed by priors" << endl;
00150 }
00151
00152 viewer->graph->setVerbose(true);
00153
00154 }
00155
00156 void MainWindow::on_actionQuit_triggered(bool)
00157 {
00158 close();
00159 }
00160
00161 void MainWindow::updateDisplayedSolvers()
00162 {
00163 const SolverFactory::CreatorList& knownSolvers = SolverFactory::instance()->creatorList();
00164
00165 bool varFound = false;
00166 string varType = "";
00167 for (SolverFactory::CreatorList::const_iterator it = knownSolvers.begin(); it != knownSolvers.end(); ++it) {
00168 const SolverProperty& sp = (*it)->property();
00169 if (sp.name == "var" || sp.name == "var_cholmod") {
00170 varType = sp.type;
00171 varFound = true;
00172 break;
00173 }
00174 }
00175
00176 if (varFound) {
00177 for (SolverFactory::CreatorList::const_iterator it = knownSolvers.begin(); it != knownSolvers.end(); ++it) {
00178 const SolverProperty& sp = (*it)->property();
00179 if (sp.type == varType) {
00180 coOptimizer->addItem(QString::fromStdString(sp.name));
00181 _knownSolvers.push_back(sp);
00182 }
00183 }
00184 }
00185
00186 map<string, vector<SolverProperty> > solverLookUp;
00187
00188 for (SolverFactory::CreatorList::const_iterator it = knownSolvers.begin(); it != knownSolvers.end(); ++it) {
00189 const SolverProperty& sp = (*it)->property();
00190 if (varFound && varType == sp.type)
00191 continue;
00192 solverLookUp[sp.type].push_back(sp);
00193 }
00194
00195 for (map<string, vector<SolverProperty> >::iterator it = solverLookUp.begin(); it != solverLookUp.end(); ++it) {
00196 if (_knownSolvers.size() > 0) {
00197 coOptimizer->insertSeparator(coOptimizer->count());
00198 _knownSolvers.push_back(SolverProperty());
00199 }
00200 const vector<SolverProperty>& vsp = it->second;
00201 for (size_t j = 0; j < vsp.size(); ++j) {
00202 coOptimizer->addItem(QString::fromStdString(vsp[j].name));
00203 _knownSolvers.push_back(vsp[j]);
00204 }
00205 }
00206 }
00207
00208 bool MainWindow::load(const QString& filename)
00209 {
00210 ifstream ifs(filename.toStdString().c_str());
00211 if (! ifs)
00212 return false;
00213 viewer->graph->clear();
00214 bool loadStatus = viewer->graph->load(ifs);
00215 if (! loadStatus)
00216 return false;
00217 _lastSolver = -1;
00218 viewer->setUpdateDisplay(true);
00219 SparseOptimizer* optimizer = viewer->graph;
00220
00221
00222 set<int> vertDims = optimizer->dimensions();
00223 for (size_t i = 0; i < _knownSolvers.size(); ++i) {
00224 const SolverProperty& sp = _knownSolvers[i];
00225 if (sp.name == "" && sp.desc == "")
00226 continue;
00227
00228 bool suitableSolver = optimizer->isSolverSuitable(sp, vertDims);
00229 qobject_cast<QStandardItemModel *>(coOptimizer->model())->item(i)->setEnabled(suitableSolver);
00230 }
00231 return loadStatus;
00232 }
00233
00234 bool MainWindow::allocateSolver(bool& allocatedNewSolver)
00235 {
00236 if (coOptimizer->count() == 0) {
00237 cerr << "No solvers available" << endl;
00238 return false;
00239 }
00240 int currentIndex = coOptimizer->currentIndex();
00241 bool enabled = qobject_cast<QStandardItemModel *>(coOptimizer->model())->item(currentIndex)->isEnabled();
00242
00243 if (! enabled) {
00244 cerr << "selected solver is not enabled" << endl;
00245 return false;
00246 }
00247
00248 if (currentIndex == _lastSolver)
00249 return true;
00250
00251 allocatedNewSolver = true;
00252 QString strSolver = coOptimizer->currentText();
00253 delete viewer->graph->solver();
00254
00255 SolverFactory* solverFactory = SolverFactory::instance();
00256 viewer->graph->setSolver(solverFactory->construct(strSolver.toStdString(), viewer->graph, _currentSolverProperty));
00257
00258 _lastSolver = currentIndex;
00259 return true;
00260 }
00261
00262 bool MainWindow::prepare()
00263 {
00264 SparseOptimizer* optimizer = viewer->graph;
00265 if (_currentSolverProperty.requiresMarginalize) {
00266 cerr << "Marginalizing Landmarks" << endl;
00267 for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer->vertices().begin(); it != optimizer->vertices().end(); ++it) {
00268 OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
00269 int vdim = v->dimension();
00270 v->setMarginalized((vdim == _currentSolverProperty.landmarkDim));
00271 }
00272 }
00273 else {
00274 cerr << "Preparing (no marginalization of Landmarks)" << endl;
00275 for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer->vertices().begin(); it != optimizer->vertices().end(); ++it) {
00276 OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
00277 v->setMarginalized(false);
00278 }
00279 }
00280 viewer->graph->initializeOptimization();
00281 return true;
00282 }
00283
00284 void MainWindow::setRobustKernel()
00285 {
00286 SparseOptimizer* optimizer = viewer->graph;
00287 bool robustKernel = cbRobustKernel->isChecked();
00288 double huberWidth = leKernelWidth->text().toDouble();
00289
00290 for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
00291 OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
00292 e->setRobustKernel(robustKernel);
00293 e->setHuberWidth(huberWidth);
00294 }
00295 }
00296
00297 void MainWindow::on_btnForceStop_clicked()
00298 {
00299 _forceStopFlag = true;
00300 }
00301
00302 bool MainWindow::loadFromFile(const QString& filename)
00303 {
00304 viewer->graph->clear();
00305 bool loadStatus = load(filename);
00306 cerr << "loaded " << filename.toStdString() << " with " << viewer->graph->vertices().size()
00307 << " vertices and " << viewer->graph->edges().size() << " measurments" << endl;
00308 viewer->updateGL();
00309 fixGraph();
00310 return loadStatus;
00311 }
00312
00313 void MainWindow::on_actionWhite_Background_triggered(bool)
00314 {
00315 viewer->setBackgroundColor(QColor::fromRgb(255, 255, 255));
00316 viewer->updateGL();
00317 }
00318
00319 void MainWindow::on_actionDefault_Background_triggered(bool)
00320 {
00321 viewer->setBackgroundColor(QColor::fromRgb(51, 51, 51));
00322 viewer->updateGL();
00323 }