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 "batch_stats.h"
#include "g2o/stuff/macros.h"
#include "g2o_core_api.h"
#include "optimizable_graph.h"
#include "sparse_block_matrix.h"
 
namespace g2o {
 
// forward declaration
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<MatrixX>& spinv,
                        const std::vector<std::pair<int, int> >& blockIndices);

  bool computeMarginals(SparseBlockMatrix<MatrixX>& spinv,
                        const Vertex* vertex);

  bool computeMarginals(SparseBlockMatrix<MatrixX>& spinv,
                        const VertexContainer& vertices);

  // 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;
};
}  // namespace g2o
 
#endif