# HG changeset patch
# User Sebastien Jodogne <s.jodogne@gmail.com>
# Date 1521202763 -3600
# Node ID 964118e7e6de7beb784a417b1da912d974882d52
# Parent  45b03b04a777db87bdb02352f1a35cbd4fdbe7d6
unit test for AlignVectorsWithRotation

diff -r 45b03b04a777 -r 964118e7e6de Framework/Toolbox/FiniteProjectiveCamera.cpp
--- a/Framework/Toolbox/FiniteProjectiveCamera.cpp	Fri Mar 16 13:02:17 2018 +0100
+++ b/Framework/Toolbox/FiniteProjectiveCamera.cpp	Fri Mar 16 13:19:23 2018 +0100
@@ -253,8 +253,8 @@
     }
       
     Matrix a;
-    LinearAlgebra::AlignVectorsWithRotation(a, camera - principalPoint,
-                                            LinearAlgebra::CreateVector(0, 0, -1));
+    GeometryToolbox::AlignVectorsWithRotation(a, camera - principalPoint,
+                                              LinearAlgebra::CreateVector(0, 0, -1));
 
     Matrix r = LinearAlgebra::Product(GeometryToolbox::CreateRotationMatrixAlongZ(angle), a);
 
diff -r 45b03b04a777 -r 964118e7e6de Framework/Toolbox/GeometryToolbox.cpp
--- a/Framework/Toolbox/GeometryToolbox.cpp	Fri Mar 16 13:02:17 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.cpp	Fri Mar 16 13:19:23 2018 +0100
@@ -415,5 +415,63 @@
         return true;
       }
     }
+
+
+    void AlignVectorsWithRotation(Matrix& r,
+                                  const Vector& a,
+                                  const Vector& b)
+    {
+      // This is Rodrigues' rotation formula:
+      // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
+
+      // Check also result A4.6 from "Multiple View Geometry in Computer
+      // Vision - 2nd edition" (p. 584)
+  
+      if (a.size() != 3 ||
+          b.size() != 3)
+      {
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+      }
+
+      double aNorm = boost::numeric::ublas::norm_2(a);
+      double bNorm = boost::numeric::ublas::norm_2(b);
+
+      if (LinearAlgebra::IsCloseToZero(aNorm) ||
+          LinearAlgebra::IsCloseToZero(bNorm))
+      {
+        LOG(ERROR) << "Vector with zero norm";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);      
+      }
+
+      Vector aUnit, bUnit;
+      aUnit = a / aNorm;
+      bUnit = b / bNorm;
+
+      Vector v;
+      LinearAlgebra::CrossProduct(v, aUnit, bUnit);
+
+      double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
+
+      if (LinearAlgebra::IsCloseToZero(1 + cosine))
+      {
+        // "a == -b": TODO
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
+      }
+
+      Matrix k;
+      LinearAlgebra::CreateSkewSymmetric(k, v);
+
+#if 0
+      double sine = boost::numeric::ublas::norm_2(v);
+
+      r = (boost::numeric::ublas::identity_matrix<double>(3) +
+           sine * k + 
+           (1 - cosine) * boost::numeric::ublas::prod(k, k));
+#else
+      r = (boost::numeric::ublas::identity_matrix<double>(3) +
+           k + 
+           boost::numeric::ublas::prod(k, k) / (1 + cosine));
+#endif
+    }
   }
 }
diff -r 45b03b04a777 -r 964118e7e6de Framework/Toolbox/GeometryToolbox.h
--- a/Framework/Toolbox/GeometryToolbox.h	Fri Mar 16 13:02:17 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.h	Fri Mar 16 13:19:23 2018 +0100
@@ -95,6 +95,10 @@
                                const Vector& origin,
                                const Vector& direction);
 
+    void AlignVectorsWithRotation(Matrix& r,
+                                  const Vector& a,
+                                  const Vector& b);
+
     inline float ComputeBilinearInterpolationUnitSquare(float x,
                                                         float y,
                                                         float f00,    // source(0, 0)
diff -r 45b03b04a777 -r 964118e7e6de Framework/Toolbox/LinearAlgebra.cpp
--- a/Framework/Toolbox/LinearAlgebra.cpp	Fri Mar 16 13:02:17 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.cpp	Fri Mar 16 13:19:23 2018 +0100
@@ -537,64 +537,6 @@
     }
 
   
-    void AlignVectorsWithRotation(Matrix& r,
-                                  const Vector& a,
-                                  const Vector& b)
-    {
-      // This is Rodrigues' rotation formula:
-      // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
-
-      // Check also result A4.6 from "Multiple View Geometry in Computer
-      // Vision - 2nd edition" (p. 584)
-  
-      if (a.size() != 3 ||
-          b.size() != 3)
-      {
-        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
-      }
-
-      double aNorm = boost::numeric::ublas::norm_2(a);
-      double bNorm = boost::numeric::ublas::norm_2(b);
-
-      if (LinearAlgebra::IsCloseToZero(aNorm) ||
-          LinearAlgebra::IsCloseToZero(bNorm))
-      {
-        LOG(ERROR) << "Vector with zero norm";
-        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);      
-      }
-
-      Vector aUnit, bUnit;
-      aUnit = a / aNorm;
-      bUnit = b / bNorm;
-
-      Vector v;
-      LinearAlgebra::CrossProduct(v, aUnit, bUnit);
-
-      double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
-
-      if (LinearAlgebra::IsCloseToZero(1 + cosine))
-      {
-        // "a == -b": TODO
-        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
-      }
-
-      Matrix k;
-      CreateSkewSymmetric(k, v);
-
-#if 0
-      double sine = boost::numeric::ublas::norm_2(v);
-
-      r = (boost::numeric::ublas::identity_matrix<double>(3) +
-           sine * k + 
-           (1 - cosine) * boost::numeric::ublas::prod(k, k));
-#else
-      r = (boost::numeric::ublas::identity_matrix<double>(3) +
-           k + 
-           boost::numeric::ublas::prod(k, k) / (1 + cosine));
-#endif
-    }
-
-
     Matrix InvertScalingTranslationMatrix(const Matrix& t)
     {
       if (t.size1() != 4 ||
diff -r 45b03b04a777 -r 964118e7e6de Framework/Toolbox/LinearAlgebra.h
--- a/Framework/Toolbox/LinearAlgebra.h	Fri Mar 16 13:02:17 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.h	Fri Mar 16 13:19:23 2018 +0100
@@ -284,10 +284,6 @@
     void CreateSkewSymmetric(Matrix& s,
                              const Vector& v);
   
-    void AlignVectorsWithRotation(Matrix& r,
-                                  const Vector& a,
-                                  const Vector& b);
-
     Matrix InvertScalingTranslationMatrix(const Matrix& t);
 
     bool IsShearMatrix(const Matrix& shear);  
diff -r 45b03b04a777 -r 964118e7e6de UnitTestsSources/UnitTestsMain.cpp
--- a/UnitTestsSources/UnitTestsMain.cpp	Fri Mar 16 13:02:17 2018 +0100
+++ b/UnitTestsSources/UnitTestsMain.cpp	Fri Mar 16 13:19:23 2018 +0100
@@ -658,6 +658,73 @@
 }
 
 
+static bool IsEqualVector(OrthancStone::Vector a,
+                          OrthancStone::Vector b)
+{
+  if (a.size() == 3 &&
+      b.size() == 3)
+  {
+    OrthancStone::LinearAlgebra::NormalizeVector(a);
+    OrthancStone::LinearAlgebra::NormalizeVector(b);
+    return OrthancStone::LinearAlgebra::IsCloseToZero(boost::numeric::ublas::norm_2(a - b));
+  }
+  else
+  {
+    return false;
+  } 
+}
+
+
+TEST(GeometryToolbox, AlignVectorsWithRotation)
+{
+  OrthancStone::Vector a, b;
+  OrthancStone::Matrix r;
+
+  OrthancStone::LinearAlgebra::AssignVector(a, -200, 200, -846.63);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b);
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(r));
+  ASSERT_TRUE(IsEqualVector(OrthancStone::LinearAlgebra::Product(r, a), b));
+
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, b, a);
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(r));
+  ASSERT_TRUE(IsEqualVector(OrthancStone::LinearAlgebra::Product(r, b), a));
+
+  OrthancStone::LinearAlgebra::AssignVector(a, 1, 0, 0);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b);
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(r));
+  ASSERT_TRUE(IsEqualVector(OrthancStone::LinearAlgebra::Product(r, a), b));
+
+  OrthancStone::LinearAlgebra::AssignVector(a, 0, 1, 0);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b);
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(r));
+  ASSERT_TRUE(IsEqualVector(OrthancStone::LinearAlgebra::Product(r, a), b));
+
+  OrthancStone::LinearAlgebra::AssignVector(a, 0, 0, 1);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b);
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(r));
+  ASSERT_TRUE(IsEqualVector(OrthancStone::LinearAlgebra::Product(r, a), b));
+
+  OrthancStone::LinearAlgebra::AssignVector(a, 0, 0, 0);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+  ASSERT_THROW(OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b), Orthanc::OrthancException);
+
+  // TODO: Deal with opposite vectors
+
+  /*
+  OrthancStone::LinearAlgebra::AssignVector(a, 0, 0, -1);
+  OrthancStone::LinearAlgebra::AssignVector(b, 0, 0, 1);
+  OrthancStone::GeometryToolbox::AlignVectorsWithRotation(r, a, b);
+  OrthancStone::LinearAlgebra::Print(r);
+  OrthancStone::LinearAlgebra::Print(boost::numeric::ublas::prod(r, a));
+  */
+}
+
+
 int main(int argc, char **argv)
 {
   Orthanc::Logging::Initialize();