12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

hybrid folder

release/4.3a0
kartik arcot 2023-01-11 13:45:45 -08:00
parent d338a7086b
commit 0873ad628e
8 changed files with 23 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -27,6 +27,8 @@
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <optional>
namespace gtsam {
// Forward declarations

8
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -71,8 +71,8 @@ Ordering HybridGaussianISAM::GetOrdering(
void HybridGaussianISAM::updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
// Remove the contaminated part of the Bayes tree
BayesNetType bn;
@ -116,8 +116,8 @@ void HybridGaussianISAM::updateInternal(
/* ************************************************************************* */
void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
Cliques orphans;
this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function);

10
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -24,6 +24,8 @@
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/ISAM.h>
#include <optional>
namespace gtsam {
/**
@ -53,8 +55,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
void updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@ -68,8 +70,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
* @param function Elimination function.
*/
void update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);

6
gtsam/hybrid/HybridNonlinearISAM.cpp
View File

@ -34,8 +34,8 @@ void HybridNonlinearISAM::saveGraph(const string& s,
/* ************************************************************************* */
void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering) {
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering) {
if (newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
@ -70,7 +70,7 @@ void HybridNonlinearISAM::reorder_relinearize() {
// Just recreate the whole BayesTree
// TODO: allow for constrained ordering here
// TODO: decouple relinearization and reordering to avoid
isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none,
isam_.update(*factors_.linearize(newLinPoint), {}, {},
eliminationFunction_);
// Update linearization point

5
gtsam/hybrid/HybridNonlinearISAM.h
View File

@ -19,6 +19,7 @@
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <optional>
namespace gtsam {
/**
@ -119,8 +120,8 @@ class GTSAM_EXPORT HybridNonlinearISAM {
/** Add new factors along with their initial linearization points */
void update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none);
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {});
/** Relinearization and reordering of variables */
void reorder_relinearize();

2
gtsam/hybrid/HybridSmoother.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
/* ************************************************************************* */
void HybridSmoother::update(HybridGaussianFactorGraph graph,
const Ordering &ordering,
boost::optional<size_t> maxNrLeaves) {
std::optional<size_t> maxNrLeaves) {
// Add the necessary conditionals from the previous timestep(s).
std::tie(graph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_, ordering);

4
gtsam/hybrid/HybridSmoother.h
View File

@ -20,6 +20,8 @@
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <optional>
namespace gtsam {
class HybridSmoother {
@ -48,7 +50,7 @@ class HybridSmoother {
* if applicable
*/
void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
boost::optional<size_t> maxNrLeaves = boost::none);
std::optional<size_t> maxNrLeaves = {});
/**
* @brief Add conditionals from previous timestep as part of liquefication.

2
gtsam/hybrid/tests/TinyHybridExample.h
View File

@ -71,7 +71,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
*/
inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
size_t num_measurements = 1,
boost::optional<VectorValues> measurements = boost::none,
std::optional<VectorValues> measurements = {},
bool manyModes = false) {
auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
if (measurements) {