diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp deleted file mode 100644 index 4fe18bea7..000000000 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HybridGaussianFactor.cpp - * @date Mar 11, 2022 - * @author Fan Jiang - */ - -#include -#include -#include -#include - -#include - -namespace gtsam { - -/* ************************************************************************* */ -HybridGaussianFactor::HybridGaussianFactor( - const boost::shared_ptr &ptr) - : Base(ptr->keys()), inner_(ptr) {} - -HybridGaussianFactor::HybridGaussianFactor( - boost::shared_ptr &&ptr) - : Base(ptr->keys()), inner_(std::move(ptr)) {} - -HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) - : Base(jf.keys()), - inner_(boost::make_shared(std::move(jf))) {} - -HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf) - : Base(hf.keys()), - inner_(boost::make_shared(std::move(hf))) {} - -/* ************************************************************************* */ -bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { - const This *e = dynamic_cast(&other); - if (e == nullptr) return false; - if (!Base::equals(*e, tol)) return false; - return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false) - : !(e->inner_); -} - -/* ************************************************************************* */ -void HybridGaussianFactor::print(const std::string &s, - const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); - if (inner_) { - inner_->print("\n", formatter); - } else { - std::cout << "\nGaussian: nullptr" << std::endl; - } -}; - -/* ************************************************************************ */ -double HybridGaussianFactor::error(const HybridValues &values) const { - return inner_->error(values.continuous()); -} -/* ************************************************************************ */ - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h deleted file mode 100644 index 6bb022396..000000000 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ /dev/null @@ -1,123 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HybridGaussianFactor.h - * @date Mar 11, 2022 - * @author Fan Jiang - */ - -#pragma once - -#include -#include - -namespace gtsam { - -// Forward declarations -class JacobianFactor; -class HessianFactor; -class HybridValues; - -/** - * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have - * a diamond inheritance i.e. an extra factor type that inherits from both - * HybridFactor and GaussianFactor. - * - * @ingroup hybrid - */ -class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { - private: - GaussianFactor::shared_ptr inner_; - - public: - using Base = HybridFactor; - using This = HybridGaussianFactor; - using shared_ptr = boost::shared_ptr; - - /// @name Constructors - /// @{ - - /// Default constructor - for serialization. - HybridGaussianFactor() = default; - - /** - * Constructor from shared_ptr of GaussianFactor. - * Example: - * auto ptr = boost::make_shared(...); - * HybridGaussianFactor factor(ptr); - */ - explicit HybridGaussianFactor(const boost::shared_ptr &ptr); - - /** - * Forwarding constructor from shared_ptr of GaussianFactor. - * Examples: - * HybridGaussianFactor factor = boost::make_shared(...); - * HybridGaussianFactor factor(boost::make_shared(...)); - */ - explicit HybridGaussianFactor(boost::shared_ptr &&ptr); - - /** - * Forwarding constructor from rvalue reference of JacobianFactor. - * - * Examples: - * HybridGaussianFactor factor = JacobianFactor(...); - * HybridGaussianFactor factor(JacobianFactor(...)); - */ - explicit HybridGaussianFactor(JacobianFactor &&jf); - - /** - * Forwarding constructor from rvalue reference of JacobianFactor. - * - * Examples: - * HybridGaussianFactor factor = HessianFactor(...); - * HybridGaussianFactor factor(HessianFactor(...)); - */ - explicit HybridGaussianFactor(HessianFactor &&hf); - - /// @} - /// @name Testable - /// @{ - - /// Check equality. - virtual bool equals(const HybridFactor &lf, double tol) const override; - - /// GTSAM print utility. - void print( - const std::string &s = "HybridGaussianFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; - - /// @} - /// @name Standard Interface - /// @{ - - /// Return pointer to the internal Gaussian factor. - GaussianFactor::shared_ptr inner() const { return inner_; } - - /// Return the error of the underlying Gaussian factor. - double error(const HybridValues &values) const override; - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int /*version*/) { - ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar &BOOST_SERIALIZATION_NVP(inner_); - } -}; - -// traits -template <> -struct traits : public Testable {}; - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index dbf480733..4d8cf2c5b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include #include @@ -112,8 +111,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { // TODO(dellaert): in C++20, we can use std::visit. continue; } - } else if (auto gf = dynamic_pointer_cast(f)) { - result = addGaussian(result, gf->inner()); } else if (dynamic_pointer_cast(f)) { // Don't do anything for discrete-only factors // since we want to eliminate continuous values only. @@ -136,8 +133,8 @@ continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &f : factors) { - if (auto hgf = dynamic_pointer_cast(f)) { - gfg.push_back(hgf->inner()); + if (auto gf = dynamic_pointer_cast(f)) { + gfg.push_back(gf); } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. // TODO(dellaert): is this correct? If so explain here. @@ -151,8 +148,7 @@ continuousElimination(const HybridGaussianFactorGraph &factors, } auto result = EliminatePreferCholesky(gfg, frontalKeys); - return {boost::make_shared(result.first), - boost::make_shared(result.second)}; + return {boost::make_shared(result.first), result.second}; } /* ************************************************************************ */ @@ -428,28 +424,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } } -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { - FactorGraph::add(boost::make_shared(std::move(factor))); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add( - const boost::shared_ptr &factor) { - FactorGraph::add(boost::make_shared(factor)); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { - FactorGraph::add(std::move(factor)); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add( - const DecisionTreeFactor::shared_ptr &factor) { - FactorGraph::add(factor); -} - /* ************************************************************************ */ const Ordering HybridGaussianFactorGraph::getHybridOrdering() const { const KeySet discrete_keys = discreteKeySet(); @@ -472,19 +446,13 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. error_tree = error_tree + gaussianMixture->error(continuousValues); - - } else if (auto hybridGaussianFactor = - dynamic_pointer_cast(f)) { + } else if (auto gaussian = dynamic_pointer_cast(f)) { // If continuous only, get the (double) error // and add it to the error_tree - GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner(); - - // Compute the error of the gaussian factor. double error = gaussian->error(continuousValues); // Add the gaussian factor error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); - } else if (dynamic_pointer_cast(f)) { // If factor at `idx` is discrete-only, we skip. continue; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index c5fa27651..27620450a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -117,59 +116,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph HybridGaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} - /// @} - /// @name Adding factors. - /// @{ - - using Base::add; - using Base::push_back; - using Base::reserve; - - /// Add a Jacobian factor to the factor graph. - void add(JacobianFactor&& factor); - - /// Add a Jacobian factor as a shared ptr. - void add(const boost::shared_ptr& factor); - - /// Add a DecisionTreeFactor to the factor graph. - void add(DecisionTreeFactor&& factor); - - /// Add a DecisionTreeFactor as a shared ptr. - void add(const boost::shared_ptr& factor); - - /** - * Add a gaussian factor *pointer* to the internal gaussian factor graph - * @param gaussianFactor - boost::shared_ptr to the factor to add - */ - template - IsGaussian push_gaussian( - const boost::shared_ptr& gaussianFactor) { - Base::push_back(boost::make_shared(gaussianFactor)); - } - - /// Construct a factor and add (shared pointer to it) to factor graph. - template - IsGaussian emplace_gaussian(Args&&... args) { - auto factor = boost::allocate_shared( - Eigen::aligned_allocator(), std::forward(args)...); - push_gaussian(factor); - } - - /** - * @brief Add a single factor shared pointer to the hybrid factor graph. - * Dynamically handles the factor type and assigns it to the correct - * underlying container. - * - * @param sharedFactor The factor to add to this factor graph. - */ - void push_back(const SharedFactor& sharedFactor) { - if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { - push_gaussian(p); - } else { - Base::push_back(sharedFactor); - } - } - /// @} /// @name Testable /// @{ @@ -184,11 +130,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Standard Interface /// @{ - using Base::empty; - using Base::size; - using Base::operator[]; - using Base::resize; - /** * @brief Compute error for each discrete assignment, * and return as a tree. diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index bc67bd0d7..71b064eb6 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -65,8 +65,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( linearFG->push_back(gmf); } else if (auto nlf = dynamic_pointer_cast(f)) { const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues); - const auto hgf = boost::make_shared(gf); - linearFG->push_back(hgf); + linearFG->push_back(gf); } else if (dynamic_pointer_cast(f)) { // If discrete-only: doesn't need linearization. linearFG->push_back(f); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index c3d056f47..2c131f864 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include @@ -64,7 +63,7 @@ TEST(HybridGaussianFactorGraph, Creation) { HybridGaussianFactorGraph hfg; - hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1))); + hfg.emplace_shared(X(0), I_3x3, Z_3x1); // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // graph @@ -85,7 +84,7 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) { // Test elimination of a single variable. HybridGaussianFactorGraph hfg; - hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + hfg.emplace_shared(0, I_3x3, Z_3x1); auto result = hfg.eliminatePartialSequential(KeyVector{0}); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 1c31d180b..afa5c8710 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -496,8 +496,7 @@ TEST(HybridFactorGraph, Printing) { string expected_hybridFactorGraph = R"( size: 7 -factor 0: Continuous [x0] - +factor 0: A[x0] = [ 10 ] @@ -549,15 +548,13 @@ factor 2: Hybrid [x1 x2; m1]{ No noise model } -factor 3: Continuous [x1] - +factor 3: A[x1] = [ 10 ] b = [ -10 ] No noise model -factor 4: Continuous [x2] - +factor 4: A[x2] = [ 10 ] diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 93b9bca81..b8e2b2e6b 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -72,16 +71,6 @@ BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); -/* ****************************************************************************/ -// Test HybridGaussianFactor serialization. -TEST(HybridSerialization, HybridGaussianFactor) { - const HybridGaussianFactor factor(JacobianFactor(X(0), I_3x3, Z_3x1)); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ****************************************************************************/ // Test GaussianMixtureFactor serialization. TEST(HybridSerialization, GaussianMixtureFactor) {