Merge branch 'develop' into hybrid/simplify

gtsam/hybrid/HybridFactorGraph.cpp

@@ -45,9 +45,10 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
 /* ************************************************************************* */
 KeySet HybridFactorGraph::discreteKeySet() const {
   KeySet keys;
-  for (const DiscreteKey& k : discreteKeys()) {
-    keys.insert(k.first);
-  }
+  std::set<DiscreteKey> key_set = discreteKeys();
+  std::transform(key_set.begin(), key_set.end(),
+                 std::inserter(keys, keys.begin()),
+                 [](const DiscreteKey& k) { return k.first; });
   return keys;
 }
 

gtsam/hybrid/HybridNonlinearFactorGraph.h

@@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
    * @param continuousValues: Dictionary of continuous values.
    * @return HybridGaussianFactorGraph::shared_ptr
    */
-  HybridGaussianFactorGraph::shared_ptr linearize(
+  boost::shared_ptr<HybridGaussianFactorGraph> linearize(
       const Values& continuousValues) const;
   /// @}
 };

gtsam/hybrid/MixtureFactor.h

@@ -21,6 +21,7 @@
 
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Symbol.h>
@@ -160,10 +161,14 @@ class MixtureFactor : public HybridFactor {
                              factor, continuousValues);
   }
 
-  /// Error for HybridValues is not provided for nonlinear hybrid factor.
+  /**
+   * @brief Compute error of factor given hybrid values.
+   *
+   * @param values The continuous Values and the discrete assignment.
+   * @return double The error of this factor.
+   */
   double error(const HybridValues& values) const override {
-    throw std::runtime_error(
-        "MixtureFactor::error(HybridValues) not implemented.");
+    return error(values.nonlinear(), values.discrete());
   }
 
   /**

gtsam/linear/GaussianConditional.cpp

@@ -124,7 +124,7 @@ namespace gtsam {
     cout << formatMatrixIndented("  d = ", getb(), true) << "\n";
     if (nrParents() == 0) {
       const auto mean = solve({});  // solve for mean.
-      mean.print("  mean");
+      mean.print("  mean", formatter);
     }
     if (model_)
       model_->print("  Noise model: ");