DiscreteFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/base/Vector.h>
23 
24 #include <cmath>
25 #include <sstream>
26 
27 using namespace std;
28 
29 namespace gtsam {
30 
31 /* ************************************************************************* */
33  return -std::log((*this)(values));
34 }
35 
36 /* ************************************************************************* */
37 double DiscreteFactor::error(const HybridValues& c) const {
38  return this->error(c.discrete());
39 }
40 
41 /* ************************************************************************* */
42 std::vector<double> expNormalize(const std::vector<double>& logProbs) {
43  double maxLogProb = -std::numeric_limits<double>::infinity();
44  for (size_t i = 0; i < logProbs.size(); i++) {
45  double logProb = logProbs[i];
46  if ((logProb != std::numeric_limits<double>::infinity()) &&
47  logProb > maxLogProb) {
48  maxLogProb = logProb;
49  }
50  }
51 
52  // After computing the max = "Z" of the log probabilities L_i, we compute
53  // the log of the normalizing constant, log S, where S = sum_j exp(L_j - Z).
54  double total = 0.0;
55  for (size_t i = 0; i < logProbs.size(); i++) {
56  double probPrime = exp(logProbs[i] - maxLogProb);
57  total += probPrime;
58  }
59  double logTotal = log(total);
60 
61  // Now we compute the (normalized) probability (for each i):
62  // p_i = exp(L_i - Z - log S)
63  double checkNormalization = 0.0;
64  std::vector<double> probs;
65  for (size_t i = 0; i < logProbs.size(); i++) {
66  double prob = exp(logProbs[i] - maxLogProb - logTotal);
67  probs.push_back(prob);
68  checkNormalization += prob;
69  }
70 
71  // Numerical tolerance for floating point comparisons
72  double tol = 1e-9;
73 
74  if (!gtsam::fpEqual(checkNormalization, 1.0, tol)) {
75  std::string errMsg =
76  std::string("expNormalize failed to normalize probabilities. ") +
77  std::string("Expected normalization constant = 1.0. Got value: ") +
78  std::to_string(checkNormalization) +
79  std::string(
80  "\n This could have resulted from numerical overflow/underflow.");
81  throw std::logic_error(errMsg);
82  }
83  return probs;
84 }
85 
86 } // namespace gtsam
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
leaf::MyValues values
Definition: BFloat16.h:88
EIGEN_DEVICE_FUNC const LogReturnType log() const
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Definition: Vector.cpp:41
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
std::vector< double > expNormalize(const std::vector< double > &logProbs)
Normalize a set of log probabilities.
const DiscreteValues & discrete() const
Return the discrete values.
Definition: HybridValues.h:92
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:10