diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index af2a3be..2d0c52c 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -22,6 +22,8 @@ INCLUDE_DIRECTORIES( ../../ ../../../ ../../../../ + + ../../eigen3 ) @@ -49,6 +51,7 @@ FILE(GLOB SOURCES mainProb.cpp Eval.cpp FtmKalman.cpp + trilateration.cpp ) diff --git a/code/main.cpp b/code/main.cpp index c80ac87..2645362 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -737,7 +737,7 @@ int main(int argc, char** argv) //Settings::data.Path10, //Settings::data.Path11 //Settings::data.Path20, - Settings::data.Path21, + //Settings::data.Path21, Settings::data.Path22, }; @@ -745,7 +745,6 @@ int main(int argc, char** argv) { Settings::CurrentPath = setupToRun; - for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp index 8a5ac6b..cee3f02 100644 --- a/code/mainTrilat.cpp +++ b/code/mainTrilat.cpp @@ -35,20 +35,29 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str { // reading file Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[walkIdx]); + Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps); // ground truth - Interpolator gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath); + std::vector gtPath = setup.gtPath; + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); CombinedStats errorStats; //calculate distance of path std::vector::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); - double distance = 0; + double gtTotalDistance = 0; + Stats::Statistics gtWalkingSpeed; for (int i = 1; i < gtEntries.size(); ++i) { - distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value); + double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value); + double timeDiff = static_cast(gtEntries[i].key - gtEntries[i - 1].key); + + double walkingSpeed = distance / (timeDiff / 1000.0f); // m / s + + gtWalkingSpeed.add(walkingSpeed); + gtTotalDistance += distance; } - std::cout << "Distance of Path: " << distance << std::endl; + std::cout << "Distance of Path: " << gtTotalDistance << std::endl; + std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl; // debug show //MeshPlotter dbg; @@ -60,138 +69,139 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Plotty plot(map); plot.buildFloorplan(); - plot.setGroundTruth(setup.gtPath); + plot.setGroundTruth(gtPath); plot.setView(30, 0); + // APs Positions + for (auto& nucConfig : setup.NUCs) + { + plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1); + } plot.plot(); + std::vector obs; + + Timestamp lastTimestamp = Timestamp::fromMS(0); + Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py"); + //plotta.add("apPos", apPositions); - std::vector apPositions{ - Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(), - }; - - plotta.add("apPos", apPositions); - - std::vector data = filterOfflineData(fr); - - const bool UseFTM = false; const int movAvgWnd = 10; - std::array, 4> movAvgsFtm { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; - std::array, 4> movAvgsRssi { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; + std::unordered_map> movAvgsFtm; + std::unordered_map> movAvgsRssi; + for (auto& nucConfig : setup.NUCs) + { + movAvgsFtm.insert({ nucConfig.first, MovingAVG(movAvgWnd) }); + movAvgsRssi.insert({ nucConfig.first, MovingAVG(movAvgWnd) }); + } + std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; - std::vector gtPath, estPathFtm, estPathRssi; + std::vector estPathFtm, estPathRssi; - for (const WifiMeas& wifi : data) + for (const Offline::Entry& e : fr.getEntries()) { - Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); - gtPath.push_back(gtPos); - - float distErrorFtm = 0; - float distErrorRssi = 0; - - //if (wifi.numSucessMeas() == 4) - { - // FTM - { - std::vector avgDists; - - for (size_t i = 0; i < 4; i++) - { - float dist = wifi.ftmDists[i]; - - if (!isnan(dist)) - { - movAvgsFtm[i].add(dist); - } - - - if (movAvgsFtm[i].getNumUsed() == 0) - { - avgDists.push_back(0); - } - else - { - avgDists.push_back(movAvgsFtm[i].get()); - } - } - - Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists); - - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); - plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); - - // draw wifi ranges - for (size_t i = 0; i < 4; i++) - { - plot.addCircle(i + 1, apPositions[i], avgDists[i]); - } - - // Error - distErrorFtm = gtPos.getDistance(estPos); - errorStats.ftm.add(distErrorFtm); - estPathFtm.push_back(estPos); - } - - - // RSSI - { - std::vector avgDists; - - for (size_t i = 0; i < 4; i++) - { - float dist = wifi.rssiDists[i]; - - if (!isnan(dist)) - { - movAvgsRssi[i].add(dist); - } - - - if (movAvgsRssi[i].getNumUsed() == 0) - { - avgDists.push_back(0); - } - else - { - avgDists.push_back(movAvgsRssi[i].get()); - } - } - - Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists); - - plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); - - // Error - distErrorRssi = gtPos.getDistance(estPos); - errorStats.rssi.add(distErrorRssi); - estPathRssi.push_back(estPos); - } - - //std::cout << wifi.ts.ms() << " " << distError << "\n"; - - errorValuesFtm.push_back(distErrorFtm); - errorValuesRssi.push_back(distErrorRssi); - timestamps.push_back(wifi.ts.ms()); - - plotta.add("t", timestamps); - plotta.add("errorFtm", errorValuesFtm); - plotta.add("errorRssi", errorValuesRssi); - plotta.frame(); + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } + + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); + + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.push_back(wifiFtm); + + if (ts - lastTimestamp >= Timestamp::fromMS(500)) + { + // Do update + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + std::unordered_map> apPosDistMap; + + for (const WiFiMeasurement& wifi : obs) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + continue; + + const MACAddress& mac = wifi.getAP().getMAC(); + float ftm_offset = setup.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; + + float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); + + movAvgsFtm[mac].add(ftmDist); + movAvgsRssi[mac].add(rssiDist); + + apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() }; + } + + if (apPosDistMap.size() > 3) + { + // Do update for real + std::vector apPositions; + std::vector ftmDists; + std::vector rssiDists; + + for (const auto& kvp : apPosDistMap) + { + apPositions.push_back(setup.NUCs.at(kvp.first).position.xy()); + ftmDists.push_back(kvp.second.first); + rssiDists.push_back(kvp.second.second); + } + + Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists); + Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists); + + // Error + float distErrorFtm = gtPos.getDistance(estFtmPos); + errorStats.ftm.add(distErrorFtm); + estPathFtm.push_back(estFtmPos); + + float distErrorRssi = gtPos.getDistance(estRssiPos); + errorStats.rssi.add(distErrorRssi); + estPathRssi.push_back(estRssiPos); + + errorValuesFtm.push_back(distErrorFtm); + errorValuesRssi.push_back(distErrorRssi); + timestamps.push_back(ts.ms()); + + plotta.add("t", timestamps); + plotta.add("errorFtm", errorValuesFtm); + plotta.add("errorRssi", errorValuesRssi); + plotta.frame(); + + // Plot + plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1)); + plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1)); + plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1)); + + // draw wifi ranges + if (Settings::PlotCircles) + { + plot.clearDistanceCircles(); + + for (size_t i = 0; i < ftmDists.size(); i++) + { + plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0)); + plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0)); + } + } + + + plot.plot(); + Sleep(100); + } + + obs.clear(); + lastTimestamp = ts; } - plot.plot(); - //Sleep(250); printf(""); } - plotta.add("gtPath", gtPath); plotta.add("estPathFtm", estPathFtm); plotta.add("estPathRssi", estPathRssi); plotta.frame(); @@ -211,46 +221,64 @@ int mainTrilat(int argc, char** argv) CombinedStats statsQuantil; CombinedStats tmp; - std::string evaluationName = "prologic/tmp"; + std::string evaluationName = "prologic/trilat"; - for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) + std::vector setupsToRun = { + //Settings::data.Path5, + //Settings::data.Path7, + //Settings::data.Path8, + //Settings::data.Path9, + //Settings::data.Path10, + //Settings::data.Path11 + //Settings::data.Path20, + Settings::data.Path21, + //Settings::data.Path22, + }; + + for (Settings::DataSetup setupToRun : setupsToRun) { - std::cout << "Executing walk " << walkIdx << "\n"; - for (int i = 0; i < 1; ++i) + Settings::CurrentPath = setupToRun; + + for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) { - std::cout << "Start of iteration " << i << "\n"; + std::cout << "Executing walk " << walkIdx << "\n"; + for (int i = 0; i < 1; ++i) + { + std::cout << "Start of iteration " << i << "\n"; - tmp = run(Settings::CurrentPath, walkIdx, evaluationName); + tmp = run(Settings::CurrentPath, walkIdx, evaluationName); - statsAVG.ftm.add(tmp.ftm.getAvg()); - statsMedian.ftm.add(tmp.ftm.getMedian()); - statsSTD.ftm.add(tmp.ftm.getStdDev()); - statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); + statsAVG.ftm.add(tmp.ftm.getAvg()); + statsMedian.ftm.add(tmp.ftm.getMedian()); + statsSTD.ftm.add(tmp.ftm.getStdDev()); + statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); - statsAVG.rssi.add(tmp.rssi.getAvg()); - statsMedian.rssi.add(tmp.rssi.getMedian()); - statsSTD.rssi.add(tmp.rssi.getStdDev()); - statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75)); + statsAVG.rssi.add(tmp.rssi.getAvg()); + statsMedian.rssi.add(tmp.rssi.getMedian()); + statsSTD.rssi.add(tmp.rssi.getStdDev()); + statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75)); - std::cout << "Iteration " << i << " completed" << std::endl; + std::cout << "Iteration " << i << " completed" << std::endl; + } } + + std::cout << "Results for path " << Settings::CurrentPath.name << std::endl; + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data FTM: " << std::endl; + std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data RSSI: " << std::endl; + std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; } - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data FTM: " << std::endl; - std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data RSSI: " << std::endl; - std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - return 0; } \ No newline at end of file diff --git a/code/trilateration.cpp b/code/trilateration.cpp new file mode 100644 index 0000000..94bfd9d --- /dev/null +++ b/code/trilateration.cpp @@ -0,0 +1,207 @@ +#include "trilateration.h" + +#include +#include + +#include + +#include +#include + +namespace Trilateration +{ + // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp + + Point2 calculateLocation2d(const std::vector& positions, const std::vector& 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); + Eigen::Vector2d pseudoInv = (m.transpose()*m).inverse() * m.transpose() *b; + + return Point2(pseudoInv.x(), pseudoInv.y()); + } + + Point3 calculateLocation3d(const std::vector& positions, const std::vector& 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()); + } + + + // Generic functor + // See http://eigen.tuxfamily.org/index.php?title=Functors + // C++ version of a function pointer that stores meta-data about the function + template + struct Functor + { + + // Information that tells the caller the numeric type (eg. double) and size (input / output dim) + typedef _Scalar Scalar; + enum { // Required by numerical differentiation module + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + + // Tell the caller the matrix sizes associated with the input, output, and jacobian + typedef Eigen::Matrix InputType; + typedef Eigen::Matrix ValueType; + typedef Eigen::Matrix JacobianType; + + // Local copy of the number of inputs + int m_inputs, m_values; + + // Two constructors: + Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} + Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + // Get methods for users to determine function input and output dimensions + int inputs() const { return m_inputs; } + int values() const { return m_values; } + + }; + + struct DistanceFunction : Functor + { + private: + const std::vector& positions; + const std::vector& distances; + + public: + DistanceFunction(const std::vector& positions, const std::vector& distances) + : Functor(positions.size(), positions.size()), positions(positions), distances(distances) + {} + + int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const + { + const Point2 p(x(0), x(1)); + + for (size_t i = 0; i < positions.size(); i++) + { + fvec(i) = p.getDistance(positions[i]) - distances[i]; + } + + return 0; + } + }; + + struct DistanceFunctionDiff : public Eigen::NumericalDiff + { + DistanceFunctionDiff(const DistanceFunction& functor) + : Eigen::NumericalDiff(functor, 1.0e-6) + {} + }; + + Point2 levenbergMarquardt(const std::vector& positions, const std::vector& distances) + { + Point2 pseudoInvApprox = calculateLocation2d(positions, distances); + Eigen::Vector2d initVal; + initVal << pseudoInvApprox.x, pseudoInvApprox.y; + + Eigen::Vector2d startVal; + //startVal << pseudoInvApprox.x, pseudoInvApprox.y; + startVal << 0, 0; + + DistanceFunction functor(positions, distances); + DistanceFunctionDiff numDiff(functor); + Eigen::LevenbergMarquardt lm(numDiff); + lm.parameters.maxfev = 2000; + lm.parameters.xtol = 1.0e-10; + std::cout << lm.parameters.maxfev << std::endl; + + Eigen::VectorXd z = startVal; + int ret = lm.minimize(z); + std::cout << "iter count: " << lm.iter << std::endl; + std::cout << "return status: " << ret << std::endl; + std::cout << "zSolver: " << z.transpose() << std::endl; + std::cout << "pseudoInv: " << initVal.transpose() << std::endl; + + + Point2 bla(z(0), z(1)); + + double errPseudo = 0; + double errLeven = 0; + for (size_t i = 0; i < positions.size(); i++) + { + double d1 = pseudoInvApprox.getDistance(positions[i]) - distances[i]; + errPseudo += d1 * d1; + + double d2 = bla.getDistance(positions[i]) - distances[i]; + errLeven += d2 * d2; + } + + //assert(errLeven <= errPseudo); + + std::cout << "err pseud: " << errPseudo << std::endl; + std::cout << "err leven: " << errLeven << std::endl << std::endl; + + return Point2(z(0), z(1)); + + } +} \ No newline at end of file diff --git a/code/trilateration.h b/code/trilateration.h index cb4dbce..3618dff 100644 --- a/code/trilateration.h +++ b/code/trilateration.h @@ -1,95 +1,14 @@ #pragma once -#include #include -#include - #include #include namespace Trilateration { - // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp + Point2 calculateLocation2d(const std::vector& positions, const std::vector& distances); + Point3 calculateLocation3d(const std::vector& positions, const std::vector& distances); - Point2 calculateLocation2d(const std::vector& positions, const std::vector& 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& positions, const std::vector& 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()); - } -} + Point2 levenbergMarquardt(const std::vector& positions, const std::vector& distances); +};