Refactored probabilistic code

code/mainProb.cpp

@@ -30,7 +30,7 @@
 static float kalman_procNoiseDistStdDev = 1.2f;  // standard deviation of distance for process noise 
 static float kalman_procNoiseVelStdDev = 0.1f;   // standard deviation of velocity for process noise 
 
-static void poorMansKDE(const BBox3& bbox, float sigma, std::array<float, 4> dist, std::array<Point2, 4> apPos, std::vector<std::pair<Point2, float>>& density, std::pair<Point2, float>& maxElem)
+static void poorMansKDE(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::function<double(Point2 pt)>& evalProc)
 {
     density.clear();
 
@@ -48,35 +48,99 @@ static void poorMansKDE(const BBox3& bbox, float sigma, std::array<float, 4> dis
         {
             const Point2 pos(x, y);
 
-            float P = 1.0f;
+            double P = evalProc(pos);
-
-            for (size_t i = 0; i < 4; i++)
-            {
-                // TODO: Was mit nan machen?
-                if (!isnan(dist[i]))
-                {
-                    float d = pos.getDistance(apPos[i]) - dist[i];
-                    float p = Distribution::Normal<float>::getProbability(0, sigma, d);
-                    P *= p;
-                }
-            }
 
             density.push_back({ pos, P });
         }
     }
 
-    auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, float> a, std::pair<Point2, float> b) {
+    auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, double> a, std::pair<Point2, double> b) {
         return a.second < b.second;
     });
 
     maxElem = *maxElement;
 }
 
+static void computeDensity(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::vector<WiFiMeasurement>& obs, bool useFtm, double sigma)
+{
+    poorMansKDE(bbox, density, maxElem, [&obs, useFtm, sigma](Point2 pt) {
+        double p = 1.0;
+
+        for (const WiFiMeasurement& wifi : obs)
+        {
+            if (wifi.getNumSuccessfulMeasurements() < 3)
+                continue;
+
+            const MACAddress& mac = wifi.getAP().getMAC();
+            int nucIndex = Settings::nucIndex(mac);
+
+            // compute AP distance
+            const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position;
+            Point3 pos = Point3(pt.x, pt.y, 1.3); // smartphone h<>he
+            const float apDist = pos.getDistance(apPos);
+
+            double dist = 0;
+            if (useFtm)
+            {
+                // compute ftm distance
+                float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset;
+                float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset
+
+                dist = ftmDist;
+            }
+            else
+            {
+                // compute rssi distance
+                float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss;
+                float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.25 /*rssi_pathloss*/, wifi.getRSSI());
+
+                dist = rssiDist;
+            }
+
+            if (dist > 0)
+            {
+                double d = apDist - dist;
+                double x = Distribution::Normal<double>::getProbability(0, 3.5, d);
+
+                p *= x;
+            }
+        }
+
+        return p;
+    });
+}
+
+static void plotDensity(Plotty& plot, std::vector<std::pair<Point2, double>>& density)
+{
+    plot.particles.clear();
+
+    double min = std::numeric_limits<double>::max();
+    double max = std::numeric_limits<double>::min();
+
+    for (auto it = density.begin(); ; std::advance(it, 2))
+    {
+        if (it > density.end())
+            break;
+
+        auto p = *it;
+
+        const K::GnuplotPoint3 p3(p.first.x, p.first.y, 0.1);
+        const double prob = std::pow(p.second, 0.25);
+
+        plot.particles.add(p3, prob);
+
+        if (prob > max) { max = prob; }
+        if (prob < min) { min = prob; }
+    }
+
+    plot.splot.getAxisCB().setRange(min, max + 0.000001);
+}
+
 static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::string folder)
 {
     // reading file
     Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
-    Offline::FileReader fr(setup.training[walkIdx]);
+    Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
 
     // ground truth
     std::vector<int> gtPath = setup.gtPath;
@@ -113,7 +177,7 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     plot.setGroundTruth(gtPath);
     plot.setView(30, 0);
     plot.plot();
-
+    
     // wifi
     std::array<Kalman, 4> ftmKalmanFilters{
         Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
@@ -129,7 +193,9 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
         Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
     };
 
-    std::vector<WifiMeas> data = filterOfflineData(fr);
+    std::vector<WiFiMeasurement> obs;
+
+    Timestamp lastTimestamp = Timestamp::fromMS(0);
 
     const float sigma = 3.5;
     const int movAvgWnd = 15;
@@ -139,124 +205,85 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     std::vector<float> errorValuesFtm, errorValuesRssi;
     std::vector<int> timestamps;
 
-    for (const WifiMeas& wifi : data)
+    for (const Offline::Entry& e : fr.getEntries())
     {
-        Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
+        if (e.type != Offline::Sensor::WIFI_FTM) {
+            continue;
+        }
 
-        Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
+        // TIME
-        plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
+        const Timestamp ts = Timestamp::fromMS(e.ts);
 
-        float distErrorFtm = 0;
+        auto wifiFtm = fr.getWifiFtm()[e.idx].data;
-        float distErrorRssi = 0;
+        obs.push_back(wifiFtm);
 
-        //if (wifi.numSucessMeas() == 4)
+        if (ts - lastTimestamp >= Timestamp::fromMS(500))
         {
+            // Do update
+            Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
+
+            Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
+            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
+
+            float distErrorFtm = 0;
+            float distErrorRssi = 0;
+
             // FTM
             {
-                std::array<float, 4> dists = wifi.ftmDists;
-
-                if (Settings::UseKalman)
-                {
-                    std::cout << "Using Kalman" << "\n";
-
-                    for (size_t i = 0; i < 4; i++)
-                    {
-                        if (!isnan(dists[i]))
-                        {
-                            dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
-                        }
-                    }
-                }
-
                 BBox3 bbox = FloorplanHelper::getBBox(map);
-                std::vector<std::pair<Point2, float>> density;
+                std::vector<std::pair<Point2, double>> density;
-                std::pair<Point2, float> maxElement;
+                std::pair<Point2, double> maxElement;
 
-                poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
+                computeDensity(bbox, density, maxElement, obs, true, 3.5);
 
                 Point2 estPos = maxElement.first;
-
-                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
                 plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
+                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
+
+                // Plot density
+                //plotDensity(plot, density);
 
                 // Error
                 distErrorFtm = gtPos.getDistance(estPos);
                 errorStats.ftm.add(distErrorFtm);
-
-
-                //std::vector<float> densityX, densityY, densityZ;
-                //for (const auto& item : density)
-                //{
-                //    densityX.push_back(item.first.x);
-                //    densityY.push_back(item.first.y);
-                //    densityZ.push_back(item.second);
-                //}
-
-                //test.add("densityX", densityX);
-                //test.add("densityY", densityY);
-                //test.add("densityZ", densityZ);
             }
 
             // RSSI
             {
-                std::array<float, 4> dists = wifi.rssiDists;
-
-                if (Settings::UseKalman)
-                {
-                    for (size_t i = 0; i < 4; i++)
-                    {
-                        if (!isnan(dists[i]))
-                        {
-                            dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
-                        }
-                    }
-                }
-
-
                 BBox3 bbox = FloorplanHelper::getBBox(map);
-                std::vector<std::pair<Point2, float>> density;
+                std::vector<std::pair<Point2, double>> density;
-                std::pair<Point2, float> maxElement;
+                std::pair<Point2, double> maxElement;
 
-                poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
+                computeDensity(bbox, density, maxElement, obs, false, 8);
 
                 Point2 estPos = maxElement.first;
-
-                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
                 plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
 
+                // Plot density
+                //plotDensity(plot, density);
+
                 // Error
                 distErrorRssi = gtPos.getDistance(estPos);
                 errorStats.rssi.add(distErrorRssi);
-
-
-                //std::vector<float> densityX, densityY, densityZ;
-                //for (const auto& item : density)
-                //{
-                //    densityX.push_back(item.first.x);
-                //    densityY.push_back(item.first.y);
-                //    densityZ.push_back(item.second);
-                //}
-
-                //test.add("densityX", densityX);
-                //test.add("densityY", densityY);
-                //test.add("densityZ", densityZ);
             }
 
-            //std::cout << wifi.ts.ms() << " " << distError << "\n";
 
             errorValuesFtm.push_back(distErrorFtm);
             errorValuesRssi.push_back(distErrorRssi);
-            timestamps.push_back(wifi.ts.ms());
+            timestamps.push_back(ts.ms());
 
             test.add("t", timestamps);
             test.add("errorFtm", errorValuesFtm);
             test.add("errorRssi", errorValuesRssi);
             test.frame();
-        }
 
-        plot.plot();
+            plot.plot();
-        //Sleep(250);
+            //Sleep(250);
-        printf("");
+            printf("");
+
+            lastTimestamp = ts;
+            obs.clear();
+        }
     }
 
     printErrorStats(errorStats);