From 2927d92a52ceffaa4af1f743f3b58793ced382cd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 May 2022 15:43:37 -0400 Subject: [PATCH] add HybridNonlinearFactor and nonlinear HybridFactorGraph --- gtsam/hybrid/HybridFactorGraph.h | 108 +++++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactor.cpp | 45 +++++++++ gtsam/hybrid/HybridNonlinearFactor.h | 60 ++++++++++++ gtsam/hybrid/tests/testHybridFactorGraph.h | 50 ++++++++++ 4 files changed, 263 insertions(+) create mode 100644 gtsam/hybrid/HybridFactorGraph.h create mode 100644 gtsam/hybrid/HybridNonlinearFactor.cpp create mode 100644 gtsam/hybrid/HybridNonlinearFactor.h create mode 100644 gtsam/hybrid/tests/testHybridFactorGraph.h diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h new file mode 100644 index 000000000..f415386ea --- /dev/null +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridFactorGraph.h + * @brief Nonlinear hybrid factor graph that uses type erasure + * @author Varun Agrawal + * @date May 28, 2022 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Hybrid Factor Graph + * ----------------------- + * This is the non-linear version of a hybrid factor graph. + * Everything inside needs to be hybrid factor or hybrid conditional. + */ +class HybridFactorGraph : public FactorGraph { + public: + using Base = FactorGraph; + using This = HybridFactorGraph; ///< this class + using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + + using Values = gtsam::Values; ///< backwards compatibility + using Indices = KeyVector; ///> map from keys to values + + /// @name Constructors + /// @{ + + HybridFactorGraph() = default; + + /** + * Implicit copy/downcast constructor to override explicit template container + * constructor. In BayesTree this is used for: + * `cachedSeparatorMarginal_.reset(*separatorMarginal)` + * */ + template + HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + + /// @} + + // Allow use of selected FactorGraph methods: + using Base::empty; + using Base::reserve; + using Base::size; + using FactorGraph::add; + using Base::operator[]; + + /// Add a Jacobian factor to the factor graph. + void add(JacobianFactor&& factor); + + /// Add a Jacobian factor as a shared ptr. + void add(boost::shared_ptr factor); + + /// Add a DecisionTreeFactor to the factor graph. + void add(DecisionTreeFactor&& factor); + + /// Add a DecisionTreeFactor as a shared ptr. + void add(boost::shared_ptr factor); + + /** + * @brief Linearize all the continuous factors in the + * NonlinearHybridFactorGraph. + * + * @param continuousValues: Dictionary of continuous values. + * @return GaussianHybridFactorGraph::shared_ptr + */ + GaussianHybridFactorGraph::shared_ptr linearize( + const Values& continuousValues) const { + // create an empty linear FG + GaussianHybridFactorGraph::shared_ptr linearFG = + boost::make_shared(); + + linearFG->reserve(size()); + + // linearize all factors + for (const sharedFactor& factor : factors_) { + if (factor) { + if (auto nf = boost::dynamic_pointer_cast) { + (*linearFG) += factor->linearize(linearizationPoint); + } else if (auto hf = boost::dynamic_pointer_cast) { + if (hf->isContinuous()) { + } + } + + } else + (*linearFG) += GaussianFactor::shared_ptr(); + } + } +}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp new file mode 100644 index 000000000..ee6e76431 --- /dev/null +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridNonlinearFactor.cpp + * @date May 28, 2022 + * @author Varun Agrawal + */ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor::shared_ptr other) + : Base(other->keys()), inner_(other) {} + +/* ************************************************************************* */ +HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor &&nf) + : Base(nf.keys()), + inner_(boost::make_shared(std::move(nf))) {} + +/* ************************************************************************* */ +bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const { + return Base(lf, tol); +} + +/* ************************************************************************* */ +void HybridNonlinearFactor::print(const std::string &s, + const KeyFormatter &formatter) const { + HybridFactor::print(s, formatter); + inner_->print("inner: ", formatter); +}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h new file mode 100644 index 000000000..74feb0531 --- /dev/null +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridNonlinearFactor.h + * @date May 28, 2022 + * @author Varun Agrawal + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not + * have a diamond inheritance. + */ +class HybridNonlinearFactor : public HybridFactor { + private: + NonlinearFactor::shared_ptr inner_; + + public: + using Base = HybridFactor; + using This = HybridNonlinearFactor; + using shared_ptr = boost::shared_ptr; + + // Explicit conversion from a shared ptr of GF + explicit HybridNonlinearFactor(NonlinearFactor::shared_ptr other); + + // Forwarding constructor from concrete NonlinearFactor + explicit HybridNonlinearFactor(NonlinearFactor &&jf); + + public: + /// @name Testable + /// @{ + + /// Check equality. + virtual bool equals(const HybridFactor &lf, double tol) const override; + + /// GTSAM print utility. + void print( + const std::string &s = "HybridNonlinearFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + /// @} + + NonlinearFactor::shared_ptr inner() const { return inner_; } +}; +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.h b/gtsam/hybrid/tests/testHybridFactorGraph.h new file mode 100644 index 000000000..284e996ce --- /dev/null +++ b/gtsam/hybrid/tests/testHybridFactorGraph.h @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + * 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 + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridFactorGraph.cpp + * @brief Unit tests for HybridFactorGraph + * @author Varun Agrawal + * @date May 2021 + */ + +#include +#include +#include + +/* **************************************************************************** + * Test that any linearizedFactorGraph gaussian factors are appended to the + * existing gaussian factor graph in the hybrid factor graph. + */ +TEST(HybridFactorGraph, GaussianFactorGraph) { + // Initialize the hybrid factor graph + HybridFactorGraph fg; + + // Add a simple prior factor to the nonlinear factor graph + fg.emplace_shared>(X(0), 0, Isotropic::Sigma(1, 0.1)); + + // Add a linear factor to the nonlinear factor graph + fg.add(X(0), I_1x1, Vector1(5)); + + // Linearization point + Values linearizationPoint; + linearizationPoint.insert(X(0), 0); + + GaussianHybridFactorGraph ghfg = fg.linearize(linearizationPoint); + + // ghfg.push_back(ghfg.gaussianGraph().begin(), ghfg.gaussianGraph().end()); + + // EXPECT_LONGS_EQUAL(2, dcmfg.gaussianGraph().size()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */