math.cpp
Go to the documentation of this file.
1 #include "math.h"
2 #include <Eigen/Eigenvalues>
3 
4 
6 {
7 
8 MultivariateNormal::MultivariateNormal(const Eigen::VectorXf &mu, float std)
9 {
10  mean = mu;
11  covariance = Eigen::MatrixXf::Identity(mean.size(), mean.size()) * std::pow(std, 2);
12 }
13 
14 float MultivariateNormal::pdf(const Eigen::VectorXf& x) const
15 {
16  float n = x.rows();
17  float sqrt2pi = std::sqrt(2 * M_PI);
18  float quadform = (x - mean).transpose() * covariance.inverse() * (x - mean);
19  float norm = std::pow(sqrt2pi, - n) *
20  std::pow(covariance.determinant(), - 0.5);
21 
22  return norm * exp(-0.5 * quadform);
23 }
24 
25 Eigen::VectorXf MultivariateNormal::pdf_gradient(const Eigen::VectorXf &x) const
26 {
27  float probability = pdf(x);
28  return - probability * (x - mean) / covariance.determinant();
29 }
30 
31 Eigen::VectorXf MultivariateNormal::sample(unsigned int nr_iterations) const
32 {
33  int n = mean.rows();
34 
35  // Generate x from the N(0, I) distribution
36  Eigen::VectorXf x(n);
37  Eigen::VectorXf sum(n);
38  sum.setZero();
39  for (unsigned int i = 0; i < nr_iterations; i++)
40  {
41  x.setRandom();
42  x = 0.5 * (x + Eigen::VectorXf::Ones(n));
43  sum = sum + x;
44  }
45  sum = sum - (static_cast<double>(nr_iterations) / 2) * Eigen::VectorXf::Ones(n);
46  x = sum / (std::sqrt(static_cast<double>(nr_iterations) / 12));
47 
48  // Find the eigen vectors of the covariance matrix
49  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigen_solver(covariance);
50  Eigen::MatrixXf eigenvectors = eigen_solver.eigenvectors().real();
51 
52  // Find the eigenvalues of the covariance matrix
53  Eigen::MatrixXf eigenvalues = eigen_solver.eigenvalues().real().asDiagonal();
54 
55  // Find the transformation matrix
56  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(eigenvalues);
57  Eigen::MatrixXf sqrt_eigenvalues = es.operatorSqrt();
58  Eigen::MatrixXf Q = eigenvectors * sqrt_eigenvalues;
59 
60  return Q * x + mean;
61 }
62 
63 
64 Eigen::VectorXf pdf_gradient(const Eigen::VectorXf &mean, float std, const Eigen::VectorXf &x)
65 {
66  Eigen::MatrixXf covariance = Eigen::MatrixXf::Identity(mean.size(), mean.size()) * std::pow(std, 2);
67  float n = x.rows();
68  float sqrt2pi = std::sqrt(2 * M_PI);
69  float quadform = (x - mean).transpose() * covariance.inverse() * (x - mean);
70  float norm = std::pow(sqrt2pi, - n) *
71  std::pow(covariance.determinant(), - 0.5);
72 
73  float probability = norm * exp(-0.5 * quadform);
74  return - probability * (x - mean) / covariance.determinant();
75 }
76 
77 }
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
math.h
armarx::control::common
This file is part of ArmarX.
Definition: aron_conversions.cpp:29
armarx::control::common::MultivariateNormal::pdf_gradient
Eigen::VectorXf pdf_gradient(const Eigen::VectorXf &x) const
Definition: math.cpp:25
armarx::control::common::MultivariateNormal::sample
Eigen::VectorXf sample(unsigned int nr_iterations=20) const
Definition: math.cpp:31
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
armarx::control::common::pdf_gradient
Eigen::VectorXf pdf_gradient(const Eigen::VectorXf &mean, float std, const Eigen::VectorXf &x)
Definition: math.cpp:64
armarx::control::common::MultivariateNormal::covariance
Eigen::MatrixXf covariance
Definition: math.h:39
std
Definition: Application.h:66
armarx::control::common::MultivariateNormal::pdf
float pdf(const Eigen::VectorXf &x) const
Definition: math.cpp:14
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::control::common::MultivariateNormal::MultivariateNormal
MultivariateNormal(const Eigen::VectorXf &mu, const Eigen::MatrixXf &cov)
armarx::control::common::MultivariateNormal::mean
Eigen::VectorXf mean
Definition: math.h:38
norm
double norm(const Point &a)
Definition: point.hpp:94