Add tests for 3D sequences

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 <http://rpg.ifi.uzh.ch/davis_data.html>.
 
 ### Batch Mode
+#### 2D
 To run the test, on a terminal type:
   ```bash
   ./example_test_sequence <path-to-events-dir> <batch-size> <path-to-estimates-saving-dir> <estimates-file-name>
@@ -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 <path-to-events-dir> <batch-size> <minimum-depth> <maximum-depth> <path-to-estimates-saving-dir> <estimates-file-name>
+  ```
+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 <path-to-events-dir> <number-events> <path-to-estimates-saving-dir> <estimates-file-name>
@@ -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 <path-to-events-dir> <number-events> <minimum-depth> <depth-scale> <path-to-estimates-saving-dir> <estimates-file-name>
+  ```
+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).
 

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()

test/main_incremental_test_sequence.cpp

@@ -37,6 +37,8 @@ main(int argc, char* argv[])
   // incremental measures
   typedef IncrementalPotential<Model> Dispersion;
   // typedef IncrementalTsallis<Model> Dispersion;
+  // typedef IncrementalPotentialWhiten<Model> Dispersion;
+  // typedef IncrementalTsallisWhiten<Model> Dispersion;
 
   // read distorted events from file
   const std::string fevents(std::string(argv[1]) + "/events.txt");

test/main_incremental_test_sequence_3d.cpp

@@ -0,0 +1,110 @@
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#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<T> Model;
+  // typedef IncrementalTranslation3D<T> Model;
+
+  constexpr int NDims = Model::NDims;
+
+  /* you can modify the dispersion measure used by uncommenting the
+   corresponding line */
+
+  // incremental measures
+  typedef IncrementalPotential<Model> Dispersion;
+  // typedef IncrementalTsallis<Model> 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<T, 3, 3> camParams;
+  const std::string fcalib(std::string(argv[1]) + "/calib.txt");
+  const IO_STATUS ioStatus = loadCamParams<T>(fcalib, width, height, camParams);
+  if (ioStatus != IO_SUCCESS)
+  {
+    ioStatusMessage(ioStatus, fcalib);
+    return -1;
+  }
+
+  Vector<T, NDims> 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<T, NDims> c, ct;
+  T ts;
+  int polarity;
+
+  for (int k = 0; load<T, NDims>(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<T, NDims>()(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;
+}

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<T> c;
@@ -112,7 +114,7 @@ main(int argc, char* argv[])
   {
     Matrix<T> ct(NDims, c.cols());
     unprojectEvents<T, NDims>()(camParams, c, ct);
-    dispersion.assignPoints(ct, ts, polarity);
+    dispersion.assignPoints(ct, ts, polarity, whiten);
 
     // optimise
     Optimiser optimiser(

test/main_test_sequence_3d.cpp

@@ -0,0 +1,117 @@
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#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<T> Model;
+  // typedef Translation3D<T> Model;
+
+  constexpr int NDims = Model::NDims, NVars = Model::NVars;
+
+  // exact measures
+  // typedef Potential<Model> Dispersion;
+  // typedef Renyi<Model> Dispersion;
+  // typedef Shannon<Model> Dispersion;
+  // typedef SharmaMittal<Model> Dispersion;
+  // typedef Tsallis<Model> Dispersion;
+  // approximate measures
+  // typedef ApproximatePotential<Model> Dispersion;
+  // typedef ApproximateRenyi<Model> Dispersion;
+  // typedef ApproximateShannon<Model> Dispersion;
+  // typedef ApproximateSharmaMittal<Model> Dispersion;
+  typedef ApproximateTsallis<Model> Dispersion;
+
+  // optimiser
+  typedef GSLfdfOptimiser<Dispersion> 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<T, 3, 3> camParams;
+  const std::string fcalib(std::string(argv[1]) + "/calib.txt");
+  const IO_STATUS ioStatus = loadCamParams<T>(fcalib, width, height, camParams);
+  if (ioStatus != IO_SUCCESS)
+  {
+    ioStatusMessage(ioStatus, fcalib);
+    return -1;
+  }
+
+  Dispersion dispersion(width);
+
+  // initial parameters
+  Vector<T, NVars> 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<T> c;
+  Vector<T> ts;
+  Vector<int> polarity;
+  while (loadDepthThresh<T>(nEvents, std::atof(argv[3]), std::atof(argv[4]),
+                            fin, c, ts, polarity) == IO_SUCCESS)
+  {
+    Matrix<T> ct(NDims, c.cols());
+    unprojectEvents<T, NDims>()(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;
+}