Hybrid factor docs and minor refactor

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

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<typename CONTAINER>
-  //  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

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<JacobianFactor>(std::move(jf))) {}
+      inner_(boost::make_shared<JacobianFactor>(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

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<This>;
 
-  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