diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 7a233bb1b..9358c473d 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -19,6 +19,7 @@ namespace gtsam { +/* ************************************************************************ */ KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { KeyVector allKeys; @@ -30,6 +31,7 @@ KeyVector CollectKeys(const KeyVector &continuousKeys, return allKeys; } +/* ************************************************************************ */ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { KeyVector allKeys; std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys)); @@ -37,6 +39,7 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { return allKeys; } +/* ************************************************************************ */ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) { DiscreteKeys allKeys; @@ -45,29 +48,32 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, return allKeys; } -HybridFactor::HybridFactor() = default; - +/* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true), nrContinuous(keys.size()) {} + : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} +/* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base(CollectKeys(continuousKeys, discreteKeys)), isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), - nrContinuous(continuousKeys.size()), + nrContinuous_(continuousKeys.size()), discreteKeys_(discreteKeys) {} +/* ************************************************************************ */ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true), discreteKeys_(discreteKeys) {} +/* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { return Base::equals(lf, tol); } +/* ************************************************************************ */ void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; @@ -77,6 +83,4 @@ void HybridFactor::print(const std::string &s, this->printKeys("", formatter); } -HybridFactor::~HybridFactor() = default; - } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index c6e4a5ffa..da103da43 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -41,6 +41,16 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, * - GaussianMixture */ class GTSAM_EXPORT HybridFactor : public Factor { + private: + bool isDiscrete_ = false; + bool isContinuous_ = false; + bool isHybrid_ = false; + + size_t nrContinuous_ = 0; + + protected: + DiscreteKeys discreteKeys_; + public: // typedefs needed to play nice with gtsam typedef HybridFactor This; ///< This class @@ -48,27 +58,11 @@ class GTSAM_EXPORT HybridFactor : public Factor { shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class - bool isDiscrete_ = false; - bool isContinuous_ = false; - bool isHybrid_ = false; - - size_t nrContinuous = 0; - - DiscreteKeys discreteKeys_; - - public: /// @name Standard Constructors /// @{ /** Default constructor creates empty factor */ - HybridFactor(); - - /** Construct from container of keys. This constructor is used internally - * from derived factor - * constructors, either from a container of keys or from a - * boost::assign::list_of. */ - // template - // HybridFactor(const CONTAINER &keys) : Base(keys) {} + HybridFactor() = default; explicit HybridFactor(const KeyVector &keys); @@ -78,7 +72,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { explicit HybridFactor(const DiscreteKeys &discreteKeys); /// Virtual destructor - virtual ~HybridFactor(); + virtual ~HybridFactor() = default; /// @} /// @name Testable @@ -96,6 +90,21 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// @name Standard Interface /// @{ + /// True if this is a factor of discrete variables only. + bool isDiscrete() const { return isDiscrete_; } + + /// True if this is a factor of continuous variables only. + bool isContinuous() const { return isContinuous_; } + + /// True is this is a Discrete-Continuous factor. + bool isHybrid() const { return isHybrid_; } + + /// Return the number of continuous variables in this factor. + size_t nrContinuous() const { return nrContinuous_; } + + /// Return vector of discrete keys. + DiscreteKeys discreteKeys() const { return discreteKeys_; } + /// @} }; // HybridFactor diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 5fa4b555a..721cb4cc7 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -23,12 +23,12 @@ namespace gtsam { HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) { - inner = other; + inner_ = other; } HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), - inner(boost::make_shared(std::move(jf))) {} + inner_(boost::make_shared(std::move(jf))) {} bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { return false; @@ -36,7 +36,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner->print("inner: ", formatter); + inner_->print("inner: ", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 4a7939cd4..1749c8e41 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -28,13 +28,14 @@ namespace gtsam { * a diamond inheritance. */ class HybridGaussianFactor : public HybridFactor { + private: + GaussianFactor::shared_ptr inner_; + public: using Base = HybridFactor; using This = HybridGaussianFactor; using shared_ptr = boost::shared_ptr; - GaussianFactor::shared_ptr inner; - // Explicit conversion from a shared ptr of GF explicit HybridGaussianFactor(GaussianFactor::shared_ptr other); @@ -47,5 +48,7 @@ class HybridGaussianFactor : public HybridFactor { void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + GaussianFactor::shared_ptr inner() const { return inner_; } }; } // namespace gtsam