From fcce5451f4ccb7e976272afe0d474921c1221483 Mon Sep 17 00:00:00 2001 From: Urbano Miguel Nunes Date: Mon, 10 Jan 2022 14:34:19 +0000 Subject: [PATCH] Add tests for 3D sequences --- README.md | 34 ++++++ test/CMakeLists.txt | 2 + test/main_incremental_test_sequence.cpp | 2 + test/main_incremental_test_sequence_3d.cpp | 110 +++++++++++++++++++ test/main_test_sequence.cpp | 4 +- test/main_test_sequence_3d.cpp | 117 +++++++++++++++++++++ 6 files changed, 268 insertions(+), 1 deletion(-) create mode 100644 test/main_incremental_test_sequence_3d.cpp create mode 100644 test/main_test_sequence_3d.cpp diff --git a/README.md b/README.md index 6d60ba6..279a3df 100755 --- a/README.md +++ b/README.md @@ -150,6 +150,7 @@ To run the example, on a terminal type: To run the sequences test, you need to download at least one sequence of the dataset provided in . ### Batch Mode +#### 2D To run the test, on a terminal type: ```bash ./example_test_sequence @@ -172,7 +173,24 @@ For example, if you downloaded the `poster_rotation` sequence and stored it unde ``` a file containig the estimates using the *Approx. Tsallis* measure should be created under the `/foo/poster_rotation/estimates` directory (`/estimates` directory should be created before running the command). +#### 3D +To run the test, on a terminal type: + ```bash + ./example_test_sequence_3d + ``` +The additional executable arguments are as follows: + +- minimum-depth: +Minimum depth of the depth-augmented events. +- maximum-depth: +Maximum depth of the depth-augmented events. + +The rest of the arguments are the same as previously. +The events should already be undistorted and augmented with depth. +See [indoor_flying1](./dataset/indoor_flying1) for an example. + ### Incremental Mode +#### 2D To run the test, on a terminal type: ```bash ./example_incremental_test_sequence @@ -195,6 +213,22 @@ For example, if you downloaded the `poster_rotation` sequence and stored it unde ``` a file containig the estimates using the *Incremental Potential* measure should be created under the `/foo/poster_rotation/estimates` directory (`/estimates` directory should be created before running the command). +#### 3D +To run the test, on a terminal type: + ```bash + ./example_incremental_test_sequence_3d + ``` +The additional executable arguments are as follows: + +- minimum-depth: +Minimum depth of the depth-augmented events. +- depth-scale: +Depth scaling factor. + +The rest of the arguments are the same as previously. +The events should already be undistorted and augmented with depth. +See [indoor_flying1](./dataset/indoor_flying1) for an example. + ### Compute Errors To compute the errors for rotational motion estimation, run the MATLAB script [sequence_error.m](./dataset/poster_rotation/sequence_error.m). diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f3222f7..3a51ac3 100755 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -11,6 +11,7 @@ if(${LIB_NAME}_BATCH_MODE) add_new_executable(translation2d) add_new_executable(test_sequence) + add_new_executable(test_sequence_3d) endif() if(${LIB_NAME}_INCREMENTAL_MODE) @@ -19,4 +20,5 @@ if(${LIB_NAME}_INCREMENTAL_MODE) add_new_executable(incremental_translation2d) add_new_executable(incremental_test_sequence) + add_new_executable(incremental_test_sequence_3d) endif() diff --git a/test/main_incremental_test_sequence.cpp b/test/main_incremental_test_sequence.cpp index cbef8a4..2c570b8 100644 --- a/test/main_incremental_test_sequence.cpp +++ b/test/main_incremental_test_sequence.cpp @@ -37,6 +37,8 @@ main(int argc, char* argv[]) // incremental measures typedef IncrementalPotential Dispersion; // typedef IncrementalTsallis Dispersion; + // typedef IncrementalPotentialWhiten Dispersion; + // typedef IncrementalTsallisWhiten Dispersion; // read distorted events from file const std::string fevents(std::string(argv[1]) + "/events.txt"); diff --git a/test/main_incremental_test_sequence_3d.cpp b/test/main_incremental_test_sequence_3d.cpp new file mode 100644 index 0000000..3358b36 --- /dev/null +++ b/test/main_incremental_test_sequence_3d.cpp @@ -0,0 +1,110 @@ +#include +#include +#include + +#include "EventEMin.h" + +using namespace EventEMin; + +int +main(int argc, char* argv[]) +{ + if (argc < 7) + { + std::cout << "usage: " << argv[0] + << " [events dir] [number of events] " + "[min depth] [depth scale] [saving dir] [file " + "name]\n"; + return -1; + } + + typedef float T; + + /* you can modify the model used by uncommenting the corresponding line */ + + // model + typedef IncrementalSixDOF Model; + // typedef IncrementalTranslation3D Model; + + constexpr int NDims = Model::NDims; + + /* you can modify the dispersion measure used by uncommenting the + corresponding line */ + + // incremental measures + typedef IncrementalPotential Dispersion; + // typedef IncrementalTsallis Dispersion; + + // read undistorted depth-augmented events from file + const std::string fevents(std::string(argv[1]) + "/events.txt"); + std::ifstream fin(fevents.c_str()); + if (!fin.is_open()) + { + std::cerr << "error reading events from file " << fevents << '\n'; + return -1; + } + + // write estimates to file + const std::string festimates(std::string(argv[5]) + "/" + + std::string(argv[6]) + "_estimates.txt"); + std::ofstream fout(festimates.c_str()); + if (!fout.is_open()) + { + std::cerr << "error writing estimates to file " << festimates << '\n'; + return -1; + } + + int width, height; + Matrix camParams; + const std::string fcalib(std::string(argv[1]) + "/calib.txt"); + const IO_STATUS ioStatus = loadCamParams(fcalib, width, height, camParams); + if (ioStatus != IO_SUCCESS) + { + ioStatusMessage(ioStatus, fcalib); + return -1; + } + + Vector scale; + scale << T(1.0), T(1.0), std::atof(argv[4]); + + // tolerance that indicates a minimum has been reached + const T minStep = T(1.0e-6); + // maximum iterations + const int maxIter = 10; + // neighbouring radius + const int wSize = 4; + // number of events to maintain + const int nEvents = std::atoi(argv[2]); + Dispersion dispersion(camParams, scale, + Dispersion::Params(minStep, maxIter, wSize), nEvents, + {width, height}); + + const T depthThresh = std::atof(argv[3]); + Vector c, ct; + T ts; + int polarity; + + for (int k = 0; load(fin, c, ts, polarity) == IO_SUCCESS; ++k) + { + const int x = std::round(c(0)); + const int y = std::round(c(1)); + + if (!(0 <= x && x < width && 0 <= y && y < height) || c(2) < depthThresh) + { + --k; + continue; + } + unprojectEvent()(camParams, c, ct); + + dispersion.run(ct, ts); + + if ((k + 1) % nEvents == 0) + { + std::cout << "ts: " << ts << ", vars: " << dispersion.vars().transpose() + << '\n'; + fout << ts << ' ' << dispersion.vars().transpose() << std::endl; + } + } + + return 0; +} diff --git a/test/main_test_sequence.cpp b/test/main_test_sequence.cpp index ccad475..c2c1621 100755 --- a/test/main_test_sequence.cpp +++ b/test/main_test_sequence.cpp @@ -102,6 +102,8 @@ main(int argc, char* argv[]) const int maxIter = 100, maxrIter = 1; // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration const int verbosity = 1; + // apply whitening pre-processing step + const bool whiten = false; const int nEvents = std::atoi(argv[2]); Matrix c; @@ -112,7 +114,7 @@ main(int argc, char* argv[]) { Matrix ct(NDims, c.cols()); unprojectEvents()(camParams, c, ct); - dispersion.assignPoints(ct, ts, polarity); + dispersion.assignPoints(ct, ts, polarity, whiten); // optimise Optimiser optimiser( diff --git a/test/main_test_sequence_3d.cpp b/test/main_test_sequence_3d.cpp new file mode 100644 index 0000000..5382b5d --- /dev/null +++ b/test/main_test_sequence_3d.cpp @@ -0,0 +1,117 @@ +#include +#include +#include + +#include "EventEMin.h" + +using namespace EventEMin; + +int +main(int argc, char* argv[]) +{ + if (argc < 7) + { + std::cout << "usage: " << argv[0] + << " [events dir] [number of events] " + "[min depth] [max depth] [saving dir] " + "[file name]\n"; + return -1; + } + + typedef float T; + + /* you can modify the model used by uncommenting the corresponding line */ + + // model + typedef SixDOF Model; + // typedef Translation3D Model; + + constexpr int NDims = Model::NDims, NVars = Model::NVars; + + // exact measures + // typedef Potential Dispersion; + // typedef Renyi Dispersion; + // typedef Shannon Dispersion; + // typedef SharmaMittal Dispersion; + // typedef Tsallis Dispersion; + // approximate measures + // typedef ApproximatePotential Dispersion; + // typedef ApproximateRenyi Dispersion; + // typedef ApproximateShannon Dispersion; + // typedef ApproximateSharmaMittal Dispersion; + typedef ApproximateTsallis Dispersion; + + // optimiser + typedef GSLfdfOptimiser Optimiser; + + // read undistorted depth-augmented events from file + const std::string fevents(std::string(argv[1]) + "/events.txt"); + std::ifstream fin(fevents.c_str()); + if (!fin.is_open()) + { + std::cerr << "error reading events from file " << fevents << '\n'; + return -1; + } + + // write estimates to file + const std::string festimates(std::string(argv[5]) + "/" + + std::string(argv[6]) + "_estimates.txt"); + std::ofstream fout(festimates.c_str()); + if (!fout.is_open()) + { + std::cerr << "error writing estimates to file " << festimates << '\n'; + return -1; + } + + int width, height; + Matrix camParams; + const std::string fcalib(std::string(argv[1]) + "/calib.txt"); + const IO_STATUS ioStatus = loadCamParams(fcalib, width, height, camParams); + if (ioStatus != IO_SUCCESS) + { + ioStatusMessage(ioStatus, fcalib); + return -1; + } + + Dispersion dispersion(width); + + // initial parameters + Vector vars; + vars.setConstant(1.0e-6); + + // initial step size of the optimisation + const double iniStep = 1.0; + // tolerance that indicates a minimum has been reached + const double tol = 1.0e-16; + // maximum iterations + const int maxIter = 100, maxrIter = 1; + // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration + const int verbosity = 1; + // apply whitening pre-processing step + const bool whiten = false; + + const int nEvents = std::atoi(argv[2]); + Matrix c; + Vector ts; + Vector polarity; + while (loadDepthThresh(nEvents, std::atof(argv[3]), std::atof(argv[4]), + fin, c, ts, polarity) == IO_SUCCESS) + { + Matrix ct(NDims, c.cols()); + unprojectEvents()(camParams, c, ct); + dispersion.assignPoints(ct, ts, polarity, whiten); + + Optimiser optimiser( + dispersion, + Optimiser::OptimiserParams(gsl_multimin_fdfminimizer_conjugate_fr, + iniStep, tol, maxIter, maxrIter, verbosity)); + + optimiser.run(vars); + vars = optimiser.vars(); + std::cout << "ts: " << dispersion.tsEnd() << ", vars: " << vars.transpose() + << '\n'; + fout << dispersion.tsEnd() << ' ' << vars.transpose() << std::endl; + } + + return 0; +}