Added trilateration code

2019-08-06 11:31:45 +02:00
parent a7571862c6
commit 52eac3a28a
3 changed files with 353 additions and 0 deletions
--- a/code/trilateration.h
+++ b/code/trilateration.h
@@ -0,0 +1,95 @@
+#pragma once
+
+#include <cmath>
+#include <vector>
+
+#include <eigen3/Eigen/Eigen>
+
+#include "..\Indoor\geo\Point2.h"
+#include "..\Indoor\geo\Point3.h"
+
+namespace Trilateration
+{
+    // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
+
+    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    {
+        // To locate position on a 2d plan, have to get at least 3 becaons,
+        // otherwise return false.
+        if (positions.size() < 3)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 2);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, r1, r2;
+        
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y;
+                x2 = positions[j].x, y2 = positions[j].y;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                    (pow(y1, 2) - pow(y2, 2)) -
+                    (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+        return Point2(location.x(), location.y());
+    }
+
+    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
+    {
+        // To locate position in a 3D space, have to get at least 4 becaons
+        if (positions.size() < 4)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 3);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, z1, z2, r1, r2;
+
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
+                x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                m(row, 2) = z1 - z2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                          (pow(y1, 2) - pow(y2, 2)) +
+                          (pow(z1, 2) - pow(z2, 2)) -
+                          (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+        return Point3(location.x(), location.y(), location.z());
+    }
+}