# HG changeset patch
# User Sebastien Jodogne <s.jodogne@gmail.com>
# Date 1518604166 -3600
# Node ID 197a5ddaf68ca216beed1cbb83002bca30084a60
# Parent  e9dae7e7bffcb15b1b5e5c695e142d9f4ee2cbbd
FiniteProjectiveCamera

diff -r e9dae7e7bffc -r 197a5ddaf68c Framework/Toolbox/FiniteProjectiveCamera.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Framework/Toolbox/FiniteProjectiveCamera.cpp	Wed Feb 14 11:29:26 2018 +0100
@@ -0,0 +1,221 @@
+/**
+ * Stone of Orthanc
+ * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
+ * Department, University Hospital of Liege, Belgium
+ * Copyright (C) 2017-2018 Osimis S.A., Belgium
+ *
+ * This program is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Affero General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Affero General Public License for more details.
+ * 
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ **/
+
+
+#include "FiniteProjectiveCamera.h"
+
+#include <Core/Logging.h>
+#include <Core/OrthancException.h>
+
+namespace OrthancStone
+{
+  void FiniteProjectiveCamera::ComputeMInverse()
+  {
+    using namespace boost::numeric::ublas;
+
+    // inv(M) = inv(K * R) = inv(R) * inv(K) = R' * inv(K). This
+    // matrix is always invertible, by definition of finite
+    // projective cameras (page 157).
+    Matrix kinv;
+    LinearAlgebra::InvertUpperTriangularMatrix(kinv, k_);
+    minv_ = prod(trans(r_), kinv);
+  }
+
+    
+  void FiniteProjectiveCamera::Setup(const Matrix& k,
+                                     const Matrix& r,
+                                     const Vector& c)
+  {
+    using namespace boost::numeric::ublas;
+
+    if (k.size1() != 3 ||
+        k.size2() != 3 ||
+        !LinearAlgebra::IsCloseToZero(k(1, 0)) ||
+        !LinearAlgebra::IsCloseToZero(k(2, 0)) ||
+        !LinearAlgebra::IsCloseToZero(k(2, 1)))
+    {
+      LOG(ERROR) << "Invalid intrinsic parameters";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+
+    if (r.size1() != 3 ||
+        r.size2() != 3)
+    {
+      LOG(ERROR) << "Invalid size for a 3D rotation matrix";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+
+    if (!LinearAlgebra::IsRotationMatrix(r, 100.0 * std::numeric_limits<float>::epsilon()))
+    {
+      LOG(ERROR) << "Invalid rotation matrix";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+
+    if (c.size() != 3)
+    {
+      LOG(ERROR) << "Invalid camera center";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+
+    k_ = k;
+    r_ = r;
+    c_ = c;
+
+    ComputeMInverse();
+      
+    Matrix tmp = identity_matrix<double>(3);
+    tmp.resize(3, 4);
+    tmp(0, 3) = -c[0];
+    tmp(1, 3) = -c[1];
+    tmp(2, 3) = -c[2];
+
+    tmp = prod(r, tmp);
+    p_ = prod(k, tmp);
+  }
+
+    
+  void FiniteProjectiveCamera::Setup(const Matrix& p)
+  {
+    using namespace boost::numeric::ublas;
+
+    if (p.size1() != 3 ||
+        p.size2() != 4)
+    {
+      LOG(ERROR) << "Invalid camera matrix";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+
+    p_ = p;
+
+    // M is the left 3x3 submatrix of "P"
+    Matrix m = p;
+    m.resize(3, 3);
+
+    // p4 is the last column of "P"
+    Vector p4(3);
+    p4[0] = p(0, 3);
+    p4[1] = p(1, 3);
+    p4[2] = p(2, 3);
+
+    // The RQ decomposition is explained on page 157
+    LinearAlgebra::RQDecomposition3x3(k_, r_, m);
+    ComputeMInverse();
+
+    c_ = prod(-minv_, p4);
+  }
+
+
+  FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9],
+                                                 const double r[9],
+                                                 const double c[3])
+  {
+    Matrix kk, rr;
+    Vector cc;
+
+    LinearAlgebra::FillMatrix(kk, 3, 3, k);
+    LinearAlgebra::FillMatrix(rr, 3, 3, r);
+    LinearAlgebra::FillVector(cc, 3, c);
+
+    Setup(kk, rr, cc);
+  }
+
+
+  FiniteProjectiveCamera::FiniteProjectiveCamera(const double p[12])
+  {
+    Matrix pp;
+    LinearAlgebra::FillMatrix(pp, 3, 4, p);
+    Setup(pp);
+  }
+
+
+  Vector FiniteProjectiveCamera::GetRayDirection(double x,
+                                                 double y) const
+  {
+    // This derives from Equation (6.14) on page 162, taking "mu =
+    // 1" and noticing that "-inv(M)*p4" corresponds to the camera
+    // center in finite projective cameras
+
+    // The (x,y) coordinates on the imaged plane, as an homogeneous vector
+    Vector xx(3);
+    xx[0] = x;
+    xx[1] = y;
+    xx[2] = 1.0;
+
+    return boost::numeric::ublas::prod(minv_, xx);
+  }
+
+
+
+  static Vector SetupApply(const Vector& v,
+                           bool infinityAllowed)
+  {
+    if (v.size() == 3)
+    {
+      // Vector "v" in non-homogeneous coordinates, add the homogeneous component
+      Vector vv;
+      LinearAlgebra::AssignVector(vv, v[0], v[1], v[2], 1.0);
+      return vv;
+    }
+    else if (v.size() == 4)
+    {
+      // Vector "v" is already in homogeneous coordinates
+
+      if (!infinityAllowed &&
+          LinearAlgebra::IsCloseToZero(v[3]))
+      {
+        LOG(ERROR) << "Cannot apply a finite projective camera to a "
+                   << "point at infinity with this method";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+      }
+
+      return v;
+    }
+    else
+    {
+      LOG(ERROR) << "The input vector must represent a point in 3D";
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+    }
+  }
+
+  
+  void FiniteProjectiveCamera::ApplyFinite(double& x,
+                                           double& y,
+                                           const Vector& v) const
+  {
+    Vector p = boost::numeric::ublas::prod(p_, SetupApply(v, false));    
+
+    if (LinearAlgebra::IsCloseToZero(p[2]))
+    {
+      // Point at infinity: Should not happen with a finite input point
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
+    }
+    else
+    {
+      x = p[0] / p[2];
+      y = p[1] / p[2];
+    }
+  }
+
+  
+  Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const
+  {
+    return boost::numeric::ublas::prod(p_, SetupApply(v, true));
+  }
+}
diff -r e9dae7e7bffc -r 197a5ddaf68c Framework/Toolbox/FiniteProjectiveCamera.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Framework/Toolbox/FiniteProjectiveCamera.h	Wed Feb 14 11:29:26 2018 +0100
@@ -0,0 +1,101 @@
+/**
+ * Stone of Orthanc
+ * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
+ * Department, University Hospital of Liege, Belgium
+ * Copyright (C) 2017-2018 Osimis S.A., Belgium
+ *
+ * This program is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Affero General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Affero General Public License for more details.
+ * 
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ **/
+
+
+#pragma once
+
+#include "LinearAlgebra.h"
+
+namespace OrthancStone
+{
+  // Reference: "Multiple View Geometry in Computer Vision (2nd Edition)"
+  class FiniteProjectiveCamera : public boost::noncopyable
+  {
+  private:
+    Matrix  p_;     // 3x4 matrix - Equation (6.11) - page 157
+    Matrix  k_;     // 3x3 matrix of intrinsic parameters - Equation (6.10) - page 157
+    Matrix  r_;     // 3x3 rotation matrix in 3D space
+    Vector  c_;     // 3x1 vector in 3D space corresponding to camera center
+    Matrix  minv_;  // Inverse of the M = P(1:3,1:3) submatrix
+
+    void ComputeMInverse();
+
+    void Setup(const Matrix& k,
+               const Matrix& r,
+               const Vector& c);
+
+    void Setup(const Matrix& p);
+
+  public:
+    FiniteProjectiveCamera(const Matrix& k,
+                           const Matrix& r,
+                           const Vector& c)
+    {
+      Setup(k, r, c);
+    }
+
+    FiniteProjectiveCamera(const Matrix& p)
+    {
+      Setup(p);
+    }
+
+    FiniteProjectiveCamera(const double k[9],
+                           const double r[9],
+                           const double c[3]);
+
+    FiniteProjectiveCamera(const double p[12]);
+
+    const Matrix& GetMatrix() const
+    {
+      return p_;
+    }
+
+    const Matrix& GetRotation() const
+    {
+      return r_;
+    }
+
+    const Vector& GetCenter() const
+    {
+      return c_;
+    }
+
+    const Matrix& GetIntrinsicParameters() const
+    {
+      return k_;
+    }
+
+    // Computes the 3D vector that represents the direction from the
+    // camera center to the (x,y) imaged point
+    Vector GetRayDirection(double x,
+                           double y) const;
+
+    // Apply the camera to a 3D point "v" that is not at infinity. "v"
+    // can be encoded either as a non-homogeneous vector (3
+    // components), or as a homogeneous vector (4 components).
+    void ApplyFinite(double& x,
+                     double& y,
+                     const Vector& v) const;
+
+    // Apply the camera to a 3D point "v" that is possibly at
+    // infinity. The result is a 2D point in homogeneous coordinates.
+    Vector ApplyGeneral(const Vector& v) const;
+  };
+}
diff -r e9dae7e7bffc -r 197a5ddaf68c Framework/Toolbox/LinearAlgebra.cpp
--- a/Framework/Toolbox/LinearAlgebra.cpp	Wed Feb 14 09:18:06 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.cpp	Wed Feb 14 11:29:26 2018 +0100
@@ -115,6 +115,20 @@
     }
 
 
+    void AssignVector(Vector& v,
+                      double v1,
+                      double v2,
+                      double v3,
+                      double v4)
+    {
+      v.resize(4);
+      v[0] = v1;
+      v[1] = v2;
+      v[2] = v3;
+      v[3] = v4;
+    }
+
+
     bool IsNear(double x,
                 double y)
     {
diff -r e9dae7e7bffc -r 197a5ddaf68c Framework/Toolbox/LinearAlgebra.h
--- a/Framework/Toolbox/LinearAlgebra.h	Wed Feb 14 09:18:06 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.h	Wed Feb 14 11:29:26 2018 +0100
@@ -60,6 +60,12 @@
                       double v2,
                       double v3);
 
+    void AssignVector(Vector& v,
+                      double v1,
+                      double v2,
+                      double v3,
+                      double v4);
+
     inline bool IsNear(double x,
                        double y,
                        double threshold)
diff -r e9dae7e7bffc -r 197a5ddaf68c Resources/CMake/OrthancStoneConfiguration.cmake
--- a/Resources/CMake/OrthancStoneConfiguration.cmake	Wed Feb 14 09:18:06 2018 +0100
+++ b/Resources/CMake/OrthancStoneConfiguration.cmake	Wed Feb 14 11:29:26 2018 +0100
@@ -180,6 +180,7 @@
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/DicomStructureSet.cpp
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/DownloadStack.cpp
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/Extent2D.cpp
+  ${ORTHANC_STONE_DIR}/Framework/Toolbox/FiniteProjectiveCamera.cpp
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/GeometryToolbox.cpp
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/LinearAlgebra.cpp
   ${ORTHANC_STONE_DIR}/Framework/Toolbox/MessagingToolbox.cpp
diff -r e9dae7e7bffc -r 197a5ddaf68c UnitTestsSources/UnitTestsMain.cpp
--- a/UnitTestsSources/UnitTestsMain.cpp	Wed Feb 14 09:18:06 2018 +0100
+++ b/UnitTestsSources/UnitTestsMain.cpp	Wed Feb 14 11:29:26 2018 +0100
@@ -25,6 +25,7 @@
 #include "../Framework/Layers/FrameRenderer.h"
 #include "../Framework/Layers/LayerSourceBase.h"
 #include "../Framework/Toolbox/DownloadStack.h"
+#include "../Framework/Toolbox/FiniteProjectiveCamera.h"
 #include "../Framework/Toolbox/OrthancSlicesLoader.h"
 #include "../Framework/Volumes/ImageBuffer3D.h"
 #include "../Framework/Volumes/SlicedVolumeBase.h"
@@ -136,11 +137,81 @@
 TEST(GeometryToolbox, Interpolation)
 {
   // https://en.wikipedia.org/wiki/Bilinear_interpolation#Application_in_image_processing
-  ASSERT_FLOAT_EQ(146.1f, OrthancStone::GeometryToolbox::ComputeBilinearInterpolation(20.5f, 14.2f, 91, 210, 162, 95));
+  ASSERT_FLOAT_EQ(146.1f, OrthancStone::GeometryToolbox::ComputeBilinearInterpolation
+                  (20.5f, 14.2f, 91, 210, 162, 95));
+
+  ASSERT_FLOAT_EQ(123.35f, OrthancStone::GeometryToolbox::ComputeTrilinearInterpolation
+                  (20.5f, 15.2f, 5.7f,
+                   91, 210, 162, 95,
+                   51, 190, 80, 92));
+}
+
+
+TEST(FiniteProjectiveCamera, Decomposition)
+{
+  // Example 6.2 of "Multiple View Geometry in Computer Vision - 2nd
+  // edition" (page 163)
+  const double p[12] = {
+    3.53553e+2,  3.39645e+2,  2.77744e+2, -1.44946e+6,
+    -1.03528e+2, 2.33212e+1,  4.59607e+2, -6.32525e+5,
+    7.07107e-1,  -3.53553e-1, 6.12372e-1, -9.18559e+2
+  };
+
+  OrthancStone::FiniteProjectiveCamera camera(p);
+  ASSERT_EQ(3u, camera.GetMatrix().size1());
+  ASSERT_EQ(4u, camera.GetMatrix().size2());
+  ASSERT_EQ(3u, camera.GetIntrinsicParameters().size1());
+  ASSERT_EQ(3u, camera.GetIntrinsicParameters().size2());
+  ASSERT_EQ(3u, camera.GetRotation().size1());
+  ASSERT_EQ(3u, camera.GetRotation().size2());
+  ASSERT_EQ(3u, camera.GetCenter().size());
+
+  ASSERT_NEAR(1000.0, camera.GetCenter()[0], 0.01);
+  ASSERT_NEAR(2000.0, camera.GetCenter()[1], 0.01);
+  ASSERT_NEAR(1500.0, camera.GetCenter()[2], 0.01);
 
-  ASSERT_FLOAT_EQ(123.35f, OrthancStone::GeometryToolbox::ComputeTrilinearInterpolation(20.5f, 15.2f, 5.7f,
-                                                                                        91, 210, 162, 95,
-                                                                                        51, 190, 80, 92));
+  ASSERT_NEAR(468.2, camera.GetIntrinsicParameters() (0, 0), 0.1);
+  ASSERT_NEAR(91.2,  camera.GetIntrinsicParameters() (0, 1), 0.1);
+  ASSERT_NEAR(300.0, camera.GetIntrinsicParameters() (0, 2), 0.1);
+  ASSERT_NEAR(427.2, camera.GetIntrinsicParameters() (1, 1), 0.1);
+  ASSERT_NEAR(200.0, camera.GetIntrinsicParameters() (1, 2), 0.1);
+  ASSERT_NEAR(1.0,   camera.GetIntrinsicParameters() (2, 2), 0.1);
+
+  ASSERT_NEAR(0, camera.GetIntrinsicParameters() (1, 0), 0.0000001);
+  ASSERT_NEAR(0, camera.GetIntrinsicParameters() (2, 0), 0.0000001);
+  ASSERT_NEAR(0, camera.GetIntrinsicParameters() (2, 1), 0.0000001);
+
+  ASSERT_NEAR(0.41380,  camera.GetRotation() (0, 0), 0.00001);
+  ASSERT_NEAR(0.90915,  camera.GetRotation() (0, 1), 0.00001);
+  ASSERT_NEAR(0.04708,  camera.GetRotation() (0, 2), 0.00001);
+  ASSERT_NEAR(-0.57338, camera.GetRotation() (1, 0), 0.00001);
+  ASSERT_NEAR(0.22011,  camera.GetRotation() (1, 1), 0.00001);
+  ASSERT_NEAR(0.78917,  camera.GetRotation() (1, 2), 0.00001);
+  ASSERT_NEAR(0.70711,  camera.GetRotation() (2, 0), 0.00001);
+  ASSERT_NEAR(-0.35355, camera.GetRotation() (2, 1), 0.00001);
+  ASSERT_NEAR(0.61237,  camera.GetRotation() (2, 2), 0.00001);
+
+  ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation()));
+
+  OrthancStone::FiniteProjectiveCamera camera2(camera.GetIntrinsicParameters(),
+                                               camera.GetRotation(),
+                                               camera.GetCenter());
+  ASSERT_EQ(3u, camera2.GetMatrix().size1());
+  ASSERT_EQ(4u, camera2.GetMatrix().size2());
+  ASSERT_EQ(3u, camera2.GetIntrinsicParameters().size1());
+  ASSERT_EQ(3u, camera2.GetIntrinsicParameters().size2());
+  ASSERT_EQ(3u, camera2.GetRotation().size1());
+  ASSERT_EQ(3u, camera2.GetRotation().size2());
+  ASSERT_EQ(3u, camera2.GetCenter().size());
+
+  for (size_t i = 0; i < 3; i++)
+  {
+    for (size_t j = 0; j < 4; j++)
+    {
+      ASSERT_NEAR(camera.GetMatrix() (i, j),
+                  camera2.GetMatrix() (i, j), 0.00000001);
+    }
+  }
 }