sparse_optimizer.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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.

#ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_
#define G2O_GRAPH_OPTIMIZER_CHOL_H_

#include "g2o/stuff/macros.h"

#include "optimizable_graph.h"
#include "sparse_block_matrix.h"
#include "g2o_core_api.h"
#include "batch_stats.h"

#include <map>

namespace g2o {

  // forward declaration
  class ActivePathCostFunction;
  class OptimizationAlgorithm;
  class EstimatePropagatorCost;

  class G2O_CORE_API SparseOptimizer : public OptimizableGraph {

    public:
    enum {
      AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
      AT_NUM_ELEMENTS, // keep as last element
    };

    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();

    virtual void computeInitialGuess(EstimatePropagatorCost& propagator);

    virtual void setToOrigin();


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

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

    bool computeMarginals(SparseBlockMatrix<MatrixXD>& spinv, const Vertex* vertex) {
      if (vertex->hessianIndex() < 0) {
          return false;
      }
      std::vector<std::pair<int, int> > index;
      index.push_back(std::pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex()));
      return computeMarginals(spinv, index);
    }

    bool computeMarginals(SparseBlockMatrix<MatrixXD>& spinv, const VertexContainer& vertices) {
      std::vector<std::pair<int, int> > indices;
      for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
        indices.push_back(std::pair<int, int>((*it)->hessianIndex(),(*it)->hessianIndex()));
      }
      return computeMarginals(spinv, indices);
    }

    // The gauge should be fixed() and then the optimization can work (if no additional dof are in
    // the system. The default implementation returns a node with maximum dimension.
    virtual Vertex* findGauge();

    bool gaugeFreedom();

    double activeChi2() const;
    double activeRobustChi2() const;

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

    void setForceStopFlag(bool* flag);
    bool* forceStopFlag() const { return _forceStopFlag;};

    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(HyperGraph::Vertex* v, bool detach=false);

    VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;
    EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;

    const OptimizationAlgorithm* algorithm() const { return _algorithm;}
    OptimizationAlgorithm* solver() { return _algorithm;}
    void setAlgorithm(OptimizationAlgorithm* algorithm);

    void push(SparseOptimizer::VertexContainer& vlist);
    void push(HyperGraph::VertexSet& vlist);
    void push();
    void pop(SparseOptimizer::VertexContainer& vlist);
    void pop(HyperGraph::VertexSet& vlist);
    void pop();

    void discardTop(SparseOptimizer::VertexContainer& vlist);
    void discardTop();
    using OptimizableGraph::discardTop;

    virtual void clear();

    void computeActiveErrors();

    G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
    {
      // nothing needed, linearization is now done inside the solver
    }

    void update(const double* update);

    const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;}
    BatchStatisticsContainer& batchStatistics() { return _batchStatistics;}
    
    void setComputeBatchStatistics(bool computeBatchStatistics);
    
    bool computeBatchStatistics() const { return _computeBatchStatistics;}

    /**** callbacks ****/
    bool addComputeErrorAction(HyperGraphAction* action);
    bool removeComputeErrorAction(HyperGraphAction* action);

    

    protected:
    bool* _forceStopFlag;
    bool _verbose;

    VertexContainer _ivMap;
    VertexContainer _activeVertices;   
    EdgeContainer _activeEdges;        

    void sortVectorContainers();
 
    OptimizationAlgorithm* _algorithm;

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

    BatchStatisticsContainer _batchStatistics;   
    bool _computeBatchStatistics;
  };
} // end namespace

#endif