DCSAM.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
#include <map>
#include <set>
#include <tuple>
#include <utility>
#include <vector>
 
namespace gtsam {

class GTSAM_EXPORT DCSAM {
 private:
  NonlinearFactorGraph nfg_;
  DiscreteFactorGraph dfg_;
  HybridNonlinearFactorGraph hfg_;
 
  // TODO(Varun): Allow for using other continuous and discrete solvers

  ISAM2 isam_;

  Values currContinuous_;
  DiscreteValues currDiscrete_;
 
 public:
  DCSAM();
 
  explicit DCSAM(const ISAM2Params &isam_params);

  void update(const HybridNonlinearFactorGraph &graph,
              const HybridValues &initialGuess = HybridValues());

  inline void update(const HybridNonlinearFactorGraph &graph,
                     const DiscreteValues &initialGuessDiscrete) {
    update(graph, HybridValues(VectorValues(), initialGuessDiscrete));
  }

  void update();

  HybridValues calculateEstimate() const;

  const VectorValues& getDelta() const;

  double error(const VectorValues& x) const;

  // DCMarginals getMarginals(const NonlinearFactorGraph &graph,
  //                          const Values &continuousEst,
  //                          const DiscreteFactorGraph &dfg);
 
  const DiscreteFactorGraph &getDiscreteFactorGraph() const { return dfg_; }
 
  const NonlinearFactorGraph &getNonlinearFactorGraph() const {
    return isam_.getFactorsUnsafe();
  }
 
 protected:
  void updateDiscrete(const DiscreteFactorGraph &dfg = DiscreteFactorGraph(),
                      const DiscreteValues &discreteVals = DiscreteValues());

  void updateContinuous(const NonlinearFactorGraph &newFactors,
                        const Values &initialGuess);

  DiscreteValues solveDiscrete() const;
};
 
}  // namespace gtsam