From 485be83b449048efdef2523f2d931941b756ea8b Mon Sep 17 00:00:00 2001
From: toni <toni@localhost.localdomain>
Date: Mon, 20 Mar 2017 11:42:46 +0100
Subject: [PATCH] fixed typo in normal, added gt point 3d

---
 floorplan/v2/Floorplan.h       |  4 ++--
 floorplan/v2/FloorplanReader.h | 32 ++++++++++++++++----------------
 floorplan/v2/FloorplanWriter.h | 19 ++++++++++---------
 math/distribution/NormalN.h    | 13 +++++++++++++
 4 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h
index cfdee4c..0a71fb0 100644
--- a/floorplan/v2/Floorplan.h
+++ b/floorplan/v2/Floorplan.h
@@ -251,9 +251,9 @@ namespace Floorplan {
         /** a GroundTruthPoint located somewhere on a floor */
         struct GroundTruthPoint {
             int id; //TODO: this value can be changed and isn't set incremental within the indoormap
-            Point2 pos;
+            Point3 pos;
             GroundTruthPoint() : id(), pos() {;}
-            GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;}
+            GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;}
             bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
         };
 
diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h
index cb29bcc..ba26232 100644
--- a/floorplan/v2/FloorplanReader.h
+++ b/floorplan/v2/FloorplanReader.h
@@ -111,7 +111,7 @@ namespace Floorplan {
 				if (std::string("pois") == n->Name())			{floor->pois = parseFloorPOIs(n);}
 				if (std::string("stairs") == n->Name())			{floor->stairs = parseFloorStairs(n);}
 				if (std::string("elevators") == n->Name())		{floor->elevators = parseFloorElevators(n);}
-                if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
+                                if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
 			}
 			return floor;
 		}
@@ -198,22 +198,22 @@ namespace Floorplan {
 		}
 
 
-        /** parse the <gtpoints> tag */
-        static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
-            std::vector<GroundTruthPoint*> vec;
-            FOREACH_NODE(n, el) {
-                if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
-            }
-            return vec;
-        }
+                /** parse the <gtpoints> tag */
+                static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
+                    std::vector<GroundTruthPoint*> vec;
+                    FOREACH_NODE(n, el) {
+                        if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
+                    }
+                    return vec;
+                }
 
-        /** parse a <gtpoint> tag */
-        static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
-            GroundTruthPoint* gtp = new GroundTruthPoint();
-            gtp->id = el->IntAttribute("id");
-            gtp->pos = parsePoint2(el);
-            return gtp;
-        }
+                /** parse a <gtpoint> tag */
+                static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
+                    GroundTruthPoint* gtp = new GroundTruthPoint();
+                    gtp->id = el->IntAttribute("id");
+                    gtp->pos = parsePoint3(el);
+                    return gtp;
+                }
 
 
 		/** parse the <accesspoints> tag */
diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h
index c8257d1..3c09292 100644
--- a/floorplan/v2/FloorplanWriter.h
+++ b/floorplan/v2/FloorplanWriter.h
@@ -146,15 +146,16 @@ namespace Floorplan {
 			}
 			floor->InsertEndChild(pois);
 
-            XMLElem* gtpoints = doc.NewElement("gtpoints");
-            for (const GroundTruthPoint* gtp : mf->gtpoints) {
-                XMLElem* elem = doc.NewElement("gtpoint");
-                elem->SetAttribute("id", gtp->id);
-                elem->SetAttribute("x", gtp->pos.x);
-                elem->SetAttribute("y", gtp->pos.y);
-                gtpoints->InsertEndChild(elem);
-            }
-            floor->InsertEndChild(gtpoints);
+                        XMLElem* gtpoints = doc.NewElement("gtpoints");
+                        for (const GroundTruthPoint* gtp : mf->gtpoints) {
+                            XMLElem* elem = doc.NewElement("gtpoint");
+                            elem->SetAttribute("id", gtp->id);
+                            elem->SetAttribute("x", gtp->pos.x);
+                            elem->SetAttribute("y", gtp->pos.y);
+                            elem->SetAttribute("z", gtp->pos.z);
+                            gtpoints->InsertEndChild(elem);
+                        }
+                        floor->InsertEndChild(gtpoints);
 
 			XMLElem* accesspoints = doc.NewElement("accesspoints");
 			for (const AccessPoint* ap : mf->accesspoints) {
diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h
index ad2f04e..d98c4d9 100644
--- a/math/distribution/NormalN.h
+++ b/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);
+        }
+
     };
 
 }