From 6c9f7ec7c7941d303a805fc2545dbc3ce9f810f3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2024 17:50:36 -0400 Subject: [PATCH 1/2] remove normlization stuff from HybridNonlinearFactor --- gtsam/hybrid/HybridNonlinearFactor.h | 59 +++------------------------- gtsam/hybrid/hybrid.i | 3 -- 2 files changed, 6 insertions(+), 56 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 742b4c1625..a7f607def0 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -63,7 +63,6 @@ class HybridNonlinearFactor : public HybridFactor { private: /// Decision tree of Gaussian factors indexed by discrete keys. Factors factors_; - bool normalized_; public: HybridNonlinearFactor() = default; @@ -74,12 +73,10 @@ class HybridNonlinearFactor : public HybridFactor { * @param keys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. * @param factors Decision tree with of shared factors. - * @param normalized Flag indicating if the factor error is already - * normalized. */ HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const Factors& factors, bool normalized = false) - : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {} + const Factors& factors) + : Base(keys, discreteKeys), factors_(factors) {} /** * @brief Convenience constructor that generates the underlying factor @@ -94,15 +91,12 @@ class HybridNonlinearFactor : public HybridFactor { * @param keys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. * @param factors Vector of nonlinear factor and scalar pairs. - * @param normalized Flag indicating if the factor error is already - * normalized. */ template HybridNonlinearFactor( const KeyVector& keys, const DiscreteKeys& discreteKeys, - const std::vector, double>>& factors, - bool normalized = false) - : Base(keys, discreteKeys), normalized_(normalized) { + const std::vector, double>>& factors) + : Base(keys, discreteKeys) { std::vector nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet factor_keys_set; @@ -224,11 +218,10 @@ class HybridNonlinearFactor : public HybridFactor { }; if (!factors_.equals(f.factors_, compare)) return false; - // If everything above passes, and the keys_, discreteKeys_ and normalized_ + // If everything above passes, and the keys_ and discreteKeys_ // member variables are identical, return true. return (std::equal(keys_.begin(), keys_.end(), f.keys().begin()) && - (discreteKeys_ == f.discreteKeys_) && - (normalized_ == f.normalized_)); + (discreteKeys_ == f.discreteKeys_)); } /// @} @@ -259,46 +252,6 @@ class HybridNonlinearFactor : public HybridFactor { return std::make_shared( continuousKeys_, discreteKeys_, linearized_factors); } - - /** - * If the component factors are not already normalized, we want to compute - * their normalizing constants so that the resulting joint distribution is - * appropriately computed. Remember, this is the _negative_ normalizing - * constant for the measurement likelihood (since we are minimizing the - * _negative_ log-likelihood). - */ - double nonlinearFactorLogNormalizingConstant(const sharedFactor& factor, - const Values& values) const { - // Information matrix (inverse covariance matrix) for the factor. - Matrix infoMat; - - // If this is a NoiseModelFactor, we'll use its noiseModel to - // otherwise noiseModelFactor will be nullptr - if (auto noiseModelFactor = - std::dynamic_pointer_cast(factor)) { - // If dynamic cast to NoiseModelFactor succeeded, see if the noise model - // is Gaussian - auto noiseModel = noiseModelFactor->noiseModel(); - - auto gaussianNoiseModel = - std::dynamic_pointer_cast(noiseModel); - if (gaussianNoiseModel) { - // If the noise model is Gaussian, retrieve the information matrix - infoMat = gaussianNoiseModel->information(); - } else { - // If the factor is not a Gaussian factor, we'll linearize it to get - // something with a normalized noise model - // TODO(kevin): does this make sense to do? I think maybe not in - // general? Should we just yell at the user? - auto gaussianFactor = factor->linearize(values); - infoMat = gaussianFactor->information(); - } - } - - // Compute the (negative) log of the normalizing constant - return -(factor->dim() * log(2.0 * M_PI) / 2.0) - - (log(infoMat.determinant()) / 2.0); - } }; } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index dcf0fce042..53bd634c49 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -255,9 +255,6 @@ class HybridNonlinearFactor : gtsam::HybridFactor { double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const; - double nonlinearFactorLogNormalizingConstant( - const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const; - HybridGaussianFactor* linearize(const gtsam::Values& continuousValues) const; void print(string s = "HybridNonlinearFactor\n", From 149601761b76cc8e690197357b3b444a9e2756f6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2024 17:59:18 -0400 Subject: [PATCH 2/2] fix wrapper --- gtsam/hybrid/hybrid.i | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 574b013a57..df4e6966da 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -248,13 +248,11 @@ class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, const gtsam::DecisionTree< - gtsam::Key, std::pair>& factors, - bool normalized = false); + gtsam::Key, std::pair>& factors); HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, - const std::vector>& factors, - bool normalized = false); + const std::vector>& factors); double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const;