Moved eval into own cpp to allow shorter compile times

code/CMakeLists.txt

@@ -38,6 +38,7 @@ FILE(GLOB HEADERS
     trilateration.h
     Plotta.h
     misc.h
+    Eval.h
 )
 
 
@@ -48,6 +49,7 @@ FILE(GLOB SOURCES
     mainFtm.cpp
     mainTrilat.cpp
     mainProb.cpp
+    Eval.cpp
 )
 
 

code/Eval.cpp

@@ -0,0 +1,70 @@
+#include "Eval.h"
+
+#include "Settings.h"
+
+#include <Indoor/math/distribution/Normal.h>
+
+double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector<WiFiMeasurement>& measurements, std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters)
+{
+    double result = 1.0;
+
+    for (WiFiMeasurement wifi : measurements)
+    {
+        if (wifi.getNumSuccessfulMeasurements() < 3)
+        {
+            continue;
+        }
+
+        const MACAddress& mac = wifi.getAP().getMAC();
+        int nucIndex = Settings::nucIndex(mac);
+
+        const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position;
+        // particlePos.z = 1.3; // smartphone h<>he
+        const float apDist = particlePos.getDistance(apPos);
+
+        // 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
+
+        float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss;
+        float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
+
+        if (UseSensor == SensorMode::FTM)
+        {
+            if (ftmDist > 0)
+            {
+                //double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
+                double sigma = 5;
+
+                if (ftmKalmanFilters != nullptr)
+                {
+                    Kalman& kalman = ftmKalmanFilters->at(mac);
+                    ftmDist = kalman.predict(currentTime, ftmDist);
+                    //sigma = std::sqrt(kalman.P(0, 0));
+
+                    Assert::isTrue(sigma > 0, "sigma");
+
+                    double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
+
+                    result *= x;
+                }
+                else
+                {
+                    double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
+
+                    result *= x;
+                }
+            }
+        }
+        else
+        {
+            // RSSI
+            double sigma = 5;
+
+            double x = Distribution::Normal<double>::getProbability(rssiDist, sigma, apDist);
+            result *= x;
+        }
+    }
+
+    return result;
+}

code/Eval.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <unordered_map>
+#include <vector>
+
+#include <Indoor/geo/Point3.h>
+
+#include "FtmKalman.h"
+#include <Indoor/sensors/radio/WiFiMeasurements.h>
+#include <Indoor/sensors/radio/model/LogDistanceModel.h>
+
+
+enum class SensorMode
+{
+    FTM,
+    RSSI
+};
+
+double ftmEval(SensorMode UseSensor,
+               const Timestamp& currentTime,
+               const Point3& particlePos,
+               const std::vector<WiFiMeasurement>& measurements,
+                     std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters);

code/filter.h

@@ -34,6 +34,7 @@
 #include <Indoor/sensors/activity/ActivityDetector.h>
 
 #include "FtmKalman.h"
+#include "Eval.h"
 
 struct MyState {
 
@@ -269,72 +270,19 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
     virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
 
         double sum = 0;
-
+        
 		#pragma omp parallel for num_threads(3)
 		for (int i = 0; i < particles.size(); ++i) {
             SMC::Particle<MyState>& p = particles[i];
 
-            double pFtm = 1.0;
+            auto kalmanFilters = ftmKalmanFilters;
 
-            for (WiFiMeasurement wifi : observation.ftm)
+            if (!Settings::UseKalman)
             {
-                if (wifi.getNumSuccessfulMeasurements() < 3)
+                kalmanFilters = nullptr;
-                {
-                    continue;
-                }
-
-                const MACAddress& mac = wifi.getAP().getMAC();
-                int nucIndex = Settings::nucIndex(mac);
-
-                const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position;
-                Point3 particlePos = p.state.pos.pos;
-                particlePos.z = 1.3; // smartphone höhe
-                const float apDist = particlePos.getDistance(apPos);
-
-                // 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
-
-                float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss;
-                float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
-                
-
-                if (ftmDist > 0)
-                {
-                    double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
-
-                    if (sigma == 0)
-                    {
-                        sigma = 38*38;
-                    }
-
-                    if (Settings::UseKalman)
-                    {
-                        Kalman& kalman = ftmKalmanFilters->at(mac);
-                        ftmDist = kalman.predict(observation.currentTime, ftmDist);
-                        sigma = kalman.P(0, 0);
-
-                        //Assert::isTrue(sigma > 0, "sigma");
-                        if (sigma <= 0)
-                        {
-                            sigma = 38 * 38;
-                        }
-
-                        double x = Distribution::Normal<double>::getProbability(ftmDist, std::sqrt(sigma), apDist);
-
-                        pFtm *= x;
-                    }
-                    else
-                    {
-                        double x = Distribution::Normal<double>::getProbability(ftmDist, std::sqrt(sigma), apDist);
-
-                        pFtm *= x;
-                    }
-                }
             }
 
-
+            double prob = ftmEval(SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters);
-            double prob = pFtm;
 
             if (assignProps)
                 p.weight = prob;