Initial project version
This commit is contained in:
2
.gitignore
vendored
Normal file
2
.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
build/
|
||||
measurements/error
|
||||
114
code/CMakeLists.txt
Normal file
114
code/CMakeLists.txt
Normal file
@@ -0,0 +1,114 @@
|
||||
# Usage:
|
||||
# Create build folder, like RC-build next to RobotControl and WifiScan folder
|
||||
# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl'
|
||||
# make
|
||||
|
||||
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
|
||||
|
||||
# select build type
|
||||
SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
|
||||
|
||||
PROJECT(ProLogic)
|
||||
|
||||
IF(NOT CMAKE_BUILD_TYPE)
|
||||
MESSAGE(STATUS "No build type selected. Default to Debug")
|
||||
SET(CMAKE_BUILD_TYPE "Debug")
|
||||
ENDIF()
|
||||
|
||||
|
||||
|
||||
INCLUDE_DIRECTORIES(
|
||||
../
|
||||
../../
|
||||
../../../
|
||||
../../../../
|
||||
)
|
||||
|
||||
|
||||
FILE(GLOB HEADERS
|
||||
filter.h
|
||||
mesh.h
|
||||
meshPlotter.h
|
||||
Plotti.h
|
||||
Plotty.h
|
||||
Settings.h
|
||||
FtmKalman.h
|
||||
main.h
|
||||
mainFtm.h
|
||||
)
|
||||
|
||||
|
||||
FILE(GLOB SOURCES
|
||||
../../Indoor/lib/tinyxml/tinyxml2.cpp
|
||||
../../Indoor/lib/Recast/*.cpp
|
||||
main.cpp
|
||||
mainFtm.cpp
|
||||
)
|
||||
|
||||
|
||||
if ("${CMAKE_GENERATOR}" MATCHES "Visual Studio*")
|
||||
add_definitions(
|
||||
-D_USE_MATH_DEFINES
|
||||
-DUNICODE
|
||||
-D_UNICODE
|
||||
-DNOGDI
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++17")
|
||||
else()
|
||||
# system specific compiler flags
|
||||
ADD_DEFINITIONS(
|
||||
|
||||
#-std=gnu++14
|
||||
|
||||
-lstdc++fs
|
||||
|
||||
-Wall
|
||||
-Werror=return-type
|
||||
-Wextra
|
||||
-Wpedantic
|
||||
|
||||
-fstack-protector-all
|
||||
|
||||
-g3
|
||||
#-O2
|
||||
-march=native
|
||||
|
||||
)
|
||||
endif()
|
||||
|
||||
add_definitions(
|
||||
# -DWITH_TESTS
|
||||
-DWITH_ASSERTIONS
|
||||
-DWITH_DEBUG_LOG
|
||||
# -DWITH_DEBUG_PLOT
|
||||
# -D_GLIBCXX_DEBUG
|
||||
)
|
||||
|
||||
# allow OMP
|
||||
find_package(OpenMP)
|
||||
if (OPENMP_FOUND)
|
||||
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
||||
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
|
||||
endif()
|
||||
|
||||
|
||||
# build a binary file
|
||||
ADD_EXECUTABLE(
|
||||
${PROJECT_NAME}
|
||||
${HEADERS}
|
||||
${SOURCES}
|
||||
)
|
||||
|
||||
# needed external libraries
|
||||
TARGET_LINK_LIBRARIES(
|
||||
${PROJECT_NAME}
|
||||
# stdc++fs
|
||||
# gtest
|
||||
# pthread
|
||||
)
|
||||
|
||||
SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER})
|
||||
|
||||
76
code/FtmKalman.h
Normal file
76
code/FtmKalman.h
Normal file
@@ -0,0 +1,76 @@
|
||||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Eigen>
|
||||
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
|
||||
|
||||
struct Kalman
|
||||
{
|
||||
int nucID = 0; // debug only
|
||||
|
||||
Eigen::Matrix<float, 2, 1> x; // predicted state
|
||||
Eigen::Matrix<float, 2, 2> P; // Covariance
|
||||
|
||||
float R = 30; // measurement noise covariance
|
||||
|
||||
float lastTimestamp = NAN; // in sec
|
||||
|
||||
Kalman(): nucID(0) { }
|
||||
|
||||
Kalman(int nucID)
|
||||
: nucID(nucID)
|
||||
{}
|
||||
|
||||
Kalman(int nucID, float measStdDev)
|
||||
: nucID(nucID), R(measStdDev*measStdDev)
|
||||
{}
|
||||
|
||||
float predict(const Timestamp timestamp, const float measurment)
|
||||
{
|
||||
constexpr auto square = [](float x) { return x * x; };
|
||||
const auto I = Eigen::Matrix2f::Identity();
|
||||
|
||||
// init kalman filter
|
||||
if (isnan(lastTimestamp))
|
||||
{
|
||||
P << 10, 0,
|
||||
0, 10; // Initial Uncertainty
|
||||
|
||||
x << measurment,
|
||||
0;
|
||||
}
|
||||
|
||||
const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
|
||||
lastTimestamp = timestamp.sec();
|
||||
|
||||
Eigen::Matrix<float, 1, 2> H; // Measurement function
|
||||
H << 1, 0;
|
||||
|
||||
Eigen::Matrix2f A; // Transition Matrix
|
||||
A << 1, dt,
|
||||
0, 1;
|
||||
|
||||
Eigen::Matrix2f Q; // Process Noise Covariance
|
||||
Q << 0, 0,
|
||||
0, square(0.3);
|
||||
|
||||
// Prediction
|
||||
x = A * x; // Pr<50>dizierter Zustand aus Bisherigem und System
|
||||
P = A * P*A.transpose()+Q; // Pr<50>dizieren der Kovarianz
|
||||
|
||||
// Correction
|
||||
float Z = measurment;
|
||||
auto y = Z - (H*x); // Innovation aus Messwertdifferenz
|
||||
auto S = (H*P*H.transpose()+R); // Innovationskovarianz
|
||||
auto K = P * H.transpose()* (1/S); //Filter-Matrix (Kalman-Gain)
|
||||
|
||||
x = x + (K*y); // aktualisieren des Systemzustands
|
||||
P = (I - (K*H))*P; // aktualisieren der Kovarianz
|
||||
|
||||
return x(0);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
348
code/Plotti.h
Normal file
348
code/Plotti.h
Normal file
@@ -0,0 +1,348 @@
|
||||
#pragma once
|
||||
|
||||
#include "Settings.h"
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include <Indoor/geo/Point2.h>
|
||||
#include <Indoor/geo/Point3.h>
|
||||
|
||||
#include <Indoor/floorplan/v2/Floorplan.h>
|
||||
|
||||
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
||||
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
||||
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
|
||||
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
||||
|
||||
#include <Indoor/smc/filtering/ParticleFilter.h>
|
||||
|
||||
struct Plotti {
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotSplot splot;
|
||||
K::GnuplotSplotElementPoints pGrid;
|
||||
K::GnuplotSplotElementLines pFloor;
|
||||
K::GnuplotSplotElementLines pOutline;
|
||||
K::GnuplotSplotElementLines pStairs;
|
||||
K::GnuplotSplotElementPoints pAPs;
|
||||
K::GnuplotSplotElementPoints pInterest;
|
||||
K::GnuplotSplotElementPoints pParticles;
|
||||
K::GnuplotSplotElementPoints pNormal1;
|
||||
K::GnuplotSplotElementPoints pNormal2;
|
||||
K::GnuplotSplotElementColorPoints pDistributation1;
|
||||
K::GnuplotSplotElementColorPoints pDistributation2;
|
||||
K::GnuplotSplotElementColorPoints pColorPoints;
|
||||
K::GnuplotSplotElementLines gtPath;
|
||||
K::GnuplotSplotElementLines estPath;
|
||||
K::GnuplotSplotElementLines estPathSmoothed;
|
||||
|
||||
Plotti() {
|
||||
gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
|
||||
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.getColor().setHexStr("#888888");
|
||||
splot.add(&pAPs); pAPs.setPointSize(0.7);
|
||||
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
|
||||
splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
|
||||
splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
|
||||
splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f);
|
||||
splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f);
|
||||
splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f);
|
||||
splot.add(&pFloor);
|
||||
splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
|
||||
splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000");
|
||||
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000");
|
||||
splot.add(>Path); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000");
|
||||
splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00");
|
||||
splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#0000ff");
|
||||
}
|
||||
|
||||
void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
|
||||
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
|
||||
}
|
||||
|
||||
void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
|
||||
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
|
||||
}
|
||||
|
||||
|
||||
void showAngle(const int idx, const float rad, const Point2 cen, const std::string& str) {
|
||||
|
||||
Point2 rot(0, 1);
|
||||
Point2 pos = cen + rot.rotated(rad) * 0.05;
|
||||
|
||||
gp << "set label "<<idx<<" at screen " << cen.x << "," << cen.y << " '" << str << "'"<< "\n";
|
||||
gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";
|
||||
|
||||
}
|
||||
|
||||
void setEst(const Point3 pos) {
|
||||
gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << std::round(pos.z * 10) / 10 << " to " << pos.x << "," << pos.y << "," << (std::round(pos.z * 10) / 10)+1 << " nohead lw 1 front \n";
|
||||
}
|
||||
|
||||
void setGT(const Point3 pos) {
|
||||
gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
|
||||
gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
|
||||
}
|
||||
|
||||
void setTimeInMinute(const int minutes, const int seconds) {
|
||||
gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
|
||||
}
|
||||
|
||||
void addGroundTruthNode(const Point3 pos) {
|
||||
K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
gtPath.add(gp);
|
||||
}
|
||||
|
||||
// estimated path
|
||||
void addEstimationNode(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
estPath.add(est);
|
||||
}
|
||||
|
||||
// estimated path
|
||||
void addEstimationNodeSmoothed(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
estPathSmoothed.add(est);
|
||||
}
|
||||
|
||||
void debugDistribution1(std::vector<SMC::Particle<MyState>> samples){
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
pDistributation1.clear();
|
||||
for (int i = 0; i < samples.size(); ++i) {
|
||||
//if (i % 10 != 0) {continue;}
|
||||
|
||||
double prob = samples[i].weight;
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
|
||||
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
||||
pDistributation1.add(pos, prob);
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
}
|
||||
|
||||
void debugDistribution2(std::vector<SMC::Particle<MyState>> samples){
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
pDistributation2.clear();
|
||||
for (int i = 0; i < samples.size(); ++i) {
|
||||
if (i % 25 != 0) {continue;}
|
||||
|
||||
double prob = samples[i].weight;
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
|
||||
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
||||
pDistributation2.add(pos, prob);
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
}
|
||||
|
||||
void drawNormalN1(Distribution::NormalDistributionN normParticle){
|
||||
|
||||
pNormal1.clear();
|
||||
for (int i = 0; i < 100000; ++i) {
|
||||
if (++i % 25 != 0) {continue;}
|
||||
Eigen::VectorXd vec = normParticle.draw();
|
||||
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
||||
pNormal1.add(pos);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void drawNormalN2(Distribution::NormalDistributionN normParticle){
|
||||
|
||||
pNormal2.clear();
|
||||
for (int i = 0; i < 100000; ++i) {
|
||||
if (++i % 25 != 0) {continue;}
|
||||
Eigen::VectorXd vec = normParticle.draw();
|
||||
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
||||
pNormal2.add(pos);
|
||||
}
|
||||
}
|
||||
|
||||
void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) {
|
||||
|
||||
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
|
||||
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan);
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
const float step = 2.0f;
|
||||
for (float x = 0; x < 80; x += step) {
|
||||
for (float y = 0; y < 55; y += step) {
|
||||
Point3 pt(x,y,z);
|
||||
double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs);
|
||||
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
|
||||
}
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
|
||||
}
|
||||
|
||||
template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {
|
||||
|
||||
pColorPoints.clear();
|
||||
|
||||
// const float step = 2.0;
|
||||
// float z = 0;
|
||||
// for (float x = -20; x < 90; x += step) {
|
||||
// for (float y = -10; y < 60; y += step) {
|
||||
// const Point3 pos_m(x,y,z);
|
||||
// const double prob = func(obs, pos_m);
|
||||
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
|
||||
// }
|
||||
// }
|
||||
|
||||
std::minstd_rand gen;
|
||||
std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
for (int i = 0; i < 10000; ++i) {
|
||||
int idx = dist(gen);
|
||||
Node& n = grid[idx];
|
||||
const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
|
||||
const Point3 pos_m = pos_cm / 100.0f;
|
||||
const double prob = func(obs, pos_m);
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
|
||||
}
|
||||
|
||||
void addStairs(Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Floorplan::Floor* f : map->floors) {
|
||||
for (Floorplan::Stair* stair : f->stairs) {
|
||||
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
|
||||
for (const Floorplan::Quad3& quad : quads) {
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
int idx1 = i;
|
||||
int idx2 = (i+1) % 4;
|
||||
pStairs.addSegment(
|
||||
K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
|
||||
K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void addFloors(Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Floorplan::Floor* f : map->floors) {
|
||||
for (Floorplan::FloorObstacle* obs : f->obstacles) {
|
||||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
if (line) {
|
||||
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
|
||||
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
|
||||
pFloor.addSegment(p1, p2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void addOutline(Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Floorplan::Floor* f : map->floors) {
|
||||
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
|
||||
const int cnt = poly->poly.points.size();
|
||||
for (int i = 0; i < cnt; ++i) {
|
||||
Point2 p1 = poly->poly.points[(i+0)];
|
||||
Point2 p2 = poly->poly.points[(i+1)%cnt];
|
||||
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
|
||||
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
|
||||
pOutline.addSegment(gp1, gp2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template <typename Node> void addGrid(Grid<Node>& grid) {
|
||||
pGrid.clear();
|
||||
for (const Node& n : grid) {
|
||||
K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
|
||||
pGrid.add(p/100.0f);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename State> void addParticles(const std::vector<SMC::Particle<State>>& particles) {
|
||||
pParticles.clear();
|
||||
int i = 0;
|
||||
for (const SMC::Particle<State>& p : particles) {
|
||||
if (++i % 25 != 0) {continue;}
|
||||
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
||||
pParticles.add(pos / 100.0f);
|
||||
}
|
||||
}
|
||||
|
||||
void show() {
|
||||
gp.draw(splot);
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
void saveToFile(std::ofstream& stream){
|
||||
gp.draw(splot);
|
||||
stream << "set terminal x11 size 2000,1500\n";
|
||||
stream << gp.getBuffer();
|
||||
stream << "pause -1\n";
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
void printSingleFloor(const std::string& path, const int floorNum) {
|
||||
gp << "set terminal png size 1280,720\n";
|
||||
gp << "set output '" << path << "_" << floorNum <<".png'\n";
|
||||
gp << "set view 0,0\n";
|
||||
gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n";
|
||||
gp << "set autoscale xy\n";
|
||||
}
|
||||
|
||||
void printSideView(const std::string& path, const int degree) {
|
||||
gp << "set terminal png size 1280,720\n";
|
||||
gp << "set output '" << path << "_deg" << degree <<".png'\n";
|
||||
gp << "set view 90,"<< degree << "\n";
|
||||
gp << "set autoscale xy\n";
|
||||
gp << "set autoscale z\n";
|
||||
}
|
||||
|
||||
void printOverview(const std::string& path) {
|
||||
gp << "set terminal png size 1280,720\n";
|
||||
gp << "set output '" << path << "_overview" << ".png'\n";
|
||||
gp << "set view 75,60\n";
|
||||
gp << "set autoscale xy\n";
|
||||
gp << "set autoscale z\n";
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
712
code/Plotty.h
Normal file
712
code/Plotty.h
Normal file
@@ -0,0 +1,712 @@
|
||||
#pragma once
|
||||
|
||||
#include <Indoor/floorplan/v2/Floorplan.h>
|
||||
#include <Indoor/floorplan/v2/FloorplanHelper.h>
|
||||
#include <Indoor/geo/BBoxes3.h>
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPM3D.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
|
||||
|
||||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
|
||||
#include <KLib/misc/gnuplot/objects/GnuplotObjects.h>
|
||||
|
||||
struct Color {
|
||||
|
||||
uint8_t r;
|
||||
uint8_t g;
|
||||
uint8_t b;
|
||||
|
||||
Color() : r(0), g(0), b(0) {
|
||||
;
|
||||
}
|
||||
|
||||
static Color fromRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
|
||||
Color c; c.setRGB(r,g,b);
|
||||
return c;
|
||||
}
|
||||
|
||||
static Color fromHSV(const uint8_t h, const uint8_t s, const uint8_t v) {
|
||||
Color c; c.setHSV(h,s,v);
|
||||
return c;
|
||||
}
|
||||
|
||||
void setRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
|
||||
this->r = r;
|
||||
this->g = g;
|
||||
this->b = b;
|
||||
}
|
||||
|
||||
void setHSV(const uint8_t h, const uint8_t s, const uint8_t v) {
|
||||
|
||||
uint8_t region, remainder, p, q, t;
|
||||
|
||||
region = h / 43;
|
||||
remainder = (h - (region * 43)) * 6;
|
||||
|
||||
p = (v * (255 - s)) >> 8;
|
||||
q = (v * (255 - ((s * remainder) >> 8))) >> 8;
|
||||
t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
|
||||
|
||||
switch (region) {
|
||||
case 0:
|
||||
r = v; g = t; b = p;
|
||||
break;
|
||||
case 1:
|
||||
r = q; g = v; b = p;
|
||||
break;
|
||||
case 2:
|
||||
r = p; g = v; b = t;
|
||||
break;
|
||||
case 3:
|
||||
r = p; g = q; b = v;
|
||||
break;
|
||||
case 4:
|
||||
r = t; g = p; b = v;
|
||||
break;
|
||||
default:
|
||||
r = v; g = p; b = q;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
std::string toHEX() const {
|
||||
char buf[8];
|
||||
sprintf(buf, "#%02x%02x%02x", r, g, b);
|
||||
std::string color(buf);
|
||||
return color;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
class Plotty {
|
||||
|
||||
public:
|
||||
|
||||
const Floorplan::IndoorMap* map;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotSplot splot;
|
||||
K::GnuplotSplotElementPoints points;
|
||||
K::GnuplotSplotElementColorPoints cpoints;
|
||||
|
||||
K::GnuplotSplotElementLines pathReal;
|
||||
K::GnuplotSplotElementLines pathEst;
|
||||
K::GnuplotSplotElementColorPoints particles;
|
||||
|
||||
K::GnuplotSplotElementLines mapOutlineGlass;
|
||||
K::GnuplotSplotElementLines mapOutlineDrywall;
|
||||
K::GnuplotSplotElementLines mapOutlineConcrete;
|
||||
K::GnuplotSplotElementLines mapBBoxes;
|
||||
|
||||
K::GnuplotSplotElementEmpty emptyElem;
|
||||
|
||||
|
||||
K::GnuplotSplotElementPM3D pm3doutline;
|
||||
|
||||
std::string codeFile;
|
||||
|
||||
struct Settings {
|
||||
std::vector<int> floors = {};
|
||||
bool stairs = true;
|
||||
bool obstacles = true;
|
||||
bool outline = true;
|
||||
bool outlineColorCustom = false;
|
||||
bool skipI1 = false;
|
||||
K::GnuplotColor outlineColor = K::GnuplotColor::fromRGB(128,128,128);
|
||||
float minZ = -9999;
|
||||
float maxZ = +9999;
|
||||
} settings;
|
||||
|
||||
public:
|
||||
|
||||
Plotty(const Floorplan::IndoorMap* map) : map(map) {
|
||||
|
||||
//gp << "set view equal xy\n";
|
||||
|
||||
//gp << "set palette model RGB\n";
|
||||
//gp << "r(x) = (x < 0) ? 0 : (x/2)\n";
|
||||
//gp << "g(x) = 0\n";
|
||||
//gp << "b(x) = (x > 0) ? 0 : (-x/2)\n";
|
||||
//gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
|
||||
gp << "set ticslevel 0\n";
|
||||
|
||||
|
||||
// how to draw the floorplan
|
||||
mapOutlineConcrete.getStroke().getColor().setHexStr("#888888"); mapOutlineConcrete.getStroke().setWidth(2);
|
||||
mapOutlineDrywall.getStroke().getColor().setHexStr("#888888");
|
||||
mapOutlineGlass.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().setType(K::GnuplotDashtype::DASHED);
|
||||
mapBBoxes.getStroke().setWidth(2);
|
||||
|
||||
splot.add(&emptyElem);
|
||||
|
||||
splot.add(&mapOutlineConcrete);
|
||||
splot.add(&mapOutlineDrywall);
|
||||
splot.add(&mapOutlineGlass);
|
||||
splot.add(&mapBBoxes);
|
||||
|
||||
splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777");
|
||||
|
||||
splot.add(&pathReal); pathReal.getStroke().setWidth(2); pathReal.getStroke().getColor().setHexStr("#000000");
|
||||
splot.add(&pathEst); pathEst.getStroke().setWidth(2); pathEst.getStroke().getColor().setHexStr("#0000ff");
|
||||
|
||||
splot.add(&pm3doutline);
|
||||
|
||||
splot.add(&points);
|
||||
points.setPointType(7);
|
||||
points.setPointSize(0.5);
|
||||
|
||||
splot.add(&cpoints);
|
||||
cpoints.setPointSize(2);
|
||||
cpoints.setPointType(7);
|
||||
|
||||
splot.getView().setEnabled(true);
|
||||
|
||||
}
|
||||
|
||||
void addCircle(int id, const Point2& center, float radius)
|
||||
{
|
||||
auto c = K::GnuplotCoordinate2(center.x, center.y, K::GnuplotCoordinateSystem::FIRST);
|
||||
auto r = K::GnuplotCoordinate1(radius, K::GnuplotCoordinateSystem::FIRST);
|
||||
|
||||
K::GnuplotFill fill(K::GnuplotFillStyle::EMPTY, K::GnuplotColor::fromRGB(0, 0, 0));
|
||||
K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromRGB(255, 0, 0));
|
||||
|
||||
K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke);
|
||||
|
||||
splot.getObjects().set(id, obj);
|
||||
|
||||
}
|
||||
|
||||
void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {
|
||||
for (BBox3 bb : boxes.get()) {
|
||||
//&&addBBoxPoly(bb, c);
|
||||
//bb.shrink(0.98);
|
||||
addBBoxPoly2(bb, c);
|
||||
}
|
||||
}
|
||||
|
||||
void addBBoxPoly2(const BBox3& bb, const K::GnuplotColor& color) {
|
||||
|
||||
|
||||
K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::SOLID, color);
|
||||
K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 1, color);
|
||||
|
||||
K::GnuplotObjectPolygon* gpol1 = new K::GnuplotObjectPolygon(filler, stroke);
|
||||
gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol1->close();
|
||||
gpol1->setZIndex(bb.getMin().z - 0.1);
|
||||
splot.getObjects().add(gpol1);
|
||||
|
||||
K::GnuplotColor color2 = K::GnuplotColor::fromRGB(128,128,128);
|
||||
K::GnuplotStroke stroke2(K::GnuplotDashtype::NONE, 1, color2);
|
||||
K::GnuplotFill noFiller = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color2);
|
||||
|
||||
K::GnuplotObjectPolygon* gpol2 = new K::GnuplotObjectPolygon(noFiller, stroke2);
|
||||
gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol2->close();
|
||||
gpol2->setZIndex(bb.getMin().z + 3.1);
|
||||
splot.getObjects().add(gpol2);
|
||||
|
||||
K::GnuplotObjectPolygon* gpol3a = new K::GnuplotObjectPolygon(noFiller, stroke2);
|
||||
gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3a->setZIndex(bb.getMin().z + 1.1);
|
||||
splot.getObjects().add(gpol3a);
|
||||
K::GnuplotObjectPolygon* gpol3b = new K::GnuplotObjectPolygon(noFiller, stroke2);
|
||||
gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3b->setZIndex(bb.getMin().z + 1.1);
|
||||
splot.getObjects().add(gpol3b);
|
||||
K::GnuplotObjectPolygon* gpol3c = new K::GnuplotObjectPolygon(noFiller, stroke2);
|
||||
gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3c->setZIndex(bb.getMin().z + 1.1);
|
||||
splot.getObjects().add(gpol3c);
|
||||
K::GnuplotObjectPolygon* gpol3d = new K::GnuplotObjectPolygon(noFiller, stroke2);
|
||||
gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol3d->setZIndex(bb.getMin().z + 1.1);
|
||||
splot.getObjects().add(gpol3d);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void addBBox(const BBox3& bb) {
|
||||
|
||||
|
||||
// // floor
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
|
||||
// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMin().z});
|
||||
// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMin().z});
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMin().z});
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
|
||||
// mapBBoxes.splitFace(); mapBBoxes.splitFace();
|
||||
|
||||
// // ceil
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
|
||||
// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMax().z});
|
||||
// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMax().z});
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMax().z});
|
||||
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
|
||||
// mapBBoxes.splitFace(); mapBBoxes.splitFace();
|
||||
|
||||
// // up
|
||||
// mapBBoxes.addSegment({bb.getMin().x, bb.getMin().y, bb.getMin().z}, {bb.getMin().x, bb.getMin().y, bb.getMax().z});
|
||||
// mapBBoxes.addSegment({bb.getMax().x, bb.getMin().y, bb.getMin().z}, {bb.getMax().x, bb.getMin().y, bb.getMax().z});
|
||||
// mapBBoxes.addSegment({bb.getMin().x, bb.getMax().y, bb.getMin().z}, {bb.getMin().x, bb.getMax().y, bb.getMax().z});
|
||||
// mapBBoxes.addSegment({bb.getMax().x, bb.getMax().y, bb.getMin().z}, {bb.getMax().x, bb.getMax().y, bb.getMax().z});
|
||||
|
||||
}
|
||||
|
||||
void addBBoxPoly(const BBox3& bb, K::GnuplotColor c) {
|
||||
|
||||
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
|
||||
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->close();
|
||||
poly->setStroke(K::GnuplotStroke::NONE());
|
||||
poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, c));
|
||||
splot.getObjects().add(poly);
|
||||
|
||||
}
|
||||
|
||||
void setGroundTruth(const Point3 pos_m) {
|
||||
gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
|
||||
}
|
||||
void setCurEst(const Point3 pos_m) {
|
||||
gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
|
||||
}
|
||||
|
||||
|
||||
void setPaletteRedBlue() {
|
||||
|
||||
float max = -9999;
|
||||
float min = +9999;
|
||||
for (const auto& e : cpoints.get()) {
|
||||
if (e.color > max) {max = e.color;}
|
||||
if (e.color < min) {min = e.color;}
|
||||
}
|
||||
setPaletteRedBlue(min, max);
|
||||
|
||||
}
|
||||
|
||||
void setPaletteRedBlue(const float blueVal, const float redVal) {
|
||||
|
||||
// we need to map the range from [blueVal:redVal] to [0:1]
|
||||
const float min = blueVal;
|
||||
const float max = redVal;
|
||||
const float range = (max - min);
|
||||
const float center01 = (0-min)/range;
|
||||
|
||||
// values above 0 dB = red
|
||||
// values below 0 dB = blue
|
||||
gp << "set palette model RGB\n";
|
||||
gp << "cen01 = " << center01 << "\n";
|
||||
gp << "r(x) = (x < cen01) ? 0 : ((x-cen01) / (1-cen01))\n";
|
||||
gp << "g(x) = 0\n";
|
||||
gp << "b(x) = (x > cen01) ? 0 : (1 - (x/cen01))\n";
|
||||
gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
|
||||
}
|
||||
|
||||
void addLabel(const std::string& txt, const Point3 pos) {
|
||||
//gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n";
|
||||
splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
|
||||
}
|
||||
|
||||
void setActivity(const int act) {
|
||||
|
||||
std::string activity = "Unkown";
|
||||
if(act == 0){
|
||||
activity = "Standing";
|
||||
} else if(act == 1) {
|
||||
activity = "Walking";
|
||||
} else if(act == 2) {
|
||||
activity = "Up";
|
||||
} else if(act == 3) {
|
||||
activity = "Down";
|
||||
}
|
||||
|
||||
gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\n";
|
||||
}
|
||||
|
||||
void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) {
|
||||
std::vector<Point3> points = {
|
||||
Point3(p1.x, p1.y, p1.z),
|
||||
Point3(p2.x, p1.y, p1.z),
|
||||
Point3(p2.x, p2.y, p1.z),
|
||||
Point3(p1.x, p2.y, p1.z),
|
||||
Point3(p1.x, p1.y, p1.z),
|
||||
};
|
||||
addPolygon(points, c.toHEX(), front, fill);
|
||||
}
|
||||
|
||||
void addRectangleW(const Point3 p1, const Point3 p2, const K::GnuplotColor c, const float w, bool front = false) {
|
||||
std::vector<Point3> points = {
|
||||
Point3(p1.x, p1.y, p1.z),
|
||||
Point3(p2.x, p1.y, p1.z),
|
||||
Point3(p2.x, p2.y, p1.z),
|
||||
Point3(p1.x, p2.y, p1.z),
|
||||
Point3(p1.x, p1.y, p1.z),
|
||||
};
|
||||
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
|
||||
poly->getStroke().setWidth(w);
|
||||
poly->getStroke().setColor(c);
|
||||
poly->setFront(front);
|
||||
for (const Point3 p : points) {
|
||||
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
}
|
||||
splot.getObjects().add(poly);
|
||||
}
|
||||
|
||||
K::GnuplotObjectPolygon* addStartIndicator(const Point3 pt, const std::string& color, const float s = 2) {
|
||||
|
||||
// for (const Point3 p : points) {
|
||||
// if (p.z < settings.minZ) {return nullptr;}
|
||||
// if (p.z > settings.maxZ) {return nullptr;}
|
||||
// }
|
||||
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
|
||||
poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color)));
|
||||
poly->setStroke(K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromRGB(0,0,0)));
|
||||
//poly->setStroke(K::GnuplotStroke::NONE());
|
||||
poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->close();
|
||||
poly->setFront(true);
|
||||
splot.getObjects().add(poly);
|
||||
|
||||
return poly;
|
||||
|
||||
}
|
||||
|
||||
K::GnuplotObjectPolygon* addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true, const float alpha = 1) {
|
||||
|
||||
for (const Point3 p : points) {
|
||||
if (p.z < settings.minZ) {return nullptr;}
|
||||
if (p.z > settings.maxZ) {return nullptr;}
|
||||
}
|
||||
|
||||
const K::GnuplotFill pfill = (fill) ? (K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotFill::NONE());
|
||||
const K::GnuplotStroke pstroke = (!fill) ? (K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1.0, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotStroke::NONE());
|
||||
|
||||
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(pfill, pstroke);
|
||||
for (const Point3 p : points) {
|
||||
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
poly->setZIndex(p.z); // manual depth ordering
|
||||
poly->getFill().setAlpha(alpha);
|
||||
}
|
||||
poly->setFront(front);
|
||||
|
||||
splot.getObjects().add(poly);
|
||||
|
||||
|
||||
// gp << "set object polygon from ";
|
||||
// for (size_t i = 0; i < points.size(); ++i) {
|
||||
// const Point3 p = points[i];
|
||||
// if (i > 0) {gp << " to ";}
|
||||
// gp << p.x << "," << p.y << "," << p.z << " ";
|
||||
// }
|
||||
// gp << (front ? "front" : "");
|
||||
// if (fill) {gp << " fs solid ";} else {gp << " fs transparent ";}
|
||||
// gp << " fc rgb " << "'" << color << "'";
|
||||
// gp << "\n";
|
||||
|
||||
return poly;
|
||||
|
||||
}
|
||||
|
||||
void setZRange(const float min, const float max) {
|
||||
gp << "set zrange [" << min << ":" << max << "]\n";
|
||||
}
|
||||
|
||||
|
||||
K::GnuplotObjectPolygon* addFloorRect(const Point3 pos_m, const float size, Color c, float ratio = 1.0) {
|
||||
|
||||
const Point3 p1 = pos_m + Point3(-size, -size/ratio, 0);
|
||||
const Point3 p2 = pos_m + Point3(+size, -size/ratio, 0);
|
||||
const Point3 p3 = pos_m + Point3(+size, +size/ratio, 0);
|
||||
const Point3 p4 = pos_m + Point3(-size, +size/ratio, 0);
|
||||
|
||||
std::vector<Point3> points = {p1,p2,p3,p4,p1};
|
||||
|
||||
return addPolygon(points, c.toHEX(), false, true);
|
||||
|
||||
// gp << "set object polygon from ";
|
||||
// for (size_t i = 0; i < points.size(); ++i) {
|
||||
// const Point3 p = points[i];
|
||||
// if (i > 0) {gp << " to ";}
|
||||
// gp << p.x << "," << p.y << "," << p.z << " ";
|
||||
// }
|
||||
// gp << "front fs solid fc rgb " << "'" << c.toHEX() << "'";
|
||||
// gp << "\n";
|
||||
|
||||
}
|
||||
|
||||
template <typename T> void showParticles(const std::vector<T>& particles) {
|
||||
this->particles.clear();
|
||||
double min = +999;
|
||||
double max = -999;
|
||||
for (const T& p : particles) {
|
||||
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
|
||||
const double prob = std::pow(p.weight, 0.25);
|
||||
this->particles.add(p3, prob);
|
||||
if (prob > max) {max = prob;}
|
||||
if (prob < min) {min = prob;}
|
||||
}
|
||||
splot.getAxisCB().setRange(min, max + 0.000001);
|
||||
}
|
||||
|
||||
// estimated path
|
||||
void addEstimationNode(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
pathEst.add(est);
|
||||
}
|
||||
|
||||
|
||||
void setTitle(const std::string& title) {
|
||||
gp << "set title '" << title << "'\n";
|
||||
}
|
||||
|
||||
void setGroundTruth(const std::vector<int> indices) {
|
||||
const std::vector<Point3> path = FloorplanHelper::getGroundTruth(map, indices);
|
||||
pathReal.clear();
|
||||
for (const Point3& p : path) {
|
||||
pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z));
|
||||
}
|
||||
}
|
||||
|
||||
void equalXY() {
|
||||
gp << "set view equal xy\n";
|
||||
}
|
||||
|
||||
void setView(const float degX, const float degY) {
|
||||
//gp << "set view " << degX << "," << degY << "\n";
|
||||
splot.getView().setCamera(degX, degY);
|
||||
}
|
||||
|
||||
void setScale(const float x, const float y, const float ox = 0, const float oy = 0) {
|
||||
gp << "set multiplot layout 1,1 scale " << x << "," << y << " offset " << ox << "," << oy << "\n";
|
||||
}
|
||||
|
||||
void writeCodeTo(const std::string& file) {
|
||||
this->codeFile = file;
|
||||
}
|
||||
|
||||
void noFrame() {
|
||||
gp << "unset border\n";
|
||||
// gp << "unset xtics\n";
|
||||
// gp << "unset ytics\n";
|
||||
// gp << "unset ztics\n";
|
||||
splot.getAxisX().setTicsVisible(false);
|
||||
splot.getAxisY().setTicsVisible(false);
|
||||
splot.getAxisZ().setTicsVisible(false);
|
||||
}
|
||||
|
||||
void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
|
||||
gp.setTerminal("epslatex", size);
|
||||
gp.setOutput(file);
|
||||
}
|
||||
|
||||
void plot() {
|
||||
|
||||
this->mapOutlineConcrete.getStroke().setColor(settings.outlineColor);
|
||||
this->mapOutlineDrywall.getStroke().setColor(settings.outlineColor);
|
||||
this->mapOutlineGlass.getStroke().setColor(settings.outlineColor);
|
||||
|
||||
gp.draw(splot);
|
||||
gp << "unset multiplot\n"; // scaling
|
||||
if (codeFile != "") {
|
||||
std::ofstream out(codeFile);
|
||||
out << gp.getBuffer();
|
||||
out.close();
|
||||
}
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
void closeStream(){
|
||||
gp.close();
|
||||
}
|
||||
|
||||
void saveToFile(std::ofstream& stream){
|
||||
gp.draw(splot);
|
||||
stream << "set terminal x11 size 2000,1500\n";
|
||||
stream << gp.getBuffer();
|
||||
stream << "pause -1\n";
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
void printOverview(const std::string& path) {
|
||||
gp << "set terminal png size 2000,1500\n";
|
||||
gp << "set output '" << path << "_overview" << ".png'\n";
|
||||
gp << "set view 75,60\n";
|
||||
gp << "set autoscale xy\n";
|
||||
gp << "set autoscale z\n";
|
||||
}
|
||||
|
||||
void buildFloorplan() {
|
||||
|
||||
std::vector<Floorplan::Floor*> floors;
|
||||
|
||||
BBox3 bbox = FloorplanHelper::getBBox(map);
|
||||
|
||||
// only some floors??
|
||||
if (settings.floors.empty()) {
|
||||
floors = map->floors;
|
||||
} else {
|
||||
for (int i : settings.floors) {
|
||||
floors.push_back(map->floors[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// mapOutlineDrywall.addSegment(
|
||||
// K::GnuplotPoint3(bbox.getMin().x, bbox.getMin().y, bbox.getMin().z),
|
||||
// K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z)
|
||||
// );
|
||||
|
||||
splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x));
|
||||
splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y));
|
||||
splot.getAxisZ().setRange(K::GnuplotAxis::Range(0, 11));
|
||||
|
||||
// process each selected floor
|
||||
for (Floorplan::Floor* floor : floors) {
|
||||
|
||||
const float vo = floor->atHeight * 4.5;
|
||||
|
||||
// plot the floor's outline
|
||||
if (settings.outline) {
|
||||
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||||
|
||||
if (floor->atHeight < settings.minZ) {continue;}
|
||||
if (floor->atHeight > settings.maxZ) {continue;}
|
||||
|
||||
// for toni
|
||||
if (settings.skipI1) {
|
||||
if (floor->atHeight == 4) {
|
||||
//if (poly->poly.points[2].y < 0) {
|
||||
if (poly->poly.points[0].x > 70 && poly->poly.points[0].y < 50) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const float v = 180 + vo;
|
||||
|
||||
K::GnuplotColor color = K::GnuplotColor::fromRGB(v,v,v);
|
||||
if (poly->outdoor) {color = K::GnuplotColor::fromRGB(180, 240, 180);}
|
||||
if (poly->method == Floorplan::OutlineMethod::REMOVE) {color = K::GnuplotColor::fromRGB(245,245,245);}
|
||||
K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color);
|
||||
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, K::GnuplotStroke::NONE());
|
||||
for (Point2 pt : poly->poly.points) {
|
||||
K::GnuplotCoordinate3 coord(pt.x, pt.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST);
|
||||
gpol->add(coord);
|
||||
}
|
||||
gpol->close();
|
||||
gpol->setZIndex(floor->atHeight-0.1); // below the lines
|
||||
//gpol->setFront(true);
|
||||
splot.getObjects().add(gpol);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// plot obstacles?
|
||||
if (settings.obstacles) {
|
||||
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
||||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
if (line) {
|
||||
|
||||
if (floor->atHeight < settings.minZ) {continue;}
|
||||
if (floor->atHeight > settings.maxZ) {continue;}
|
||||
|
||||
// const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
|
||||
// const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
|
||||
// switch(line->material) {
|
||||
// case Floorplan::Material::CONCRETE: mapOutlineConcrete.addSegment(p1, p2); break;
|
||||
// case Floorplan::Material::GLASS: mapOutlineGlass.addSegment(p1, p2); break;
|
||||
// case Floorplan::Material::UNKNOWN:
|
||||
// case Floorplan::Material::DRYWALL: mapOutlineDrywall.addSegment(p1, p2); break;
|
||||
// }
|
||||
|
||||
// K::GnuplotObjectArrow* arrow = new K::GnuplotObjectArrow(
|
||||
// K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST),
|
||||
// K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST)
|
||||
// );
|
||||
// arrow->setHead(K::GnuplotObjectArrow::Head::NONE);
|
||||
// splot.getObjects().add(arrow);
|
||||
|
||||
const float v = 140 + vo;
|
||||
|
||||
// drawing outlines as polygon is a hack for correct depth-order in gnuplot
|
||||
K::GnuplotColor color = (settings.outlineColorCustom) ? (settings.outlineColor) : (K::GnuplotColor::fromRGB(v,v,v));
|
||||
K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color);
|
||||
K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 6, color);
|
||||
//K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), stroke);
|
||||
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke);
|
||||
//K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), K::GnuplotStroke::NONE());
|
||||
|
||||
gpol->add(K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->add(K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->close();
|
||||
gpol->setZIndex(floor->atHeight); // above the ground polygon
|
||||
//gpol->setFront(true);
|
||||
splot.getObjects().add(gpol);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// plot the stairs as polygon
|
||||
if (settings.stairs) {
|
||||
for (Floorplan::Stair* s : floor->stairs) {
|
||||
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), floor);
|
||||
for (const Floorplan::Quad3& q : quads) {
|
||||
// K::GnuplotObjectPolygon* poly = addPolygon({q.p1, q.p2, q.p3, q.p4, q.p1}, "#c0c0c0");
|
||||
// if (poly) {
|
||||
// poly->setZIndex(floor->atHeight+1.5); // above the floor
|
||||
// }
|
||||
|
||||
const float v1 = 180 + q.p1.z * 4.5;
|
||||
const float v2 = 140 + q.p1.z * 4.5;
|
||||
|
||||
const float z = (q.p1.z + q.p2.z + q.p3.z + q.p4.z) / 4.0f;
|
||||
|
||||
if (z < settings.minZ) {continue;}
|
||||
if (z > settings.maxZ) {continue;}
|
||||
|
||||
K::GnuplotColor color = K::GnuplotColor::fromRGB(v1,v1,v1);
|
||||
K::GnuplotColor color2 = K::GnuplotColor::fromRGB(v2,v2,v2);
|
||||
K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color);
|
||||
K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, color2);
|
||||
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke);
|
||||
gpol->add(K::GnuplotCoordinate3(q.p1.x, q.p1.y, q.p1.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->add(K::GnuplotCoordinate3(q.p2.x, q.p2.y, q.p2.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->add(K::GnuplotCoordinate3(q.p3.x, q.p3.y, q.p3.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->add(K::GnuplotCoordinate3(q.p4.x, q.p4.y, q.p4.z, K::GnuplotCoordinateSystem::FIRST));
|
||||
gpol->close();
|
||||
gpol->setZIndex(z); // above the ground
|
||||
splot.getObjects().add(gpol);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
142
code/Settings.h
Normal file
142
code/Settings.h
Normal file
@@ -0,0 +1,142 @@
|
||||
#pragma once
|
||||
|
||||
#include <Indoor/grid/GridPoint.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
#include <Indoor/sensors/radio/VAPGrouper.h>
|
||||
|
||||
namespace Settings {
|
||||
|
||||
const bool useKLB = false;
|
||||
|
||||
const int numParticles = 5000;
|
||||
const int numBSParticles = 50;
|
||||
|
||||
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");
|
||||
|
||||
struct NUCSettings
|
||||
{
|
||||
int ID;
|
||||
float ftm_offset;
|
||||
float rssi_pathloss;
|
||||
};
|
||||
|
||||
const std::unordered_map<MACAddress, NUCSettings> NUCS = {
|
||||
{ NUC1, { 1, 1.25, 3.375 }},
|
||||
{ NUC2, { 2, 2.00, 3.000 }},
|
||||
{ NUC3, { 3, 1.75, 3.375 }},
|
||||
{ NUC4, { 4, 2.75, 2.750 }}
|
||||
};
|
||||
|
||||
namespace IMU {
|
||||
const float turnSigma = 2.5; // 3.5
|
||||
const float stepLength = 1.00;
|
||||
const float stepSigma = 0.15; //toni changed
|
||||
}
|
||||
|
||||
const float smartphoneAboveGround = 1.3;
|
||||
|
||||
const float offlineSensorSpeedup = 2;
|
||||
|
||||
namespace Grid {
|
||||
constexpr int gridSize_cm = 20;
|
||||
}
|
||||
|
||||
namespace Smoothing {
|
||||
const bool activated = true;
|
||||
const double stepLength = 0.7;
|
||||
const double stepSigma = 0.2;
|
||||
const double headingSigma = 25.0;
|
||||
const double zChange = 0.0; // mu change in height between two time steps
|
||||
const double zSigma = 0.1;
|
||||
const int lag = 5;
|
||||
|
||||
}
|
||||
|
||||
namespace KDE {
|
||||
const Point2 bandwidth(1,1);
|
||||
const float gridSize = 0.2;
|
||||
}
|
||||
|
||||
namespace KDE3D {
|
||||
const Point3 bandwidth(1, 1, 1);
|
||||
const Point3 gridSize(0.2, 0.2, 1); // in meter
|
||||
}
|
||||
|
||||
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
|
||||
const GridPoint destination = GridPoint(0,0,0); // do not use destination
|
||||
|
||||
namespace SensorDebug {
|
||||
const Timestamp updateEvery = Timestamp::fromMS(200);
|
||||
}
|
||||
|
||||
namespace WiFiModel {
|
||||
constexpr float sigma = 8.0;
|
||||
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
|
||||
constexpr float TXP = -45;
|
||||
constexpr float EXP = 2.3;
|
||||
constexpr float WAF = -11.0;
|
||||
|
||||
const bool optimize = false;
|
||||
const bool useRegionalOpt = false;
|
||||
|
||||
// how to perform VAP grouping. see
|
||||
// - calibration in Controller.cpp
|
||||
// - eval in Filter.h
|
||||
// NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
|
||||
const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM
|
||||
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM
|
||||
}
|
||||
|
||||
namespace BeaconModel {
|
||||
constexpr float sigma = 8.0;
|
||||
constexpr float TXP = -71;
|
||||
constexpr float EXP = 1.5;
|
||||
constexpr float WAF = -20.0; //-5 //20??
|
||||
}
|
||||
|
||||
namespace MapView3D {
|
||||
const int maxColorPoints = 1000;
|
||||
constexpr int fps = 15;
|
||||
const Timestamp msPerFrame = Timestamp::fromMS(1000/fps);
|
||||
}
|
||||
|
||||
namespace Filter {
|
||||
const Timestamp updateEvery = Timestamp::fromMS(500);
|
||||
constexpr bool useMainThread = false; // perform filtering in the main thread
|
||||
}
|
||||
|
||||
const std::string mapDir = "../map/";
|
||||
const std::string dataDir = "../measurements/data/";
|
||||
const std::string errorDir = "../measurements/error/";
|
||||
|
||||
/** describes one dataset (map, training, parameter-estimation, ...) */
|
||||
struct DataSetup {
|
||||
std::string map;
|
||||
std::vector<std::string> training;
|
||||
std::unordered_map<MACAddress, Point3> APs;
|
||||
int numGTPoints;
|
||||
};
|
||||
|
||||
/** all configured datasets */
|
||||
const struct Data {
|
||||
|
||||
const DataSetup Path0 = {
|
||||
mapDir + "map0_ap_path0.xml",
|
||||
{
|
||||
dataDir + "Pixel2/Path0_0605.csv",
|
||||
},
|
||||
{
|
||||
{ NUC1, { 7.5, 18.7, 0.8} }, // NUC 1
|
||||
{ NUC2, { 8.6, 26.8, 0.8} }, // NUC 2
|
||||
{ NUC3, {21.6, 19.1, 0.8} }, // NUC 3
|
||||
{ NUC4, {20.8, 27.1, 0.8} }, // NUC 4
|
||||
},
|
||||
4
|
||||
};
|
||||
} data;
|
||||
|
||||
}
|
||||
|
||||
349
code/filter.h
Normal file
349
code/filter.h
Normal file
@@ -0,0 +1,349 @@
|
||||
#pragma once
|
||||
|
||||
#include "mesh.h"
|
||||
#include "Settings.h"
|
||||
#include <omp.h>
|
||||
|
||||
#include <Indoor/geo/Heading.h>
|
||||
#include <Indoor/math/distribution/Uniform.h>
|
||||
#include <Indoor/math/distribution/Normal.h>
|
||||
//#include <Indoor/math/distribution/Region.h>
|
||||
|
||||
#include <Indoor/smc/Particle.h>
|
||||
#include <Indoor/smc/filtering/ParticleFilter.h>
|
||||
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
|
||||
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
|
||||
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
|
||||
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
|
||||
|
||||
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
|
||||
//#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
|
||||
//#include <Indoor/navMesh/NavMeshRandom.h>
|
||||
|
||||
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
|
||||
#include <Indoor/sensors/radio/WiFiMeasurements.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
||||
#include <Indoor/sensors/activity/ActivityDetector.h>
|
||||
|
||||
#include "FtmKalman.h"
|
||||
|
||||
struct MyState {
|
||||
|
||||
/** the state's position (within the mesh) */
|
||||
MyNavMeshLocation pos;
|
||||
|
||||
/** the state's heading */
|
||||
Heading heading;
|
||||
|
||||
MyState() : pos(), heading(0) {;}
|
||||
|
||||
MyState(Point3 p) : pos(p, nullptr), heading(0){;}
|
||||
|
||||
MyState& operator += (const MyState& o) {
|
||||
pos.tria = nullptr; // impossible
|
||||
pos.pos += o.pos.pos;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState& operator /= (const double val) {
|
||||
pos.tria = nullptr; // impossible
|
||||
pos.pos /= val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState operator * (const double val) const {
|
||||
MyState res;
|
||||
res.pos.pos = pos.pos * val;
|
||||
return res;
|
||||
}
|
||||
|
||||
float getX(){
|
||||
return pos.pos.x;
|
||||
}
|
||||
|
||||
float getY() {
|
||||
return pos.pos.y;
|
||||
}
|
||||
|
||||
float getZ() {
|
||||
return pos.pos.z;
|
||||
}
|
||||
|
||||
|
||||
float getBinValue(const int dim) const {
|
||||
switch (dim) {
|
||||
case 0: return this->pos.pos.x;
|
||||
case 1: return this->pos.pos.y;
|
||||
case 2: return this->pos.pos.z;
|
||||
case 3: return this->heading.getRAD();
|
||||
}
|
||||
throw "cant find this value within the bin";
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
struct MyControl {
|
||||
|
||||
int numStepsSinceLastEval = 0;
|
||||
float headingChangeSinceLastEval = 0;
|
||||
|
||||
void afterEval() {
|
||||
numStepsSinceLastEval = 0;
|
||||
headingChangeSinceLastEval = 0;
|
||||
}
|
||||
|
||||
//wifi
|
||||
std::map<MACAddress, WiFiMeasurement> wifi;
|
||||
|
||||
//time
|
||||
Timestamp currentTime;
|
||||
|
||||
//last estimation
|
||||
Point3 lastEstimate = Point3(26, 43, 7.5);
|
||||
|
||||
};
|
||||
|
||||
struct MyObservation {
|
||||
|
||||
// pressure
|
||||
float sigmaPressure = 0.10f;
|
||||
float relativePressure = 0;
|
||||
|
||||
//wifi
|
||||
std::unordered_map<MACAddress, WiFiMeasurement> wifi;
|
||||
|
||||
//time
|
||||
Timestamp currentTime;
|
||||
|
||||
//activity
|
||||
Activity activity;
|
||||
|
||||
};
|
||||
|
||||
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
|
||||
|
||||
const MyNavMesh* mesh;
|
||||
|
||||
public:
|
||||
|
||||
MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
|
||||
;
|
||||
}
|
||||
|
||||
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
||||
|
||||
/** random position and heading within the mesh */
|
||||
Distribution::Uniform<float> dHead(0, 2*M_PI);
|
||||
MyNavMeshRandom rnd = mesh->getRandom();
|
||||
for (SMC::Particle<MyState>& p : particles) {
|
||||
p.state.pos = rnd.draw();
|
||||
p.state.heading = dHead.draw();
|
||||
p.weight = 1.0 / particles.size();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
|
||||
|
||||
const MyNavMesh* mesh;
|
||||
const Point3 pos;
|
||||
|
||||
public:
|
||||
|
||||
MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
|
||||
;
|
||||
}
|
||||
|
||||
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
||||
|
||||
/** random position and heading within the mesh */
|
||||
Distribution::Uniform<float> dHead(0, 2*M_PI);
|
||||
for (SMC::Particle<MyState>& p : particles) {
|
||||
p.state.pos = mesh->getLocation(pos);
|
||||
p.state.heading = 1.5*M_PI;// dHead.draw();
|
||||
p.weight = 1.0 / particles.size();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>{
|
||||
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
// nop
|
||||
}
|
||||
};
|
||||
|
||||
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
|
||||
|
||||
MyNavMeshWalk walker;
|
||||
|
||||
const double lambda = 0.03;
|
||||
|
||||
public:
|
||||
|
||||
//std::vector<double> listRadiusSub;
|
||||
|
||||
MyPFTrans(MyNavMesh& mesh) :
|
||||
walker(mesh) {
|
||||
|
||||
// how to evaluate drawn points
|
||||
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
|
||||
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
|
||||
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
|
||||
}
|
||||
|
||||
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
// walking and heading random
|
||||
Distribution::Normal<float> dStepSizeFloor(0.60, 0.1);
|
||||
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
|
||||
Distribution::Normal<float> dHeading(0.0, 0.1);
|
||||
|
||||
#pragma omp parallel for num_threads(3)
|
||||
for (int i = 0; i < particles.size(); ++i) {
|
||||
SMC::Particle<MyState>& p = particles[i];
|
||||
|
||||
// how to walk
|
||||
MyNavMeshWalkParams params;
|
||||
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
|
||||
params.numSteps = control->numStepsSinceLastEval;
|
||||
params.start = p.state.pos;
|
||||
|
||||
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
|
||||
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
|
||||
|
||||
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
|
||||
params.stepSizes.stepSizeFloor_m = 0.1;
|
||||
params.stepSizes.stepSizeStair_m = 0.1;
|
||||
}
|
||||
|
||||
double deltaUnblockable = 0.01;
|
||||
|
||||
// walk
|
||||
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
|
||||
//MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
|
||||
|
||||
// assign back to particle's state
|
||||
p.weight *= res.probability;
|
||||
p.state.pos = res.location;
|
||||
p.state.heading = res.heading;
|
||||
|
||||
}
|
||||
|
||||
// reset the control (0 steps, 0 delta-heading)
|
||||
//control->afterEval();
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
||||
|
||||
//TODO: add this to transition probability
|
||||
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
||||
|
||||
const float kappa = 0.75;
|
||||
|
||||
switch (act) {
|
||||
|
||||
case Activity::WALKING:
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
|
||||
case Activity::WALKING_UP:
|
||||
case Activity::WALKING_DOWN:
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
}
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
// FRANK
|
||||
MyPFEval() { };
|
||||
|
||||
bool assignProps = false;
|
||||
|
||||
std::shared_ptr<std::unordered_map<MACAddress, Kalman>> kalmanMap;
|
||||
|
||||
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;
|
||||
|
||||
if (observation.wifi.size() == 0)
|
||||
{
|
||||
printf("");
|
||||
}
|
||||
|
||||
for (auto& wifi : observation.wifi) {
|
||||
|
||||
if ( (true && wifi.second.getAP().getMAC() == Settings::NUC1)
|
||||
|| (true && wifi.second.getAP().getMAC() == Settings::NUC2)
|
||||
|| (true && wifi.second.getAP().getMAC() == Settings::NUC3)
|
||||
|| (true && wifi.second.getAP().getMAC() == Settings::NUC4)
|
||||
)
|
||||
{
|
||||
float rssi_pathloss = Settings::NUCS.at(wifi.second.getAP().getMAC()).rssi_pathloss;
|
||||
|
||||
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.second.getRSSI());
|
||||
float ftmDist = wifi.second.getFtmDist();
|
||||
|
||||
Point3 apPos = Settings::data.Path0.APs.find(wifi.first)->second;
|
||||
Point3 particlePos = p.state.pos.pos;
|
||||
particlePos.z = 1.3; // smartphone h<>he
|
||||
float apDist = particlePos.getDistance(apPos);
|
||||
|
||||
auto kalman = kalmanMap->at(wifi.second.getAP().getMAC());
|
||||
|
||||
pFtm *= Distribution::Normal<float>::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist);
|
||||
//pFtm *= Distribution::Normal<float>::getProbability(apDist, 3.5, ftmDist);
|
||||
//pFtm *= Distribution::Region<float>::getProbability(apDist, 3.5/2, ftmDist);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
double prob = pFtm;
|
||||
|
||||
if (assignProps)
|
||||
p.weight = prob; // p.weight *= prob
|
||||
else
|
||||
p.weight *= prob;
|
||||
|
||||
#pragma omp atomic
|
||||
sum += prob;
|
||||
}
|
||||
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
|
||||
|
||||
326
code/main.cpp
Normal file
326
code/main.cpp
Normal file
@@ -0,0 +1,326 @@
|
||||
//#include "main.h"
|
||||
|
||||
#include "mesh.h"
|
||||
#include "filter.h"
|
||||
#include "Settings.h"
|
||||
#include "meshPlotter.h"
|
||||
#include "Plotty.h"
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <filesystem>
|
||||
#include <chrono>
|
||||
|
||||
#include <Indoor/floorplan/v2/FloorplanReader.h>
|
||||
#include <Indoor/sensors/offline/FileReader.h>
|
||||
#include <Indoor/geo/Heading.h>
|
||||
#include <Indoor/geo/Point2.h>
|
||||
#include <Indoor/sensors/imu/TurnDetection.h>
|
||||
#include <Indoor/sensors/imu/StepDetection.h>
|
||||
#include <Indoor/sensors/imu/PoseDetection.h>
|
||||
#include <Indoor/sensors/imu/MotionDetection.h>
|
||||
#include <Indoor/sensors/pressure/RelativePressure.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
|
||||
|
||||
#include <Indoor/math/stats/Statistics.h>
|
||||
|
||||
#include "FtmKalman.h"
|
||||
#include "mainFtm.h"
|
||||
|
||||
#include <sys/stat.h>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
|
||||
|
||||
// reading file
|
||||
std::string currDir = std::filesystem::current_path().string();
|
||||
|
||||
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
|
||||
Offline::FileReader fr(setup.training[numFile]);
|
||||
|
||||
// ground truth
|
||||
std::vector<int> gtPath;
|
||||
for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
|
||||
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
|
||||
Stats::Statistics<float> errorStats;
|
||||
|
||||
//calculate distance of path
|
||||
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
|
||||
double distance = 0;
|
||||
for(int i = 1; i < gtEntries.size(); ++i){
|
||||
distance += gtEntries[i].value.getDistance(gtEntries[i-1].value);
|
||||
}
|
||||
|
||||
std::cout << "Distance of Path: " << distance << std::endl;
|
||||
|
||||
// error file
|
||||
const long int t = static_cast<long int>(time(NULL));
|
||||
auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
|
||||
evalDir.append(folder);
|
||||
|
||||
if (!std::experimental::filesystem::exists(evalDir)) {
|
||||
std::experimental::filesystem::create_directory(evalDir);
|
||||
}
|
||||
|
||||
std::ofstream errorFile;
|
||||
errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
|
||||
|
||||
|
||||
// wifi
|
||||
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
|
||||
kalmanMap->insert({ Settings::NUC1, Kalman(1, 6.156) });
|
||||
kalmanMap->insert({ Settings::NUC2, Kalman(2, 5.650) });
|
||||
kalmanMap->insert({ Settings::NUC3, Kalman(3, 6.107) });
|
||||
kalmanMap->insert({ Settings::NUC4, Kalman(4, 3.985) });
|
||||
|
||||
|
||||
// mesh
|
||||
NM::NavMeshSettings set;
|
||||
MyNavMesh mesh;
|
||||
MyNavMeshFactory fac(&mesh, set);
|
||||
fac.build(map);
|
||||
|
||||
const Point3 srcPath0(9.8, 24.9, 0); // fixed start pos
|
||||
|
||||
// add shortest-path to destination
|
||||
//const Point3 dst(51, 45, 1.7);
|
||||
//const Point3 dst(25, 45, 0);
|
||||
//NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
|
||||
|
||||
// debug show
|
||||
//MeshPlotter dbg;
|
||||
//dbg.addFloors(map);
|
||||
//dbg.addOutline(map);
|
||||
//dbg.addMesh(mesh);
|
||||
////dbg.addDijkstra(mesh);
|
||||
//dbg.draw();
|
||||
|
||||
Plotty plot(map);
|
||||
plot.buildFloorplan();
|
||||
plot.setGroundTruth(gtPath);
|
||||
plot.plot();
|
||||
|
||||
// particle-filter
|
||||
const int numParticles = 5000;
|
||||
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath0); // known position
|
||||
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
|
||||
auto eval = std::make_unique<MyPFEval>();
|
||||
eval->kalmanMap = kalmanMap;
|
||||
|
||||
auto trans = std::make_unique<MyPFTrans>(mesh);
|
||||
//auto trans = std::make_unique<MyPFTransStatic>();
|
||||
|
||||
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
|
||||
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
|
||||
|
||||
// setup
|
||||
MyFilter pf(numParticles, std::move(init));
|
||||
pf.setEvaluation(std::move(eval));
|
||||
pf.setTransition(std::move(trans));
|
||||
pf.setResampling(std::move(resample));
|
||||
pf.setEstimation(std::move(estimate));
|
||||
pf.setNEffThreshold(0.85);
|
||||
|
||||
// sensors
|
||||
MyControl ctrl;
|
||||
MyObservation obs;
|
||||
|
||||
StepDetection sd;
|
||||
PoseDetection pd;
|
||||
TurnDetection td(&pd);
|
||||
RelativePressure relBaro;
|
||||
ActivityDetector act;
|
||||
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
|
||||
Timestamp lastTimestamp = Timestamp::fromMS(0);
|
||||
|
||||
int i = 0;
|
||||
// parse each sensor-value within the offline data
|
||||
for (const Offline::Entry& e : fr.getEntries()) {
|
||||
|
||||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
if (e.type == Offline::Sensor::WIFI_FTM) {
|
||||
auto ftm = fr.getWifiFtm()[e.idx].data;
|
||||
|
||||
float ftm_offset = Settings::NUCS.at(ftm.getAP().getMAC()).ftm_offset;
|
||||
float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset
|
||||
|
||||
auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
|
||||
float predictDist = kalman.predict(ts, ftmDist);
|
||||
|
||||
ftm.setFtmDist(predictDist);
|
||||
|
||||
obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm);
|
||||
} else if (e.type == Offline::Sensor::WIFI) {
|
||||
//obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
|
||||
//ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
|
||||
} else if (e.type == Offline::Sensor::ACC) {
|
||||
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
|
||||
++ctrl.numStepsSinceLastEval;
|
||||
}
|
||||
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
|
||||
pd.addAccelerometer(ts, _acc.data);
|
||||
|
||||
//simpleActivity walking / standing
|
||||
act.add(ts, fr.getAccelerometer()[e.idx].data);
|
||||
|
||||
} else if (e.type == Offline::Sensor::GYRO) {
|
||||
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
|
||||
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
|
||||
|
||||
ctrl.headingChangeSinceLastEval += delta_gyro;
|
||||
|
||||
} else if (e.type == Offline::Sensor::BARO) {
|
||||
relBaro.add(ts, fr.getBarometer()[e.idx].data);
|
||||
obs.relativePressure = relBaro.getPressureRealtiveToStart();
|
||||
obs.sigmaPressure = relBaro.getSigma();
|
||||
|
||||
//simpleActivity stairs up / down
|
||||
act.add(ts, fr.getBarometer()[e.idx].data);
|
||||
obs.activity = act.get();
|
||||
}
|
||||
|
||||
if (ctrl.numStepsSinceLastEval > 0)
|
||||
//if (ts - lastTimestamp >= Timestamp::fromMS(500))
|
||||
//if (obs.wifi.size() == 4)
|
||||
{
|
||||
|
||||
obs.currentTime = ts;
|
||||
ctrl.currentTime = ts;
|
||||
|
||||
// if(ctrl.numStepsSinceLastEval > 0){
|
||||
// pf.updateTransitionOnly(&ctrl);
|
||||
// }
|
||||
MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs);
|
||||
ctrl.afterEval();
|
||||
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
|
||||
lastTimestamp = ts;
|
||||
|
||||
ctrl.lastEstimate = est.pos.pos;
|
||||
|
||||
|
||||
// draw wifi ranges
|
||||
for (auto& ftm : obs.wifi)
|
||||
{
|
||||
int nucid = Settings::NUCS.at(ftm.second.getAP().getMAC()).ID;
|
||||
|
||||
if (nucid == 1)
|
||||
{
|
||||
Point3 apPos = Settings::data.Path0.APs.find(ftm.first)->second;
|
||||
//plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist());
|
||||
}
|
||||
}
|
||||
|
||||
obs.wifi.clear();
|
||||
|
||||
//plot
|
||||
//dbg.showParticles(pf.getParticles());
|
||||
//dbg.setCurPos(est.pos.pos);
|
||||
//dbg.setGT(gtPos);
|
||||
//dbg.addEstimationNode(est.pos.pos);
|
||||
//dbg.addGroundTruthNode(gtPos);
|
||||
//dbg.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
|
||||
//dbg.draw();
|
||||
|
||||
plot.showParticles(pf.getParticles());
|
||||
plot.setCurEst(est.pos.pos);
|
||||
plot.setGroundTruth(gtPos);
|
||||
|
||||
plot.addEstimationNode(est.pos.pos);
|
||||
plot.setActivity((int) act.get());
|
||||
//plot.setView(0, 0);
|
||||
//plot.splot.getView().setEnabled(false);
|
||||
//plot.splot.getView().setCamera(0, 0);
|
||||
//plot.splot.getView().setEqualXY(true);
|
||||
|
||||
plot.plot();
|
||||
|
||||
|
||||
//std::this_thread::sleep_for(500ms);
|
||||
|
||||
// error calc
|
||||
// float err_m = gtPos.getDistance(est.pos.pos);
|
||||
// errorStats.add(err_m);
|
||||
// errorFile << ts.ms() << " " << err_m << "\n";
|
||||
|
||||
//error calc with penalty for wrong floor
|
||||
double errorFactor = 3.0;
|
||||
Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
|
||||
Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
|
||||
float err_m = gtPosError.getDistance(estError);
|
||||
errorStats.add(err_m);
|
||||
errorFile << ts.ms() << " " << err_m << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// get someting on console
|
||||
std::cout << "Statistical Analysis Filtering: " << std::endl;
|
||||
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
|
||||
|
||||
// save the statistical data in file
|
||||
errorFile << "========================================================== \n";
|
||||
errorFile << "Average of all statistical data: \n";
|
||||
errorFile << "Median: " << errorStats.getMedian() << "\n";
|
||||
errorFile << "Average: " << errorStats.getAvg() << "\n";
|
||||
errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n";
|
||||
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
|
||||
errorFile.close();
|
||||
|
||||
return errorStats;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
//mainFtm(argc, argv);
|
||||
//return 0;
|
||||
|
||||
|
||||
Stats::Statistics<float> statsAVG;
|
||||
Stats::Statistics<float> statsMedian;
|
||||
Stats::Statistics<float> statsSTD;
|
||||
Stats::Statistics<float> statsQuantil;
|
||||
Stats::Statistics<float> tmp;
|
||||
|
||||
std::string evaluationName = "prologic/tmp";
|
||||
|
||||
for(int i = 0; i < 1; ++i){
|
||||
for(int j = 0; j < 1; ++j){
|
||||
tmp = run(Settings::data.Path0, j, evaluationName);
|
||||
statsMedian.add(tmp.getMedian());
|
||||
statsAVG.add(tmp.getAvg());
|
||||
statsSTD.add(tmp.getStdDev());
|
||||
statsQuantil.add(tmp.getQuantile(0.75));
|
||||
}
|
||||
|
||||
std::cout << "Iteration " << i << " completed" << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
std::cout << "Average of all statistical data: " << std::endl;
|
||||
std::cout << "Median: " << statsMedian.getAvg() << std::endl;
|
||||
std::cout << "Average: " << statsAVG.getAvg() << std::endl;
|
||||
std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
|
||||
std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
|
||||
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
|
||||
std::ofstream finalStatisticFile;
|
||||
finalStatisticFile.open (Settings::errorDir + evaluationName + ".csv", std::ios_base::app);
|
||||
|
||||
finalStatisticFile << "========================================================== \n";
|
||||
finalStatisticFile << "Average of all statistical data: \n";
|
||||
finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n";
|
||||
finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n";
|
||||
finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n";
|
||||
finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n";
|
||||
finalStatisticFile << "========================================================== \n";
|
||||
|
||||
finalStatisticFile.close();
|
||||
|
||||
//std::this_thread::sleep_for(std::chrono::seconds(60));
|
||||
|
||||
}
|
||||
2
code/main.h
Normal file
2
code/main.h
Normal file
@@ -0,0 +1,2 @@
|
||||
#pragma once
|
||||
|
||||
428
code/mainFtm.cpp
Normal file
428
code/mainFtm.cpp
Normal file
@@ -0,0 +1,428 @@
|
||||
#include "mainFtm.h"
|
||||
|
||||
#include "mesh.h"
|
||||
#include "filter.h"
|
||||
#include "Settings.h"
|
||||
//#include "meshPlotter.h"
|
||||
#include "Plotty.h"
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <filesystem>
|
||||
#include <chrono>
|
||||
|
||||
#include <Indoor/floorplan/v2/FloorplanReader.h>
|
||||
#include <Indoor/sensors/offline/FileReader.h>
|
||||
#include <Indoor/geo/Heading.h>
|
||||
#include <Indoor/geo/Point2.h>
|
||||
#include <Indoor/sensors/imu/TurnDetection.h>
|
||||
#include <Indoor/sensors/imu/StepDetection.h>
|
||||
#include <Indoor/sensors/imu/PoseDetection.h>
|
||||
#include <Indoor/sensors/imu/MotionDetection.h>
|
||||
#include <Indoor/sensors/pressure/RelativePressure.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
|
||||
|
||||
#include <Indoor/math/stats/Statistics.h>
|
||||
|
||||
#include "FtmKalman.h"
|
||||
|
||||
#include <sys/stat.h>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
std::vector<std::tuple<float, float, float>> getFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator, const MACAddress nuc)
|
||||
{
|
||||
std::vector<std::tuple<float, float, float>> result;
|
||||
|
||||
for (const Offline::Entry& e : fr.getEntries())
|
||||
{
|
||||
if (e.type == Offline::Sensor::WIFI_FTM)
|
||||
{
|
||||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
|
||||
|
||||
auto wifi = fr.getWifiFtm()[e.idx].data;
|
||||
|
||||
if (wifi.getAP().getMAC() == nuc)
|
||||
{
|
||||
Point3 apPos = Settings::data.Path0.APs.find(wifi.getAP().getMAC())->second;
|
||||
float apDist = gtPos.getDistance(apPos);
|
||||
float ftmDist = wifi.getFtmDist();
|
||||
float rssi = wifi.getRSSI();
|
||||
|
||||
result.push_back({ apDist, ftmDist, rssi });
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
std::pair<float, float> optimizeFtm(std::vector<std::tuple<float, float, float>>& values)
|
||||
{
|
||||
std::vector<std::pair<float, float>> error;
|
||||
|
||||
for (float offset = 0; offset < 10.0f; offset += 0.25)
|
||||
{
|
||||
Stats::Statistics<float> diffs;
|
||||
|
||||
for (const auto& data : values)
|
||||
{
|
||||
float apDist = std::get<0>(data);
|
||||
float ftmDist = std::get<1>(data);
|
||||
ftmDist += offset;
|
||||
|
||||
float diff = (apDist - ftmDist);
|
||||
|
||||
diffs.add(diff);
|
||||
}
|
||||
|
||||
error.push_back({ offset, diffs.getSquaredSumAvg() });
|
||||
}
|
||||
|
||||
auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
|
||||
return a.second < b.second;
|
||||
});
|
||||
|
||||
std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n";
|
||||
|
||||
return *minElement;
|
||||
}
|
||||
|
||||
std::pair<float, float> optimizeRssi(std::vector<std::tuple<float, float, float>>& values)
|
||||
{
|
||||
std::vector<std::pair<float, float>> error;
|
||||
|
||||
for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125)
|
||||
{
|
||||
Stats::Statistics<float> diffs;
|
||||
|
||||
for (const auto& data : values)
|
||||
{
|
||||
float apDist = std::get<0>(data);
|
||||
float rssi = std::get<2>(data);
|
||||
float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, rssi);
|
||||
|
||||
float diff = (apDist - rssiDist);
|
||||
|
||||
diffs.add(diff);
|
||||
}
|
||||
|
||||
error.push_back({ pathLoss, diffs.getSquaredSumAvg() });
|
||||
}
|
||||
|
||||
auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
|
||||
return a.second < b.second;
|
||||
});
|
||||
|
||||
std::cout << "Min path loss \t" << minElement->first << "\t" << minElement->second << "\n";
|
||||
|
||||
return *minElement;
|
||||
}
|
||||
|
||||
void optimize(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
|
||||
{
|
||||
int i = 1;
|
||||
for (auto nuc : {Settings::NUC1, Settings::NUC2, Settings::NUC3, Settings::NUC4})
|
||||
{
|
||||
auto values = getFtmValues(fr, gtInterpolator, nuc);
|
||||
std::cout << "NUC" << i++ << "\n";
|
||||
|
||||
optimizeFtm(values);
|
||||
optimizeRssi(values);
|
||||
}
|
||||
}
|
||||
|
||||
void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
|
||||
{
|
||||
std::fstream fs;
|
||||
fs.open("test.txt", std::fstream::out);
|
||||
|
||||
fs << "timestamp;nucid;dist;rssiDist;ftmDist;ftmStdDev" << "\n";
|
||||
|
||||
for (const Offline::Entry& e : fr.getEntries())
|
||||
{
|
||||
if (e.type == Offline::Sensor::WIFI_FTM)
|
||||
{
|
||||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
|
||||
|
||||
auto wifi = fr.getWifiFtm()[e.idx].data;
|
||||
|
||||
int nucid = Settings::NUCS.at(wifi.getAP().getMAC()).ID;
|
||||
float ftm_offset = Settings::NUCS.at(wifi.getAP().getMAC()).ftm_offset;
|
||||
float rssi_pathloss = Settings::NUCS.at(wifi.getAP().getMAC()).rssi_pathloss;
|
||||
|
||||
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
|
||||
float ftmDist = wifi.getFtmDist() + ftm_offset; //in m; plus static offset
|
||||
float ftmStdDev = wifi.getFtmDistStd();
|
||||
|
||||
Point3 apPos = Settings::data.Path0.APs.find(wifi.getAP().getMAC())->second;
|
||||
float apDist = gtPos.getDistance(apPos);
|
||||
|
||||
fs << ts.ms() << ";" << nucid << ";" << apDist << ";" << rssiDist << ";" << ftmDist << ";" << ftmStdDev << "\n";
|
||||
}
|
||||
|
||||
}
|
||||
fs.close();
|
||||
|
||||
}
|
||||
|
||||
static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
|
||||
|
||||
// reading file
|
||||
std::string currDir = std::filesystem::current_path().string();
|
||||
|
||||
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
|
||||
Offline::FileReader fr(setup.training[numFile]);
|
||||
|
||||
// ground truth
|
||||
std::vector<int> gtPath;
|
||||
for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
|
||||
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
|
||||
Stats::Statistics<float> errorStats;
|
||||
|
||||
//calculate distance of path
|
||||
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
|
||||
double distance = 0;
|
||||
for(int i = 1; i < gtEntries.size(); ++i){
|
||||
distance += gtEntries[i].value.getDistance(gtEntries[i-1].value);
|
||||
}
|
||||
|
||||
std::cout << "Distance of Path: " << distance << std::endl;
|
||||
|
||||
// error file
|
||||
const long int t = static_cast<long int>(time(NULL));
|
||||
auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
|
||||
evalDir.append(folder);
|
||||
|
||||
if (!std::experimental::filesystem::exists(evalDir)) {
|
||||
std::experimental::filesystem::create_directory(evalDir);
|
||||
}
|
||||
|
||||
std::ofstream errorFile;
|
||||
errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
|
||||
|
||||
|
||||
// wifi
|
||||
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
|
||||
kalmanMap->insert({ Settings::NUC1, Kalman(1, 6.156) });
|
||||
kalmanMap->insert({ Settings::NUC2, Kalman(2, 5.650) });
|
||||
kalmanMap->insert({ Settings::NUC3, Kalman(3, 6.107) });
|
||||
kalmanMap->insert({ Settings::NUC4, Kalman(4, 3.985) });
|
||||
|
||||
// mesh
|
||||
NM::NavMeshSettings set;
|
||||
MyNavMesh mesh;
|
||||
MyNavMeshFactory fac(&mesh, set);
|
||||
fac.build(map);
|
||||
|
||||
const Point3 srcPath0(9.8, 24.9, 0); // fixed start pos
|
||||
|
||||
// add shortest-path to destination
|
||||
//const Point3 dst(51, 45, 1.7);
|
||||
//const Point3 dst(25, 45, 0);
|
||||
//NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
|
||||
|
||||
// debug show
|
||||
//MeshPlotter dbg;
|
||||
//dbg.addFloors(map);
|
||||
//dbg.addOutline(map);
|
||||
//dbg.addMesh(mesh);
|
||||
////dbg.addDijkstra(mesh);
|
||||
//dbg.draw();
|
||||
|
||||
Plotty plot(map);
|
||||
plot.buildFloorplan();
|
||||
plot.setGroundTruth(gtPath);
|
||||
plot.plot();
|
||||
|
||||
// particle-filter
|
||||
const int numParticles = 5000;
|
||||
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath0); // known position
|
||||
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
|
||||
auto eval = std::make_unique<MyPFEval>();
|
||||
eval->assignProps = true;
|
||||
eval->kalmanMap = kalmanMap;
|
||||
|
||||
//auto trans = std::make_unique<MyPFTrans>(mesh);
|
||||
auto trans = std::make_unique<MyPFTransStatic>();
|
||||
|
||||
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
|
||||
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
|
||||
|
||||
// setup
|
||||
MyFilter pf(numParticles, std::move(init));
|
||||
pf.setEvaluation(std::move(eval));
|
||||
pf.setTransition(std::move(trans));
|
||||
pf.setResampling(std::move(resample));
|
||||
pf.setEstimation(std::move(estimate));
|
||||
//pf.setNEffThreshold(0.85);
|
||||
pf.setNEffThreshold(0.0);
|
||||
|
||||
// sensors
|
||||
MyControl ctrl;
|
||||
MyObservation obs;
|
||||
|
||||
StepDetection sd;
|
||||
PoseDetection pd;
|
||||
TurnDetection td(&pd);
|
||||
RelativePressure relBaro;
|
||||
ActivityDetector act;
|
||||
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
|
||||
Timestamp lastTimestamp = Timestamp::fromMS(0);
|
||||
|
||||
//optimize(fr, gtInterpolator);
|
||||
//return errorStats;
|
||||
|
||||
int i = 0;
|
||||
//exportFtmValues(fr, gtInterpolator);
|
||||
|
||||
|
||||
// parse each sensor-value within the offline data
|
||||
for (const Offline::Entry& e : fr.getEntries()) {
|
||||
|
||||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
if (e.type == Offline::Sensor::WIFI_FTM) {
|
||||
auto ftm = fr.getWifiFtm()[e.idx].data;
|
||||
|
||||
float ftm_offset = Settings::NUCS.at(ftm.getAP().getMAC()).ftm_offset;
|
||||
float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset
|
||||
|
||||
auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
|
||||
float predictDist = kalman.predict(ts, ftmDist);
|
||||
|
||||
//ftm.setFtmDist(predictDist);
|
||||
|
||||
obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm);
|
||||
}
|
||||
|
||||
//if (ctrl.numStepsSinceLastEval > 0)
|
||||
//if (ts - lastTimestamp >= Timestamp::fromMS(500))
|
||||
if (obs.wifi.size() == 4)
|
||||
{
|
||||
|
||||
obs.currentTime = ts;
|
||||
ctrl.currentTime = ts;
|
||||
|
||||
// if(ctrl.numStepsSinceLastEval > 0){
|
||||
// pf.updateTransitionOnly(&ctrl);
|
||||
// }
|
||||
MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs);
|
||||
ctrl.afterEval();
|
||||
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
|
||||
lastTimestamp = ts;
|
||||
|
||||
ctrl.lastEstimate = est.pos.pos;
|
||||
|
||||
|
||||
// draw wifi ranges
|
||||
for (auto& ftm : obs.wifi)
|
||||
{
|
||||
int nucid = Settings::NUCS.at(ftm.second.getAP().getMAC()).ID;
|
||||
|
||||
if (nucid == 1)
|
||||
{
|
||||
Point3 apPos = Settings::data.Path0.APs.find(ftm.first)->second;
|
||||
// plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist());
|
||||
}
|
||||
}
|
||||
|
||||
obs.wifi.clear();
|
||||
|
||||
//plot
|
||||
//dbg.showParticles(pf.getParticles());
|
||||
//dbg.setCurPos(est.pos.pos);
|
||||
//dbg.setGT(gtPos);
|
||||
//dbg.addEstimationNode(est.pos.pos);
|
||||
//dbg.addGroundTruthNode(gtPos);
|
||||
//dbg.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
|
||||
//dbg.draw();
|
||||
|
||||
plot.showParticles(pf.getParticles());
|
||||
plot.setCurEst(est.pos.pos);
|
||||
plot.setGroundTruth(gtPos);
|
||||
|
||||
plot.addEstimationNode(est.pos.pos);
|
||||
plot.setActivity((int) act.get());
|
||||
plot.splot.getView().setCamera(0, 0);
|
||||
plot.splot.getView().setEqualXY(true);
|
||||
|
||||
plot.plot();
|
||||
|
||||
|
||||
//std::this_thread::sleep_for(500ms);
|
||||
|
||||
// error calc
|
||||
// float err_m = gtPos.getDistance(est.pos.pos);
|
||||
// errorStats.add(err_m);
|
||||
// errorFile << ts.ms() << " " << err_m << "\n";
|
||||
|
||||
//error calc with penalty for wrong floor
|
||||
double errorFactor = 3.0;
|
||||
Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
|
||||
Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
|
||||
float err_m = gtPosError.getDistance(estError);
|
||||
errorStats.add(err_m);
|
||||
errorFile << ts.ms() << " " << err_m << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// get someting on console
|
||||
std::cout << "Statistical Analysis Filtering: " << std::endl;
|
||||
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
|
||||
|
||||
// save the statistical data in file
|
||||
errorFile << "========================================================== \n";
|
||||
errorFile << "Average of all statistical data: \n";
|
||||
errorFile << "Median: " << errorStats.getMedian() << "\n";
|
||||
errorFile << "Average: " << errorStats.getAvg() << "\n";
|
||||
errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n";
|
||||
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
|
||||
errorFile.close();
|
||||
|
||||
return errorStats;
|
||||
}
|
||||
|
||||
int mainFtm(int argc, char** argv) {
|
||||
|
||||
Stats::Statistics<float> statsAVG;
|
||||
Stats::Statistics<float> statsMedian;
|
||||
Stats::Statistics<float> statsSTD;
|
||||
Stats::Statistics<float> statsQuantil;
|
||||
Stats::Statistics<float> tmp;
|
||||
|
||||
std::string evaluationName = "prologic/tmp";
|
||||
|
||||
for(int i = 0; i < 1; ++i){
|
||||
for(int j = 0; j < 1; ++j){
|
||||
tmp = run(Settings::data.Path0, j, evaluationName);
|
||||
statsMedian.add(tmp.getMedian());
|
||||
statsAVG.add(tmp.getAvg());
|
||||
statsSTD.add(tmp.getStdDev());
|
||||
statsQuantil.add(tmp.getQuantile(0.75));
|
||||
}
|
||||
|
||||
std::cout << "Iteration " << i << " completed" << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
std::cout << "Average of all statistical data: " << std::endl;
|
||||
std::cout << "Median: " << statsMedian.getAvg() << std::endl;
|
||||
std::cout << "Average: " << statsAVG.getAvg() << std::endl;
|
||||
std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
|
||||
std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
|
||||
|
||||
//std::this_thread::sleep_for(std::chrono::seconds(60));
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
4
code/mainFtm.h
Normal file
4
code/mainFtm.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
int mainFtm(int argc, char** argv);
|
||||
|
||||
29
code/mesh.h
Normal file
29
code/mesh.h
Normal file
@@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <Indoor/navMesh/NavMesh.h>
|
||||
#include <Indoor/navMesh/NavMeshLocation.h>
|
||||
#include <Indoor/navMesh/NavMeshRandom.h>
|
||||
#include <Indoor/navMesh/NavMeshFactory.h>
|
||||
|
||||
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
|
||||
#include <Indoor/navMesh/meta/NavMeshDijkstra.h>
|
||||
|
||||
/** the triangle to use with the nav-mesh */
|
||||
class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {
|
||||
|
||||
// add own parameters here
|
||||
|
||||
public:
|
||||
|
||||
MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) {
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
using MyNavMeshFactory = NM::NavMeshFactory<MyNavMeshTriangle>;
|
||||
using MyNavMesh = NM::NavMesh<MyNavMeshTriangle>;
|
||||
using MyNavMeshLocation = NM::NavMeshLocation<MyNavMeshTriangle>;
|
||||
using MyNavMeshRandom = NM::NavMeshRandom<MyNavMeshTriangle>;
|
||||
using MyNavMeshWalkParams = NM::NavMeshWalkParams<MyNavMeshTriangle>;
|
||||
|
||||
240
code/meshPlotter.h
Normal file
240
code/meshPlotter.h
Normal file
@@ -0,0 +1,240 @@
|
||||
#pragma once
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
||||
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
|
||||
|
||||
#include <Indoor/math/Distributions.h>
|
||||
#include <Indoor/navMesh/NavMesh.h>
|
||||
#include <Indoor/floorplan/v2/Floorplan.h>
|
||||
|
||||
class NavMeshTriangleDijkstra;
|
||||
|
||||
/**
|
||||
* debug plot NavMeshes
|
||||
*/
|
||||
class MeshPlotter {
|
||||
|
||||
public:
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotSplot plot;
|
||||
K::GnuplotSplotElementLines pFloor;
|
||||
K::GnuplotSplotElementLines pOutline;
|
||||
K::GnuplotSplotElementLines lines;
|
||||
K::GnuplotSplotElementPoints border;
|
||||
K::GnuplotSplotElementColorPoints particles;
|
||||
K::GnuplotSplotElementColorPoints distances;
|
||||
K::GnuplotSplotElementLines pathEstimated;
|
||||
K::GnuplotSplotElementLines shortestPath;
|
||||
K::GnuplotSplotElementLines groundtruthPath;
|
||||
|
||||
private:
|
||||
|
||||
K::GnuplotFill gFill[6] = {
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level
|
||||
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
MeshPlotter() {
|
||||
gp << "set view equal xy\n";
|
||||
plot.add(&lines); lines.setShowPoints(true);
|
||||
plot.add(&border);
|
||||
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
|
||||
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
|
||||
plot.add(&groundtruthPath); groundtruthPath.getStroke().setWidth(2); groundtruthPath.getStroke().getColor().setHexStr("#000000");
|
||||
plot.add(&distances); distances.setPointSize(0.75); distances.setPointType(7);
|
||||
plot.add(&shortestPath); shortestPath.getStroke().setWidth(3);
|
||||
plot.add(&pFloor);
|
||||
plot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
|
||||
}
|
||||
|
||||
void draw() {
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
template <typename T> void showParticles(const std::vector<T>& particles) {
|
||||
this->particles.clear();
|
||||
double min = +999;
|
||||
double max = -999;
|
||||
for (const T& p : particles) {
|
||||
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
|
||||
const double prob = std::pow(p.weight, 0.25);
|
||||
this->particles.add(p3, prob);
|
||||
if (prob > max) {max = prob;}
|
||||
if (prob < min) {min = prob;}
|
||||
}
|
||||
plot.getAxisCB().setRange(min, max + 0.000001);
|
||||
}
|
||||
|
||||
|
||||
template <typename Tria> void addMesh(NM::NavMesh<Tria>& nm) {
|
||||
|
||||
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
|
||||
|
||||
const BBox3 bbox = nm.getBBox();
|
||||
|
||||
border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
|
||||
border.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
|
||||
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
|
||||
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
|
||||
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
|
||||
|
||||
//stairs in eigene group? vlt gehen dann auch die dellen weg?
|
||||
|
||||
for (const Tria* tria : nm) {
|
||||
const uint8_t type = tria->getType();
|
||||
if (type < 0 || type > 5) {
|
||||
throw std::runtime_error("out of type-bounds");
|
||||
}
|
||||
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
|
||||
pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
|
||||
pol->close();
|
||||
pol->setZIndex(tria->getP3().z);
|
||||
plot.getObjects().add(pol);
|
||||
|
||||
//for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
|
||||
// const Tria* o = nm.getNeighbor(tria, i);
|
||||
// const Point3 p1 = tria->getCenter();
|
||||
// const Point3 p2 = o.getCenter();
|
||||
// //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
|
||||
//}
|
||||
|
||||
for (const NM::NavMeshTriangle* o : *tria) {
|
||||
const Point3 p1 = tria->getCenter();
|
||||
const Point3 p2 = o->getCenter();
|
||||
// lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
plot.getObjects().reOrderByZIndex();
|
||||
|
||||
}
|
||||
|
||||
template <typename Tria> void addDijkstra(NM::NavMesh<Tria>& mesh) {
|
||||
|
||||
distances.clear();
|
||||
|
||||
// ensure Tria extends NavMeshTriangleDijkstra
|
||||
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
|
||||
|
||||
NM::NavMeshRandom<Tria> rnd = mesh.getRandom();
|
||||
|
||||
for (int i = 0; i < 5000; ++i) {
|
||||
NM::NavMeshLocation<Tria> loc = rnd.draw();
|
||||
float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance);
|
||||
distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v);
|
||||
}
|
||||
|
||||
|
||||
// Distribution::Uniform<float> dist (-0.5, +0.5);
|
||||
// for (const Tria* t : mesh) {
|
||||
// const Point3 posC = t->getCenter();
|
||||
// distances.add(K::GnuplotPoint3(posC.x+dist.draw(), posC.y+dist.draw(), posC.z), t->distAtCenter);
|
||||
// const Point3 pos1 = t->getP1();
|
||||
// distances.add(K::GnuplotPoint3(pos1.x+dist.draw(), pos1.y+dist.draw(), pos1.z), t->distAtP1);
|
||||
// const Point3 pos2 = t->getP2();
|
||||
// distances.add(K::GnuplotPoint3(pos2.x+dist.draw(), pos2.y+dist.draw(), pos2.z), t->distAtP2);
|
||||
// const Point3 pos3 = t->getP3();
|
||||
// distances.add(K::GnuplotPoint3(pos3.x+dist.draw(), pos3.y+dist.draw(), pos3.z), t->distAtP3);
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
template <typename Tria> void addDijkstra(std::vector<NM::NavMeshLocation<Tria>>& path) {
|
||||
shortestPath.clear();
|
||||
for (auto& e : path) {
|
||||
K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z);
|
||||
shortestPath.add(gp);
|
||||
}
|
||||
}
|
||||
|
||||
void addGroundTruthNode(const Point3 pos) {
|
||||
K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
groundtruthPath.add(gp);
|
||||
}
|
||||
|
||||
void addEstimationNode(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
pathEstimated.add(est);
|
||||
}
|
||||
|
||||
|
||||
void setTimeInMinute(const int minutes, const int seconds) {
|
||||
gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
|
||||
}
|
||||
|
||||
void setGT(const Point3 pt) {
|
||||
gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
|
||||
}
|
||||
|
||||
void setCurPos(const Point3 pt) {
|
||||
gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n";
|
||||
}
|
||||
|
||||
void saveToFile(std::ofstream& stream){
|
||||
gp.draw(plot);
|
||||
stream << "set terminal x11 size 2000,1500\n";
|
||||
stream << gp.getBuffer();
|
||||
stream << "pause -1\n";
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
void printOverview(const std::string& path) {
|
||||
gp << "set terminal png size 1280,720\n";
|
||||
gp << "set output '" << path << "_overview" << ".png'\n";
|
||||
gp << "set view 75,60\n";
|
||||
gp << "set autoscale xy\n";
|
||||
gp << "set autoscale z\n";
|
||||
draw();
|
||||
}
|
||||
|
||||
|
||||
//meshless drawing
|
||||
void addFloors(Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Floorplan::Floor* f : map->floors) {
|
||||
for (Floorplan::FloorObstacle* obs : f->obstacles) {
|
||||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
if (line) {
|
||||
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
|
||||
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
|
||||
pFloor.addSegment(p1, p2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void addOutline(Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Floorplan::Floor* f : map->floors) {
|
||||
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
|
||||
const int cnt = poly->poly.points.size();
|
||||
for (int i = 0; i < cnt; ++i) {
|
||||
Point2 p1 = poly->poly.points[(i+0)];
|
||||
Point2 p2 = poly->poly.points[(i+1)%cnt];
|
||||
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
|
||||
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
|
||||
pOutline.addSegment(gp1, gp2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
88
map/map.xml
Normal file
88
map/map.xml
Normal file
@@ -0,0 +1,88 @@
|
||||
<map width="0" depth="1.4012985e-45">
|
||||
<earthReg>
|
||||
<correspondences/>
|
||||
</earthReg>
|
||||
<floors>
|
||||
<floor atHeight="0" height="3" name="floor">
|
||||
<outline>
|
||||
<polygon name="" method="0" outdoor="false">
|
||||
<point x="4.9200001" y="28.9"/>
|
||||
<point x="4.9200001" y="16.6"/>
|
||||
<point x="14.04" y="16.6"/>
|
||||
<point x="14.04" y="17.74"/>
|
||||
<point x="18.08" y="17.74"/>
|
||||
<point x="18.08" y="16.6"/>
|
||||
<point x="23.879999" y="16.6"/>
|
||||
<point x="23.879999" y="28.9"/>
|
||||
</polygon>
|
||||
</outline>
|
||||
<obstacles>
|
||||
<wall material="3" type="1" x1="10.679999" y1="24.9" x2="5.2000003" y2="24.9" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.080291912" width="0.89999998" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="10.719999" y1="28.619999" x2="10.719999" y2="22.4" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="10.719999" y1="22.4" x2="13.54" y2="22.4" thickness="0.18000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="28.76" x2="23.719999" y2="28.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="23.719999" y1="28.76" x2="23.719999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="18.219999" y1="16.76" x2="23.719999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="18.219999" y1="17.9" x2="18.219999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="13.88" y1="17.9" x2="18.219999" y2="17.9" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="13.88" y1="16.76" x2="13.88" y2="17.9" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="16.76" x2="13.88" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="28.76" x2="5.0799999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="15.259999" y1="22.4" x2="18.08" y2="22.4" thickness="0.2"/>
|
||||
<wall material="3" type="1" x1="18.08" y1="28.619999" x2="18.08" y2="22.4" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="9.0999994" y1="24.859999" x2="9.0999994" y2="20.799999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.85221666" width="0.85000002" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="5.2199998" y1="20.779999" x2="10.66" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.92279404" width="0.85000002" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="16.859999" y1="17.98" x2="16.859999" y2="20.699999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="18.299999" y1="18.059999" x2="18.299999" y2="20.699999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="18.1" y1="24.859999" x2="23.619999" y2="24.859999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.083333187" width="0.85000002" height="2.0999999" io="false" lr="false"/>
|
||||
<door type="1" material="2" x01="0.41666663" width="0.89999998" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="19.699999" y1="20.82" x2="19.699999" y2="24.859999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="19.699999" y1="22.719999" x2="23.699999" y2="22.719999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="15.639999" y1="20.779999" x2="23.68" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.59057063" width="0.89999998" height="2.0999999" io="false" lr="false"/>
|
||||
<door type="1" material="2" x01="0.36476421" width="0.85000002" height="2.0999999" io="true" lr="false"/>
|
||||
<door type="1" material="2" x01="0.30769235" width="0.75" height="2.0999999" io="true" lr="true"/>
|
||||
<door type="1" material="2" x01="0.01736965" width="0.75" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="1" type="1" x1="10.7" y1="16.9" x2="10.7" y2="20.779999" thickness="0.12"/>
|
||||
<wall material="1" type="1" x1="10.7" y1="20.779999" x2="15.559999" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.94650221" width="1.1" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="1" type="1" x1="15.559999" y1="18.039999" x2="15.559999" y2="20.779999" thickness="0.12"/>
|
||||
</obstacles>
|
||||
<underlays>
|
||||
<underlay x="-0" y="0" sx="0.0105" sy="0.0105" name="" file="D:/Source/Navegadors/NavData/2019_05_12_prologic/map/Datenplan_OG.png"/>
|
||||
</underlays>
|
||||
<pois/>
|
||||
<gtpoints>
|
||||
<gtpoint id="1" x="9.8000002" y="24.9" z="0"/>
|
||||
<gtpoint id="2" x="9.8000002" y="21.6" z="0"/>
|
||||
<gtpoint id="3" x="19" y="21.6" z="0"/>
|
||||
<gtpoint id="4" x="19" y="27" z="0"/>
|
||||
<gtpoint id="5" x="12.2" y="23.299999" z="0"/>
|
||||
<gtpoint id="6" x="12.3" y="28" z="0"/>
|
||||
<gtpoint id="7" x="17.299999" y="28" z="0"/>
|
||||
<gtpoint id="8" x="17.700001" y="23.200001" z="0"/>
|
||||
<gtpoint id="0" x="14.2" y="23.9" z="0"/>
|
||||
</gtpoints>
|
||||
<accesspoints>
|
||||
<accesspoint name="NUC1" mac="38:de:ad:6d:77:25" x="7.5" y="18.700001" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC2" mac="38:de:ad:6d:60:ff" x="8.6000004" y="26.799999" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC3" mac="1c:1b:b5:ef:a2:9a" x="21.6" y="19.1" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC4" mac="1c:1b:b5:ec:d1:82" x="20.799999" y="27.1" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
</accesspoints>
|
||||
<beacons/>
|
||||
<fingerprints/>
|
||||
<stairs/>
|
||||
<elevators/>
|
||||
</floor>
|
||||
</floors>
|
||||
</map>
|
||||
83
map/map0_ap_path0.xml
Normal file
83
map/map0_ap_path0.xml
Normal file
@@ -0,0 +1,83 @@
|
||||
<map width="0" depth="1.4012985e-45">
|
||||
<earthReg>
|
||||
<correspondences/>
|
||||
</earthReg>
|
||||
<floors>
|
||||
<floor atHeight="0" height="3" name="floor">
|
||||
<outline>
|
||||
<polygon name="" method="0" outdoor="false">
|
||||
<point x="4.9200001" y="28.9"/>
|
||||
<point x="4.9200001" y="16.6"/>
|
||||
<point x="14.04" y="16.6"/>
|
||||
<point x="14.04" y="17.74"/>
|
||||
<point x="18.08" y="17.74"/>
|
||||
<point x="18.08" y="16.6"/>
|
||||
<point x="23.879999" y="16.6"/>
|
||||
<point x="23.879999" y="28.9"/>
|
||||
</polygon>
|
||||
</outline>
|
||||
<obstacles>
|
||||
<wall material="3" type="1" x1="10.679999" y1="24.9" x2="5.2000003" y2="24.9" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.080291912" width="0.89999998" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="10.719999" y1="28.619999" x2="10.719999" y2="22.4" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="10.719999" y1="22.4" x2="13.54" y2="22.4" thickness="0.18000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="28.76" x2="23.719999" y2="28.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="23.719999" y1="28.76" x2="23.719999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="18.219999" y1="16.76" x2="23.719999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="18.219999" y1="17.9" x2="18.219999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="13.88" y1="17.9" x2="18.219999" y2="17.9" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="13.88" y1="16.76" x2="13.88" y2="17.9" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="16.76" x2="13.88" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="5.0799999" y1="28.76" x2="5.0799999" y2="16.76" thickness="0.30000001"/>
|
||||
<wall material="3" type="1" x1="15.259999" y1="22.4" x2="18.08" y2="22.4" thickness="0.2"/>
|
||||
<wall material="3" type="1" x1="18.08" y1="28.619999" x2="18.08" y2="22.4" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="9.0999994" y1="24.859999" x2="9.0999994" y2="20.799999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.85221666" width="0.85000002" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="5.2199998" y1="20.779999" x2="10.66" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.92279404" width="0.85000002" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="16.859999" y1="17.98" x2="16.859999" y2="20.699999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="18.299999" y1="18.059999" x2="18.299999" y2="20.699999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="18.1" y1="24.859999" x2="23.619999" y2="24.859999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.083333187" width="0.85000002" height="2.0999999" io="false" lr="false"/>
|
||||
<door type="1" material="2" x01="0.41666663" width="0.89999998" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="3" type="1" x1="19.699999" y1="20.82" x2="19.699999" y2="24.859999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="19.699999" y1="22.719999" x2="23.699999" y2="22.719999" thickness="0.12"/>
|
||||
<wall material="3" type="1" x1="15.639999" y1="20.779999" x2="23.68" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.59057063" width="0.89999998" height="2.0999999" io="false" lr="false"/>
|
||||
<door type="1" material="2" x01="0.36476421" width="0.85000002" height="2.0999999" io="true" lr="false"/>
|
||||
<door type="1" material="2" x01="0.30769235" width="0.75" height="2.0999999" io="true" lr="true"/>
|
||||
<door type="1" material="2" x01="0.01736965" width="0.75" height="2.0999999" io="true" lr="false"/>
|
||||
</wall>
|
||||
<wall material="1" type="1" x1="10.7" y1="16.9" x2="10.7" y2="20.779999" thickness="0.12"/>
|
||||
<wall material="1" type="1" x1="10.7" y1="20.779999" x2="15.559999" y2="20.779999" thickness="0.12">
|
||||
<door type="1" material="2" x01="0.94650221" width="1.1" height="2.0999999" io="true" lr="true"/>
|
||||
</wall>
|
||||
<wall material="1" type="1" x1="15.559999" y1="18.039999" x2="15.559999" y2="20.779999" thickness="0.12"/>
|
||||
</obstacles>
|
||||
<underlays>
|
||||
<underlay x="-0" y="0" sx="0.0105" sy="0.0105" name="" file="D:/Source/Navegadors/NavData/2019_05_12_prologic/map/Datenplan_OG.png"/>
|
||||
</underlays>
|
||||
<pois/>
|
||||
<gtpoints>
|
||||
<gtpoint id="0" x="9.8000002" y="24.9" z="0"/>
|
||||
<gtpoint id="1" x="9.8000002" y="21.6" z="0"/>
|
||||
<gtpoint id="2" x="19" y="21.6" z="0"/>
|
||||
<gtpoint id="3" x="19" y="27" z="0"/>
|
||||
</gtpoints>
|
||||
<accesspoints>
|
||||
<accesspoint name="NUC1" mac="38:de:ad:6d:77:25" x="7.5" y="18.700001" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC2" mac="38:de:ad:6d:60:ff" x="8.6000004" y="26.799999" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC3" mac="1c:1b:b5:ef:a2:9a" x="21.6" y="19.1" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
<accesspoint name="NUC4" mac="1c:1b:b5:ec:d1:82" x="20.799999" y="27.1" z="2" mdl_txp="0" mdl_exp="0" mdl_waf="0"/>
|
||||
</accesspoints>
|
||||
<beacons/>
|
||||
<fingerprints/>
|
||||
<stairs/>
|
||||
<elevators/>
|
||||
</floor>
|
||||
</floors>
|
||||
</map>
|
||||
57852
measurements/data/Pixel2/Path0_0605.csv
Normal file
57852
measurements/data/Pixel2/Path0_0605.csv
Normal file
File diff suppressed because it is too large
Load Diff
3
measurements/data/wifiModel.xml
Normal file
3
measurements/data/wifiModel.xml
Normal file
@@ -0,0 +1,3 @@
|
||||
<root type="WiFiModelLogDistCeiling">
|
||||
|
||||
</root>
|
||||
Reference in New Issue
Block a user