FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
math/distribution/NormalN.h
View File

@@ -3,6 +3,7 @@
#include <cmath>
#include <random>
#include <vector>
#include <eigen3/Eigen/Dense>
@@ -20,23 +21,28 @@ namespace Distribution {
Eigen::VectorXd mu;
Eigen::MatrixXd sigma;
const double _a;
const Eigen::MatrixXd _sigmaInv;
double _a;
Eigen::MatrixXd _sigmaInv;
const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
Eigen::MatrixXd transform; //can i make this const?
Eigen::MatrixXd transform;
Random::RandomGenerator gen;
std::normal_distribution<> dist;
Random::RandomGenerator gen;
std::normal_distribution<> dist;
public:
/** ctor */
NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) :
mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()), eigenSolver(sigma) {
/** empty ctor */
NormalDistributionN() {
;
}
transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
}
/** ctor */
NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) :
mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) {
const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(sigma);
transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
}
/** get probability for the given value */
double getProbability(const Eigen::VectorXd val) const {
@@ -50,12 +56,12 @@ namespace Distribution {
}
/** get the mean vector */
const Eigen::VectorXd getMu(){
const Eigen::VectorXd getMu() const {
return this->mu;
}
/** get covariance matrix */
const Eigen::MatrixXd getSigma(){
const Eigen::MatrixXd getSigma() const {
return this->sigma;
}
@@ -71,6 +77,34 @@ namespace Distribution {
this->mu = mu;
}
/** return a NormalN based on given data */
static NormalDistributionN getNormalNFromSamples(const std::vector<std::vector<double>>& data) {
int numAttrs = data[0].size();
Eigen::MatrixXd eCovar = Eigen::MatrixXd::Zero(numAttrs, numAttrs);
Eigen::VectorXd eSum = Eigen::VectorXd::Zero(numAttrs);
Eigen::VectorXd eAvg = Eigen::VectorXd::Zero(numAttrs);
// mean
for (const std::vector<double>& vec : data) {
const Eigen::Map<Eigen::VectorXd> eVec((double*)vec.data(), vec.size());
eSum += eVec;
}
eAvg = eSum / data.size();
// covariance
for (const std::vector<double>& vec : data) {
const Eigen::Map<Eigen::VectorXd> eVec((double*)vec.data(), vec.size());
const Eigen::VectorXd eTmp = (eVec - eAvg);
eCovar += eTmp * eTmp.transpose();
}
eCovar /= data.size();
return NormalDistributionN(eAvg, eCovar);
}
/** return a NormalN based on given data */
static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) {
@@ -98,7 +132,7 @@ namespace Distribution {
return NormalDistributionN(mean, cov);
}
std::vector<Point2> getContour2(const double percentWithin) {
std::vector<Point2> getContour2(const double percentWithin) const {
const int degreesOfFreedom = 2; // 2D distribution
const ChiSquared<double> chi(degreesOfFreedom);
@@ -114,7 +148,7 @@ namespace Distribution {
std::vector<Point2> res;
std::cout << sigma << std::endl;
//std::cout << sigma << std::endl;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(this->sigma);
Eigen::Vector2d evec1 = solver.eigenvectors().col(0);
@@ -127,7 +161,8 @@ namespace Distribution {
const float rad = deg / 180.0f * M_PI;
Eigen::Vector2d pos =
std::cos(rad) * std::sqrt(mul * eval1) * evec1 +
std::sin(rad) * std::sqrt(mul * eval2) * evec2;
std::sin(rad) * std::sqrt(mul * eval2) * evec2 +
this->mu;
res.push_back(Point2(pos(0), pos(1)));
}