From 528dde7376980c48cc07acfcc50118526c63116c Mon Sep 17 00:00:00 2001
From: Urbano Miguel Nunes <um.nunes@imperial.ac.uk>
Date: Mon, 10 Jan 2022 14:14:16 +0000
Subject: [PATCH] Fix scaling in incremental mode

---
 .../dispersion/incremental_dispersion.h          | 16 +++++++++++-----
 .../incremental_dispersion/dispersion.h          |  4 ++--
 include/EventEMin/test.h                         | 10 ++++++----
 test/main_incremental_6dof.cpp                   |  2 ++
 test/main_incremental_test_sequence.cpp          |  2 +-
 5 files changed, 22 insertions(+), 12 deletions(-)

diff --git a/include/EventEMin/dispersion/incremental_dispersion.h b/include/EventEMin/dispersion/incremental_dispersion.h
index f7bbf18..4087b5d 100644
--- a/include/EventEMin/dispersion/incremental_dispersion.h
+++ b/include/EventEMin/dispersion/incremental_dispersion.h
@@ -128,6 +128,8 @@ class Whitening
   typedef typename Model::Vars Vars;
 
  private:
+  projectEvent<T, Model::NDims> projectEvent_;
+
   Model model_;
 
   Stats<M> istats_;
@@ -136,20 +138,24 @@ class Whitening
   Whitening(void) = default;
 
   void
-  processEvent(const Ref<const Point>& scale, const Ref<const Point>& c,
-               Ref<Point> cs) const
+  processEvent(const Matrix<T, 3, 3>& camParams, const Ref<const Point>& scale,
+               const Ref<const Point>& c, Ref<Point> cs) const
   {
     if constexpr (W)
     {
       Point cw;
       whitenPoints(c - istats_.mean, istats_.w, cw);
-      cs = scale.array() *
-           (istats_.singularValues.array() * cw.array() + istats_.mean.array());
+      projectEvent_(
+          camParams,
+          ((istats_.singularValues.array() * cw.array() + istats_.mean.array())
+               .matrix()),
+          cs);
     }
     else
     {
-      cs = scale.array() * c.array();
+      projectEvent_(camParams, c, cs);
     }
+    cs.array() *= scale.array();
   }
 
   void
diff --git a/include/EventEMin/dispersion/incremental_dispersion/dispersion.h b/include/EventEMin/dispersion/incremental_dispersion/dispersion.h
index 556595d..8a4a5b0 100644
--- a/include/EventEMin/dispersion/incremental_dispersion/dispersion.h
+++ b/include/EventEMin/dispersion/incremental_dispersion/dispersion.h
@@ -220,7 +220,7 @@ class Dispersion
     DMatrix dcm, dcml;
     PMatrix per, perl;
     model_(varse.vars, c, tsDiffRef, cm, dcm, cg, per);
-    whitening_.processEvent(scale_, cm, cms);
+    whitening_.processEvent(camParams_, scale_, cm, cms);
 
     varse.val = T(0.0);
     varse.vNum.setZero();
@@ -229,7 +229,7 @@ class Dispersion
     for (int i = 0; i < static_cast<int>(inds.size()); ++i)
     {
       model_(varse.vars, c_[inds[i]], tsDiffRef_[i], cml, dcml, cgl, perl);
-      whitening_.processEvent(scale_, cml, cmsl);
+      whitening_.processEvent(camParams_, scale_, cml, cmsl);
       cmDiff = cms - cmsl;
 
       dispersionImpl_(cmDiff, dcm - dcml, cm - cml - cg + cgl, per - perl,
diff --git a/include/EventEMin/test.h b/include/EventEMin/test.h
index 58985e2..e9609e1 100755
--- a/include/EventEMin/test.h
+++ b/include/EventEMin/test.h
@@ -116,6 +116,9 @@ struct TestIncrementalParams
   int wSize;
   // number of events to maintain
   int nEvents;
+  // depth scaling factor
+  // only used when the number of dimensions is higher than 2
+  float depthScale;
 };
 
 template <typename Dispersion>
@@ -164,11 +167,10 @@ testIncrementalExample(const TestIncrementalParams& testIncrementalParams)
   Matrix<T> ct(NDims, nEvents);
   unprojectEvents<T, NDims>()(camParams, c, ct);
 
-  Vector<T, NDims> scale;
-  scale.template head<NDims>() = camParams.diagonal().template head<NDims>();
-  if (NDims == 3)
+  Vector<T, NDims> scale(Vector<T, NDims>::Ones());
+  if constexpr (NDims == 3)
   {
-    scale(2) = T(100.0);
+    scale(2) = testIncrementalParams.depthScale;
   }
 
   Dispersion dispersion(
diff --git a/test/main_incremental_6dof.cpp b/test/main_incremental_6dof.cpp
index ffc0629..bfd955b 100644
--- a/test/main_incremental_6dof.cpp
+++ b/test/main_incremental_6dof.cpp
@@ -29,6 +29,8 @@ main(void)
   testIncrementalParams.wSize = 4;
   // number of events to maintain
   testIncrementalParams.nEvents = 15000;
+  // depth scaling factor
+  testIncrementalParams.depthScale = T(100.0);
 
   return testIncrementalExample<Dispersion>(testIncrementalParams);
 }
diff --git a/test/main_incremental_test_sequence.cpp b/test/main_incremental_test_sequence.cpp
index 9d42acd..cbef8a4 100644
--- a/test/main_incremental_test_sequence.cpp
+++ b/test/main_incremental_test_sequence.cpp
@@ -74,7 +74,7 @@ main(int argc, char* argv[])
   cv::Mat undistortionMap;
   initUndistort<T>(width, height, camParamsCV, distCoeffs, undistortionMap);
 
-  const Vector<T, NDims> scale(camParams.diagonal().template head<NDims>());
+  const Vector<T, NDims> scale(Vector<T, NDims>::Ones());
 
   // tolerance that indicates a minimum has been reached
   const T minStep = T(1.0e-6);