Merge branch 'direct-hybrid-fg' into hybridnonlinearfactor

gtsam/hybrid/HybridGaussianFactor.h

@@ -45,7 +45,7 @@ using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
  * where the set of discrete variables indexes to
  * the continuous gaussian distribution.
  *
- * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e.,
+ * In factor graphs the error function typically returns 0.5*|A*x - b|^2, i.e.,
  * the negative log-likelihood for a Gaussian noise model.
  * In hybrid factor graphs we allow *adding* an arbitrary scalar dependent on
  * the discrete assignment.

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -770,9 +770,10 @@ static HybridGaussianFactorGraph CreateFactorGraph(
           ->linearize(values);
 
   // Create HybridGaussianFactor
+  // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|)
   std::vector<GaussianFactorValuePair> factors{
-      {f0, ComputeLogNormalizerConstant(model0)},
+      {f0, -2 * model0->logNormalizationConstant()},
-      {f1, ComputeLogNormalizerConstant(model1)}};
+      {f1, -2 * model1->logNormalizationConstant()}};
   HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
 
   HybridGaussianFactorGraph hfg;

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -868,9 +868,10 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
       std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
 
   // Create HybridNonlinearFactor
+  // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|)
   std::vector<NonlinearFactorValuePair> factors{
-      {f0, ComputeLogNormalizerConstant(model0)},
+      {f0, -2 * model0->logNormalizationConstant()},
-      {f1, ComputeLogNormalizerConstant(model1)}};
+      {f1, -2 * model1->logNormalizationConstant()}};
 
   HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
 

gtsam/linear/NoiseModel.cpp

@@ -238,6 +238,25 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
 
 Matrix Gaussian::information() const { return R().transpose() * R(); }
 
+/* *******************************************************************************/
+double Gaussian::logNormalizationConstant() const {
+  // Since noise models are Gaussian, we can get the logDeterminant easily
+  // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
+  // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
+  // Hence, log det(Sigma)) = -2.0 * logDeterminant()
+  // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
+  //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
+  //     = -0.5*n*log(2*pi) + logDeterminant()
+  double logDetR =
+      R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
+
+  size_t n = dim();
+  constexpr double log2pi = 1.8378770664093454835606594728112;
+  // Get 1/log(\sqrt(|2pi Sigma|)) = -0.5*log(|2pi Sigma|)
+  return -0.5 * n * log2pi + logDetR;
+}
+
+
 /* ************************************************************************* */
 // Diagonal
 /* ************************************************************************* */
@@ -708,24 +727,4 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
 
 /* ************************************************************************* */
 }  // namespace noiseModel
-
-/* *******************************************************************************/
-double ComputeLogNormalizerConstant(
-    const noiseModel::Gaussian::shared_ptr& noise_model) {
-  // Since noise models are Gaussian, we can get the logDeterminant using
-  // the same trick as in GaussianConditional
-  // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
-  // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
-  // Hence, log det(Sigma)) = -2.0 * logDetR()
-  double logDetR = noise_model->R()
-                       .diagonal()
-                       .unaryExpr([](double x) { return log(x); })
-                       .sum();
-  double logDeterminantSigma = -2.0 * logDetR;
-
-  size_t n = noise_model->dim();
-  constexpr double log2pi = 1.8378770664093454835606594728112;
-  return 0.5*(n * log2pi + logDeterminantSigma);
-}
-
 } // gtsam

gtsam/linear/NoiseModel.h

@@ -266,6 +266,19 @@ namespace gtsam {
       /// Compute covariance matrix
       virtual Matrix covariance() const;
 
+      /**
+       * @brief Helper method to compute the normalization constant
+       * for a Gaussian noise model.
+       * k = 1/log(\sqrt(|2πΣ|))
+       *
+       * We compute this in the log-space for numerical accuracy.
+       *
+       * @param noise_model The Gaussian noise model
+       * whose normalization constant we wish to compute.
+       * @return double
+       */
+      double logNormalizationConstant() const;
+
      private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
@@ -751,18 +764,6 @@ namespace gtsam {
   template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
   template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
 
-  /**
-   * @brief Helper function to compute the log(|2πΣ|) normalizer values
-   * for a Gaussian noise model.
-   * We compute this in the log-space for numerical accuracy.
-   *
-   * @param noise_model The Gaussian noise model
-   * whose normalization constant we wish to compute.
-   * @return double
-   */
-  GTSAM_EXPORT double ComputeLogNormalizerConstant(
-      const noiseModel::Gaussian::shared_ptr& noise_model);
-
 } //\ namespace gtsam
 
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -811,19 +811,18 @@ TEST(NoiseModel, ComputeLogNormalizerConstant) {
   // Very simple 1D noise model, which we can compute by hand.
   double sigma = 0.1;
   auto noise_model = Isotropic::Sigma(1, sigma);
-  double actual_value = ComputeLogNormalizerConstant(noise_model);
+  double actual_value = noise_model->logNormalizationConstant();
-  // Compute log(|2πΣ|) by hand.
+  // Compute 1/log(sqrt(|2πΣ|)) by hand.
-  // = log(2π) + log(Σ) (since it is 1D)
+  // = -0.5*(log(2π) + log(Σ)) (since it is 1D)
-  constexpr double log2pi = 1.8378770664093454835606594728112;
+  double expected_value = -0.5 * log(2 * M_PI * sigma * sigma);
-  double expected_value = log2pi + log(sigma * sigma);
   EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9);
 
   // Similar situation in the 3D case
   size_t n = 3;
   auto noise_model2 = Isotropic::Sigma(n, sigma);
-  double actual_value2 = ComputeLogNormalizerConstant(noise_model2);
+  double actual_value2 = noise_model2->logNormalizationConstant();
   // We multiply by 3 due to the determinant
-  double expected_value2 = n * (log2pi + log(sigma * sigma));
+  double expected_value2 = -0.5 * n * log(2 * M_PI * sigma * sigma);
   EXPECT_DOUBLES_EQUAL(expected_value2, actual_value2, 1e-9);
 }