fixed typo in normal, added gt point 3d

math/distribution/NormalN.h

@@ -75,6 +75,19 @@ namespace Distribution {
                 return NormalDistributionN(mean, cov);
         }
 
+        /** return a NormalN based on given data and a given mean vector mu*/
+        static NormalDistributionN getNormalNFromSamplesAndMean(const Eigen::MatrixXd& data, const Eigen::VectorXd mean) {
+
+                const int numElements = data.rows();
+                Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the distribution!");
+                Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!");
+
+                const Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
+                const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1);
+
+                return NormalDistributionN(mean, cov);
+        }
+
     };
 
 }