graph_optimizer_sparse.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 
// g2o is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// g2o is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _GRAPH_OPTIMIZER_CHOL_H_
#define _GRAPH_OPTIMIZER_CHOL_H_

#include "optimizable_graph.h"

#include "solver.h"

#include <map>

// forward declaration
namespace g2o {
class ActivePathCostFunction;
}

namespace g2o {

  struct G2OBatchStatistics;

  struct SparseOptimizer : public OptimizableGraph {

    enum Method{GaussNewton, LevenbergMarquardt};

    friend class ActivePathCostFunction;

    // Attention: _solver & _statistics is own by SparseOptimizer and will be
    // deleted in its destructor.
    SparseOptimizer();
    virtual ~SparseOptimizer();

    // new interface for the optimizer
    // the old functions will be dropped
    virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);

    virtual bool initializeOptimization
       (HyperGraph::VertexSet& vset, int level=0);

    virtual bool initializeOptimization(int level=0);

    virtual bool updateInitialization
       (HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset);
  
    virtual void computeInitialGuess();


    int optimize(int iterations, bool online = false);

    bool computeMarginals();

    bool computeMarginals
    (
       SparseBlockMatrix<MatrixXd>& spinv, 
       const std::vector<std::pair<int, int> >& blockIndices
    );

    virtual Vertex* findGauge();

    bool gaugeFreedom();

    double activeChi2() const;

    bool verbose()  const {return _verbose;}
    void setVerbose(bool verbose);

    Method method() const { return _method;}
    void setMethod(Method method);

    void setForceStopFlag(bool* flag);

    bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }

    const VertexContainer& indexMapping() const {return _ivMap;}

    const VertexContainer& activeVertices() const { return _activeVertices;}

    const EdgeContainer& activeEdges() const { return _activeEdges;}

    virtual bool removeVertex(Vertex* v);

    VertexContainer::const_iterator findActiveVertex
       (OptimizableGraph::Vertex* v) const;

    EdgeContainer::const_iterator findActiveEdge
       (OptimizableGraph::Edge* e) const;

    const Solver* solver() const { return _solver;}
    Solver* solver() { return _solver;}
    void setSolver(Solver* solver);

    void push(SparseOptimizer::VertexContainer& vlist);

    void push(HyperGraph::VertexSet& vlist);

    void pop(SparseOptimizer::VertexContainer& vlist);

    void pop(HyperGraph::VertexSet& vlist);

    void discardTop(SparseOptimizer::VertexContainer& vlist);

    double userLambdaInit() {return _userLambdaInit;}

    void setUserLambdaInit(double lambda);

    virtual void clear();

    void computeActiveErrors();

    void linearizeSystem();

    void update(double* update);

    double currentLambda() const { return _currentLambda;}

    void setMaxTrialsAfterFailure(int max_trials);

    int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure;}

    protected:
    bool* _forceStopFlag;
    bool _verbose;

    Method _method;
    double _currentLambda;
    int _maxTrialsAfterFailure;

    VertexContainer _ivMap;
    VertexContainer _activeVertices;   
    EdgeContainer _activeEdges;        

    void sortVectorContainers();
 
    Solver* _solver;

    // LM parameters
    double _tau;
    double _userLambdaInit;
    double _goodStepLowerScale;
    double _goodStepUpperScale;

    double computeLambdaInit();
    double computeScale(double currentLMLambda, Solver* solver);

    bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist);
    void clearIndexMapping();

  public:
    G2OBatchStatistics* _statistics;

  };

} // end namespace

#endif