12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rename ComputeLogNormalizer to ComputeLogNormalizerConstant

release/4.3a0
Varun Agrawal 2024-09-19 16:09:54 -04:00
parent 9b6facd262
commit 244661afb1
6 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -30,8 +30,8 @@ namespace gtsam {
/** /**
* @brief Helper function to augment the [A|b] matrices in the factor components * @brief Helper function to augment the [A|b] matrices in the factor components
* with the normalizer values. * with the additional scalar values.
* This is done by storing the normalizer value in * This is done by storing the value in
* the `b` vector as an additional row. * the `b` vector as an additional row.
* *
* @param factors DecisionTree of GaussianFactors and arbitrary scalars. * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
@ -56,7 +56,7 @@ HybridGaussianFactor::Factors augment(
const HybridGaussianFactor::sharedFactor &gf) { const HybridGaussianFactor::sharedFactor &gf) {
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf); auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
if (!jf) return gf; if (!jf) return gf;
// If the log_normalizer is 0, do nothing // If the value is 0, do nothing
if (values(assignment) == 0.0) return gf; if (values(assignment) == 0.0) return gf;
GaussianFactorGraph gfg; GaussianFactorGraph gfg;

3
gtsam/hybrid/tests/testHybridGaussianFactor.cpp
View File

@ -771,7 +771,8 @@ static HybridGaussianFactorGraph CreateFactorGraph(
// Create HybridGaussianFactor // Create HybridGaussianFactor
std::vector<GaussianFactorValuePair> factors{ std::vector<GaussianFactorValuePair> factors{
{f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; {f0, ComputeLogNormalizerConstant(model0)},
{f1, ComputeLogNormalizerConstant(model1)}};
HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors); HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
HybridGaussianFactorGraph hfg; HybridGaussianFactorGraph hfg;

3
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -869,7 +869,8 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
// Create HybridNonlinearFactor // Create HybridNonlinearFactor
std::vector<NonlinearFactorValuePair> factors{ std::vector<NonlinearFactorValuePair> factors{
{f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; {f0, ComputeLogNormalizerConstant(model0)},
{f1, ComputeLogNormalizerConstant(model1)}};
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors); HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);

4
gtsam/linear/NoiseModel.cpp
View File

@ -710,7 +710,7 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
} // namespace noiseModel } // namespace noiseModel
/* *******************************************************************************/ /* *******************************************************************************/
double ComputeLogNormalizer( double ComputeLogNormalizerConstant(
const noiseModel::Gaussian::shared_ptr& noise_model) { const noiseModel::Gaussian::shared_ptr& noise_model) {
// Since noise models are Gaussian, we can get the logDeterminant using // Since noise models are Gaussian, we can get the logDeterminant using
// the same trick as in GaussianConditional // the same trick as in GaussianConditional
@ -725,7 +725,7 @@ double ComputeLogNormalizer(
size_t n = noise_model->dim(); size_t n = noise_model->dim();
constexpr double log2pi = 1.8378770664093454835606594728112; constexpr double log2pi = 1.8378770664093454835606594728112;
return n * log2pi + logDeterminantSigma; return 0.5*(n * log2pi + logDeterminantSigma);
} }
} // gtsam } // gtsam

4
gtsam/linear/NoiseModel.h
View File

@ -757,10 +757,10 @@ namespace gtsam {
* We compute this in the log-space for numerical accuracy. * We compute this in the log-space for numerical accuracy.
* *
* @param noise_model The Gaussian noise model * @param noise_model The Gaussian noise model
* whose normalizer we wish to compute. * whose normalization constant we wish to compute.
* @return double * @return double
*/ */
GTSAM_EXPORT double ComputeLogNormalizer( GTSAM_EXPORT double ComputeLogNormalizerConstant(
const noiseModel::Gaussian::shared_ptr& noise_model); const noiseModel::Gaussian::shared_ptr& noise_model);
} //\ namespace gtsam } //\ namespace gtsam

6
gtsam/linear/tests/testNoiseModel.cpp
View File

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