move ComputeLogNormalizer to NoiseModel.h
parent
a9013aad8e
commit
e2f1ad78a0
|
@ -163,20 +163,4 @@ double HybridGaussianFactor::error(const HybridValues &values) const {
|
||||||
return gf->error(values.continuous());
|
return gf->error(values.continuous());
|
||||||
}
|
}
|
||||||
|
|
||||||
/* *******************************************************************************/
|
|
||||||
double ComputeLogNormalizer(
|
|
||||||
const noiseModel::Gaussian::shared_ptr &noise_model) {
|
|
||||||
// Since noise models are Gaussian, we can get the logDeterminant using
|
|
||||||
// the same trick as in GaussianConditional
|
|
||||||
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 n * log2pi + logDeterminantSigma;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
@ -175,16 +175,4 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
||||||
template <>
|
template <>
|
||||||
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
|
struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Helper function to compute the sqrt(|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 normalizer we wish to compute.
|
|
||||||
* @return double
|
|
||||||
*/
|
|
||||||
GTSAM_EXPORT double ComputeLogNormalizer(
|
|
||||||
const noiseModel::Gaussian::shared_ptr &noise_model);
|
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
@ -706,6 +706,22 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
|
||||||
return shared_ptr(new Robust(robust,noise));
|
return shared_ptr(new Robust(robust,noise));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* *******************************************************************************/
|
||||||
|
double ComputeLogNormalizer(
|
||||||
|
const noiseModel::Gaussian::shared_ptr& noise_model) {
|
||||||
|
// Since noise models are Gaussian, we can get the logDeterminant using
|
||||||
|
// the same trick as in GaussianConditional
|
||||||
|
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 n * log2pi + logDeterminantSigma;
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -751,6 +751,18 @@ namespace gtsam {
|
||||||
template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
|
template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
|
||||||
template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
|
template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Helper function to compute the sqrt(|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 normalizer we wish to compute.
|
||||||
|
* @return double
|
||||||
|
*/
|
||||||
|
GTSAM_EXPORT double ComputeLogNormalizer(
|
||||||
|
const noiseModel::Gaussian::shared_ptr& noise_model);
|
||||||
|
|
||||||
} //\ namespace gtsam
|
} //\ namespace gtsam
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue