ORBSLAM2017工程

// g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "optimizable_graph.h" #include <cassert> #include <iostream> #include <iomanip> #include <fstream> #include <algorithm> #include <Eigen/Dense> #include "estimate_propagator.h" #include "factory.h" #include "optimization_algorithm_property.h" #include "hyper_graph_action.h" #include "cache.h" #include "robust_kernel.h" #include "../stuff/macros.h" #include "../stuff/color_macros.h" #include "../stuff/string_tools.h" #include "../stuff/misc.h" namespace g2o { using namespace std; OptimizableGraph::Data::Data(){ _next = 0; } OptimizableGraph::Data::~Data(){ if (_next) delete _next; } OptimizableGraph::Vertex::Vertex() : HyperGraph::Vertex(), _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false), _colInHessian(-1), _cacheContainer(0) { } CacheContainer* OptimizableGraph::Vertex::cacheContainer(){ if (! _cacheContainer) _cacheContainer = new CacheContainer(this); return _cacheContainer; } void OptimizableGraph::Vertex::updateCache(){ if (_cacheContainer){ _cacheContainer->setUpdateNeeded(); _cacheContainer->update(); } } OptimizableGraph::Vertex::~Vertex() { if (_cacheContainer) delete (_cacheContainer); if (_userData) delete _userData; } OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const { return 0; } bool OptimizableGraph::Vertex::setEstimateData(const double* v) { bool ret = setEstimateDataImpl(v); updateCache(); return ret; } bool OptimizableGraph::Vertex::getEstimateData(double *) const { return false; } int OptimizableGraph::Vertex::estimateDimension() const { return -1; } bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v) { bool ret = setMinimalEstimateDataImpl(v); updateCache(); return ret; } bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const { return false; } int OptimizableGraph::Vertex::minimalEstimateDimension() const { return -1; } OptimizableGraph::Edge::Edge() : HyperGraph::Edge(), _dimension(-1), _level(0), _robustKernel(0) { } OptimizableGraph::Edge::~Edge() { delete _robustKernel; } OptimizableGraph* OptimizableGraph::Edge::graph(){ if (! _vertices.size()) return 0; OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0]; if (!v) return 0; return v->graph(); } const OptimizableGraph* OptimizableGraph::Edge::graph() const{ if (! _vertices.size()) return 0; const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0]; if (!v) return 0; return v->graph(); } bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){ if ((int)_parameters.size()<=argNum) return false; if (argNum<0) return false; *_parameters[argNum] = 0; _parameterIds[argNum] = paramId; return true; } bool OptimizableGraph::Edge::resolveParameters() { if (!graph()) { cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl; return false; } assert (_parameters.size() == _parameterIds.size()); //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl; for (size_t i=0; i<_parameters.size(); i++){ int index = _parameterIds[i]; *_parameters[i] = graph()->parameter(index); if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){ cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl; } if (!*_parameters[i]) { cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl; return false; } } return true; } void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr) { if (_robustKernel) delete _robustKernel; _robustKernel = ptr; } bool OptimizableGraph::Edge::resolveCaches() { return true; } bool OptimizableGraph::Edge::setMeasurementData(const double *) { return false; } bool OptimizableGraph::Edge::getMeasurementData(double *) const { return false; } int OptimizableGraph::Edge::measurementDimension() const { return -1; } bool OptimizableGraph::Edge::setMeasurementFromState(){ return false; } OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const { // TODO return 0; } OptimizableGraph::OptimizableGraph() { _nextEdgeId = 0; _edge_has_id = false; _graphActions.resize(AT_NUM_ELEMENTS); } OptimizableGraph::~OptimizableGraph() { clear(); clearParameters(); } bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData) { Vertex* inserted = vertex(v->id()); if (inserted) { cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl; assert(0 && "Vertex with this ID already contained in the graph"); return false; } OptimizableGraph::Vertex* ov=dynamic_cast<OptimizableGraph::Vertex*>(v); assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex"); if (ov->_graph != 0 && ov->_graph != this) { cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl; assert(0 && "Vertex already registered with another graph"); return false; } if (userData) ov->setUserData(userData); ov->_graph=this; return HyperGraph::addVertex(v); } bool OptimizableGraph::addEdge(HyperGraph::Edge* e_) { OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_); assert(e && "Edge does not inherit from OptimizableGraph::Edge"); if (! e) return false; bool eresult = HyperGraph::addEdge(e); if (! eresult) return false; e->_internalId = _nextEdgeId++; if (! e->resolveParameters()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl; return false; } if (! e->resolveCaches()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl; return false; } _jacobianWorkspace.updateSize(e); return true; } int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;} double OptimizableGraph::chi2() const { double chi = 0.0;