DCSAM.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteMarginals.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>
 
#include "dcsam/DCContinuousFactor.h"
#include "dcsam/DCFactor.h"
#include "dcsam/DCFactorGraph.h"
#include "dcsam/DCSAM_types.h"
#include "dcsam/HybridFactorGraph.h"
 
namespace dcsam {
 
class DCSAM {
 public:
  DCSAM();
 
  explicit DCSAM(const gtsam::ISAM2Params &isam_params);
 
  void update(const gtsam::NonlinearFactorGraph &graph,
              const gtsam::DiscreteFactorGraph &dfg, const DCFactorGraph &dcfg,
              const gtsam::Values &initialGuessContinuous = gtsam::Values(),
              const DiscreteValues &initialGuessDiscrete = DiscreteValues());
 
  void update(const HybridFactorGraph &hfg,
              const gtsam::Values &initialGuessContinuous = gtsam::Values(),
              const DiscreteValues &initialGuessDiscrete = DiscreteValues());
 
  inline void update(const HybridFactorGraph &hfg,
                     const DiscreteValues &initialGuessDiscrete) {
    update(hfg, gtsam::Values(), initialGuessDiscrete);
  }
 
  void update();
 
  void updateDiscrete(const gtsam::DiscreteFactorGraph &graph,
                      const gtsam::Values &continuousVals,
                      const DiscreteValues &discreteVals);
 
  void updateDiscreteInfo(const gtsam::Values &continuousVals,
                          const DiscreteValues &discreteVals);
 
  void updateContinuous();
 
  void updateContinuousInfo(const DiscreteValues &discreteVals,
                            const gtsam::NonlinearFactorGraph &newFactors,
                            const gtsam::Values &initialGuess);
 
  DiscreteValues solveDiscrete() const;
 
  DCValues calculateEstimate() const;
 
  DCMarginals getMarginals(const gtsam::NonlinearFactorGraph &graph,
                           const gtsam::Values &continuousEst,
                           const gtsam::DiscreteFactorGraph &dfg);
 
  gtsam::DiscreteFactorGraph getDiscreteFactorGraph() const { return dfg_; }
 
  gtsam::NonlinearFactorGraph getNonlinearFactorGraph() const {
    return isam_.getFactorsUnsafe();
  }
 
 private:
  // Global factor graph and iSAM2 instance
  gtsam::NonlinearFactorGraph fg_;  // NOTE: unused
  gtsam::ISAM2Params isam_params_;
  gtsam::ISAM2 isam_;
  gtsam::DiscreteFactorGraph dfg_;
  gtsam::Values currContinuous_;
  DiscreteValues currDiscrete_;
 
 
  std::vector<DCContinuousFactor::shared_ptr> dcContinuousFactors_;
  gtsam::FastVector<gtsam::DiscreteFactor::shared_ptr> dcDiscreteFactors_;
};
}  // namespace dcsam