95 lines
3.3 KiB
C++
95 lines
3.3 KiB
C++
#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());
|
|
}
|
|
} |