FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added gen7 NUCs and path 10/11

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-13 16:16:53 +01:00
parent ac824937aa
commit 9700f88449
3 changed files with 104 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/Eval.cpp
View File

@@ -60,7 +60,7 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
result *= x;
}
hadMeas[nucIndex] = true;
// hadMeas[nucIndex] = true; TODO
}
}
else

62
code/Settings.h
View File

@@ -16,10 +16,17 @@ namespace Settings {
/** describes one dataset (map, training, parameter-estimation, ...) */
// NUC Gen6 - Intel AC 8260
const MACAddress NUC1("38:de:ad:6d:77:25");
const MACAddress NUC2("38:de:ad:6d:60:ff");
const MACAddress NUC3("1c:1b:b5:ef:a2:9a");
const MACAddress NUC4("1c:1b:b5:ec:d1:82");
// NUC Gen7 - Intel AC 9461
const MACAddress NUC5("d0:c6:37:bc:5c:41");
const MACAddress NUC6("d0:c6:37:bc:77:8a");
const MACAddress NUC7("d0:c6:37:bc:77:ad");
const MACAddress NUC8("d0:c6:37:bc:6b:4b");
static int nucIndex(const MACAddress& addr)
{
@@ -27,6 +34,10 @@ namespace Settings {
if (addr == Settings::NUC2) return 1;
if (addr == Settings::NUC3) return 2;
if (addr == Settings::NUC4) return 3;
if (addr == Settings::NUC5) return 4;
if (addr == Settings::NUC6) return 5;
if (addr == Settings::NUC7) return 6;
if (addr == Settings::NUC8) return 7;
else {
assert(false); return 0;
}
@@ -40,6 +51,10 @@ namespace Settings {
case 1: return NUC2;
case 2: return NUC3;
case 3: return NUC4;
case 4: return NUC5;
case 5: return NUC6;
case 6: return NUC7;
case 7: return NUC8;
default: assert(false); return NUC1;
}
}
@@ -285,8 +300,53 @@ namespace Settings {
{ 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 },
true
};
// 10 Path: SHL Path 2; NUCs steht in den Räumen; Neue gen7 NUCs mit interner Antenne
const DataSetup Path10 = {
"path10",
mapDir + "shl_nuc_2.xml",
{
dataDir + "Pixel2/path10/1470971060974.csv",
dataDir + "Pixel2/path10/1578633767584.csv",
dataDir + "Pixel3/path10/3600316210200.csv",
dataDir + "Pixel3/path10/3715912582913.csv",
},
{
// NUC, ID Pos X Y Z offset loss kalman stddev
{ NUC1, {1, { 7, 6, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 I.2.14
{ NUC5, {5, { 54, 46, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 5 I.2.37
{ NUC6, {6, { 46, 37, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 6 I.2.03
{ NUC7, {7, { 27, 45, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 7 I.2.28
{ NUC8, {8, { 16, 36, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 8 I.2.10
},
{ 100, 101, 102, 103, 104, 103, 102, 101, 100 },
true
};
// 11 Path: SHL Path 2; NUCs steht in den Räumen; Neue gen7 NUCs mit interner Antenne
const DataSetup Path11 = {
"path11",
mapDir + "shl_nuc_2.xml",
{
dataDir + "Pixel2/path11/1688129692555.csv",
dataDir + "Pixel2/path11/1841800940572.csv",
dataDir + "Pixel3/path11/3833358280134.csv",
dataDir + "Pixel3/path11/3997838116311.csv",
},
{
// NUC, ID Pos X Y Z offset loss kalman stddev
{ NUC1, {1, { 7, 6, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 I.2.14
{ NUC5, {5, { 54, 46, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 5 I.2.37
{ NUC6, {6, { 46, 37, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 6 I.2.03
{ NUC7, {7, { 27, 45, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 7 I.2.28
{ NUC8, {8, { 16, 36, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 8 I.2.10
},
{ 100, 101, 102, 103, 104, 105, 104, 103, 102, 101, 100 },
true
};
} data;
static DataSetup CurrentPath = data.Path9;
static DataSetup CurrentPath = data.Path10;
}

80
code/main.cpp
View File

@@ -255,9 +255,9 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
// ground truth
std::vector<int> gtPath = setup.gtPath;
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
CombinedStats<float> errorStats;
@@ -281,14 +281,14 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
// error file
const long int t = static_cast<long int>(time(NULL));
auto evalDir = std::filesystem::path(Settings::errorDir);
evalDir.append(folder);
evalDir.append(folder);
if (!std::filesystem::exists(evalDir)) {
std::filesystem::create_directory(evalDir);
}
}
std::ofstream errorFile;
errorFile.open (evalDir.string() + "/" + std::to_string(walkIdx) + "_" + std::to_string(t) + ".csv");
errorFile.open(evalDir.string() + "/" + std::to_string(walkIdx) + "_" + std::to_string(t) + ".csv");
// Output dir
auto outputDir = std::filesystem::path(Settings::outputDir);
@@ -300,10 +300,11 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
// wifi
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
for (auto& nucConfig : setup.NUCs)
{
kalmanMap->insert({ nucConfig.first, Kalman(nucConfig.second.ID, nucConfig.second.kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
}
std::cout << "Optimal wifi parameters for " << setup.training[walkIdx] << "\n";
optimizeWifiParameters(fr, gtInterpolator);
@@ -328,10 +329,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
// APs Positions
plot.addCircle(100000 + 0, Settings::CurrentPath.nucInfo(0).position.xy(), 0.1);
plot.addCircle(100000 + 1, Settings::CurrentPath.nucInfo(1).position.xy(), 0.1);
plot.addCircle(100000 + 2, Settings::CurrentPath.nucInfo(2).position.xy(), 0.1);
plot.addCircle(100000 + 3, Settings::CurrentPath.nucInfo(3).position.xy(), 0.1);
for (auto& nucConfig : setup.NUCs)
{
plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
}
plot.plot();
// particle-filter
@@ -376,10 +377,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
std::unordered_map<MACAddress, MovingMedianTS2> movMedianPerAP;
movMedianPerAP[Settings::NUC1] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC2] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC3] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC4] = MovingMedianTS2(Timestamp::fromMS(500));
for (auto& nucConfig : setup.NUCs)
{
movMedianPerAP[nucConfig.first] = MovingMedianTS2(Timestamp::fromMS(500));
}
for (const Offline::Entry& e : fr.getEntries())
{
@@ -400,10 +401,11 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(1).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(2).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(3).position.xy()) });
// TODO
//gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(1).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(2).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(3).position.xy()) });
Point3 estPos;
float distErrorFtm = 0;
@@ -450,22 +452,22 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
}
// Store measurements
for (WiFiMeasurement wifi : obs.ftm)
{
if (wifi.getNumSuccessfulMeasurements() < 3)
{
continue;
}
//for (WiFiMeasurement wifi : obs.ftm)
//{
// if (wifi.getNumSuccessfulMeasurements() < 3)
// {
// continue;
// }
Point2 gtPos2 = gtInterpolator.get(static_cast<uint64_t>(wifi.getTimestamp().ms())).xy();
Point2 apPos2 = Settings::CurrentPath.NUCs[wifi.getAP().getMAC()].position.xy();
// Point2 gtPos2 = gtInterpolator.get(static_cast<uint64_t>(wifi.getTimestamp().ms())).xy();
// Point2 apPos2 = Settings::CurrentPath.NUCs[wifi.getAP().getMAC()].position.xy();
float gtDist2 = gtPos2.getDistance(apPos2);
// float gtDist2 = gtPos2.getDistance(apPos2);
// store distances
const int nucIdx = Settings::nucIndex(wifi.getAP().getMAC());
ftmDistances[nucIdx].add(wifi.getTimestamp(), { wifi.getFtmDist(), gtDist2, wifi.getRSSI() });
}
// // store distances
// const int nucIdx = Settings::nucIndex(wifi.getAP().getMAC());
// ftmDistances[nucIdx].add(wifi.getTimestamp(), { wifi.getFtmDist(), gtDist2, wifi.getRSSI() }); // TODO
//}
// Kalman debugging (can't be used with active PF)
//{
@@ -692,9 +694,11 @@ int main(int argc, char** argv)
std::vector<Settings::DataSetup> setupsToRun = {
//Settings::data.Path5,
Settings::data.Path7,
Settings::data.Path8,
//Settings::data.Path9
//Settings::data.Path7,
//Settings::data.Path8,
//Settings::data.Path9,
//Settings::data.Path10,
Settings::data.Path11
};
for (Settings::DataSetup setupToRun : setupsToRun)
@@ -702,7 +706,7 @@ int main(int argc, char** argv)
Settings::CurrentPath = setupToRun;
for (size_t walkIdx = 0; walkIdx < 1 /*Settings::CurrentPath.training.size()*/; walkIdx++)
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)