Gaussian.cpp
Go to the documentation of this file.
1 // *****************************************************************
2 // Filename: Gaussian.h
3 // Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
4 // Institute for Computer Science and Engineering (CSE),
6 // Author: Kai Welke
7 // Date: 27.05.2012
8 // *****************************************************************
9
10 // *****************************************************************
11 // includes
12 // *****************************************************************
13 #include "Gaussian.h"
14 #include <cstdio>
15 #include <iostream>
16
17 // *****************************************************************
18 // construction
19 // *****************************************************************
21 {
22  this->dimension = -1;
23 }
24
25 Gaussian::Gaussian(int dimension)
26 {
27  this->dimension = dimension;
28
29  // initialize valid distribution
30  this->cov = covariance_type::Identity(dimension, dimension);
31  this->mean = value_type::Zero(dimension, 1);
32 }
33
34 Gaussian::Gaussian(const Gaussian& prototype)
35 {
36  *this = prototype;
37 }
38
40 {
41  this->cov = covariance;
42  this->mean = mean;
43
44  dimension = cov.rows();
45
46  if (mean.rows() != dimension)
47  {
49  }
50 }
51
52 // *****************************************************************
54 // *****************************************************************
55 double Gaussian::evaluate(const value_type& point)
56 {
57  if (dimension == -1)
58  {
60  }
61
62  if (point.rows() != dimension)
63  {
65  }
66
67  Eigen::Matrix<double, 1, 1> inner = -0.5 * (point - mean).transpose() * cov.inverse() * (point - mean);
68  double e = exp(inner(0, 0));
69
70  covariance_type i = cov;
71  return pow(2 * M_PI, -getDimensions() / 2.0f) * pow(i.determinant(), -0.5) * e;
72 }
73
74 double Gaussian::mahalanobis(const value_type& point)
75 {
76  if (dimension == -1)
77  {
79  }
80
81  if (point.rows() != dimension)
82  {
84  }
85
87  dist = (point - mean).transpose() * cov.inverse() * (point - mean);
88
89  return std::sqrt(dist(0, 0));
90 }
91
92 // generic draw sample for arbitrary covariance matrix
94 {
95  if (dimension == -1)
96  {
98  }
99
100  // go through all dimensions and calculate independen random processes on unit
101  // gaussian using Box-Muller method
102
103  float fRand1, fRand2;
104  value_type gaussianSample(getDimensions());
105
106  // calculate gaussian distributed with Cov=I and mean = 0 (ndimension independant box muller gaussians)
107  for (int d = 0 ; d < getDimensions() ; d++)
108  {
109  fRand1 = float(rand()) / RAND_MAX;
110  fRand2 = float(rand()) / RAND_MAX;
111
112  // calculate standard normal distribution sample (Box-Muller)
113  gaussianSample(d) = sqrt(-2 * log(fRand1)) * cos(2 * M_PI * fRand2);
114  }
115
116  // in order to retrieve samples distributed according to cov
117  // use cholesky decomposition:
118  // A * A^t = cov
119  // ==> random_vector=A * gaussianSample
120  // (can be proved by X=AY, D= cov(Y) (with D=I, produced by above code), C=cov(X) (which is desired cov), C= E(XX^T)=E(AY(AY)^t)=AE(YY^t)A^t=ADA^t)
121  // thus only matrx A has to be found
122  covariance_type A = cov.llt().matrixL();
123
124  value_type sample = A * gaussianSample + mean;
125
126  return sample;
127 }
128
129 // draw sample for cases where only elements in diagonal of cov (can be used with zero in diagonal)
131 {
132  if (dimension == -1)
133  {
135  }
136
137  // go through all dimensions and calculate independen random processes on unit
138  // gaussian using Box-Muller method
139
140  float fRand1, fRand2;
141  value_type gaussianSample(getDimensions());
142
143  // calculate gaussian distributed with Cov=I and mean = 0 (ndimension independant box muller gaussians)
144  for (int d = 0 ; d < getDimensions() ; d++)
145  {
146  fRand1 = float(rand()) / RAND_MAX;
147  fRand2 = float(rand()) / RAND_MAX;
148
149  // calculate standard normal distribution sample (Box-Muller)
150  gaussianSample(d) = sqrt(-2 * log(fRand1)) * cos(2 * M_PI * fRand2);
151  }
152
153  // generate matrix A
155
156  for (int i = 0 ; i < getDimensions() ; i++)
157  for (int u = 0 ; u < getDimensions() ; u++)
158  if (i == u)
159  {
160  A(i, u) = sqrt(cov(i, u));
161  }
162  else
163  {
164  A(i, u) = 0;
165  }
166
167  value_type sample = A * gaussianSample + mean;
168
169  return sample;
170 }
171
172 // *****************************************************************
173 // content manipulation
174 // *****************************************************************
176 {
177  if (dimension == -1)
178  {
180  }
181
182  if (samples.rows() != dimension)
183  {
185  }
186
187  int numberSamples = samples.cols();
188
189  // calculate mean
190  mean = value_type::Zero(dimension, 1);
191
192  for (int i = 0 ; i < numberSamples ; i++)
193  {
194  mean += samples.block(0, i, mean.rows(), 1);
195  }
196
197  mean = mean / numberSamples;
198
200
202  for (int s = 0 ; s < numberSamples ; s++)
203  {
205  }
206
207  // calculate covariance
209 }
210
211 void Gaussian::set(const Gaussian& prototype)
212 {
213  if ((dimension != prototype.getDimensions()))
214  {
216  }
217
218  this->mean = prototype.getMean();
219  this->cov = prototype.getCovariance();
220 }
221
223 {
224  // isSymmetric(cov);
225
226  if ((dimension != -1) && (mean.rows() != dimension))
227  {
229  }
230
231  if ((dimension != -1) && (cov.rows() != dimension))
232  {
234  }
235
236  if (dimension == -1)
237  {
238  dimension = cov.rows();
239  }
240
241  this->mean = mean;
242  this->cov = cov;
243 }
244
246 {
247  if ((dimension != -1) && (mean.rows() != dimension))
248  {
250  }
251
252  if (dimension == -1)
253  {
254  dimension = mean.rows();
255  this->cov = Eigen::MatrixXd::Identity(dimension, dimension);
256  }
257
258  this->mean = mean;
259 }
260
262 {
263  if ((dimension != -1) && (cov.rows() != dimension))
264  {
266  }
267
268  if ((dimension != -1) && (cov.cols() != dimension))
269  {
271  }
272
273  if ((dimension == -1) && (cov.rows() != cov.cols()))
274  {
276  }
277
278  if (dimension == -1)
279  {
280  dimension = cov.rows();
281  this->mean = Eigen::VectorXd::Zero(dimension);
282  }
283
284  this->cov = cov;
285 }
286
287 // *****************************************************************
288 // content manipulation
289 // *****************************************************************
290 void Gaussian::isSymmetric(const covariance_type& matrix)
291 {
292  if (matrix.rows() != matrix.cols())
293  {
295  }
296
297  for (int r = 0 ; r < matrix.rows() ; r++)
298  {
299  for (int c = 0 ; c < matrix.cols() ; c++)
300  {
301  if (matrix(r, c) != matrix(c, r))
302  {
304  }
305  }
306  }
307 }
308
Gaussian::getCovariance
const covariance_type & getCovariance() const
Definition: Gaussian.h:77
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
Gaussian::covariance_type
Eigen::MatrixXd covariance_type
Definition: Gaussian.h:53
Gaussian::mahalanobis
double mahalanobis(const value_type &point)
Definition: Gaussian.cpp:74
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
CovarianceNotSymmetricException
Definition: Gaussian.h:35
Gaussian::evaluate
double evaluate(const value_type &point)
Definition: Gaussian.cpp:55
Gaussian::getDimensions
int getDimensions() const
Definition: Gaussian.h:85
InvalidDimensionException
Definition: Gaussian.h:19
Gaussian::samples_type
Eigen::MatrixXd samples_type
Definition: Gaussian.h:54
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1615
M_PI
#define M_PI
Definition: MathTools.h:17
Gaussian::setMean
void setMean(const value_type &mean)
Definition: Gaussian.cpp:245
Gaussian::drawSample
value_type drawSample()
Definition: Gaussian.cpp:93
Gaussian::setCovariance
void setCovariance(const covariance_type &cov)
Definition: Gaussian.cpp:261
Gaussian::value_type
Eigen::VectorXd value_type
Definition: Gaussian.h:52
Gaussian.h
A
class A(deque< T, A >)) ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((class T
Enables hashing of std::list.
float
#define float
Definition: 16_Level.h:22
Gaussian::drawSampleDiagonalCovariance
value_type drawSampleDiagonalCovariance()
Definition: Gaussian.cpp:130
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
GaussianNotInitializedException
Definition: Gaussian.h:27
Gaussian::getMean
const value_type & getMean() const
Definition: Gaussian.h:81
Gaussian
Definition: Gaussian.h:46
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
Gaussian::generateFromSamples
void generateFromSamples(const samples_type &samples)
Definition: Gaussian.cpp:175
Gaussian::Gaussian
Gaussian()
Definition: Gaussian.cpp:20
Gaussian::set
void set(const Gaussian &prototype)
Definition: Gaussian.cpp:211