From d2148b102f86243fb8024b879f8301c7a3272eac Mon Sep 17 00:00:00 2001
From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com>
Date: Wed, 1 May 2024 13:11:25 +0100
Subject: [PATCH] ENH: test absolute translation solver.

---
 .../AbsoluteTranslationSolver.hpp             | 22 ++++++++++++-------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp
index bbf32038e..fb2a67078 100644
--- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp
+++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/AbsoluteTranslationSolver.hpp
@@ -27,28 +27,34 @@ namespace DO::Sara {
     static constexpr auto num_points = 2;
     static constexpr auto num_models = 1;
 
-    using model_type = Eigen::Vector3<T>;
+    using translation_vector_type = Eigen::Vector3<T>;
+    using scale_vector_type = Eigen::Vector2<T>;
+    using model_type = std::pair<translation_vector_type, scale_vector_type>;
 
     //! @brief Inputs are rotated world scene points and the backprojected rays.
     auto operator()(const Eigen::Matrix<T, 3, 2>& Rx,
                     const Eigen::Matrix<T, 3, 2>& y) const -> model_type
     {
-      const auto x0 = Rx.col(0);
-      const auto x1 = Rx.col(1);
-      const auto y0 = y.col(0);
-      const auto y1 = y.col(1);
+      const Eigen::Vector3<T> x0 = Rx.col(0);
+      const Eigen::Vector3<T> x1 = Rx.col(1);
+      const Eigen::Vector3<T> y0 = y.col(0);
+      const Eigen::Vector3<T> y1 = y.col(1);
 
       static const auto I3 = Eigen::Matrix3<T>::Identity();
       static const auto O3 = Eigen::Vector3<T>::Zero();
 
       auto A = Eigen::Matrix<T, 6, 5>{};
-      A.template topRows<3>() << I3, y0, O3;
-      A.template bottomRows<3>() << I3, O3, y1;
+      A.template topRows<3>() << -I3, y0, O3;
+      A.template bottomRows<3>() << -I3, O3, y1;
 
       auto b = Eigen::Vector<T, 6>{};
       b << x0, x1;
 
-      return A.template colPivHouseholderQr().solve(b);
+      const auto x = A.colPivHouseholderQr().solve(b);
+
+      const translation_vector_type t = x.head(3);
+      const scale_vector_type scales = x.tail(2);
+      return {t, scales};
     }
   };