From bb057491eae3ca3b1e39b2d4892f95c0fb06b27e Mon Sep 17 00:00:00 2001
From: ozantonkal <ozantonkal@gmail.com>
Date: Thu, 8 Aug 2013 12:14:27 +0200
Subject: [PATCH] create camera from projection matrix (used for getCamera in
 viz)

---
 modules/viz/include/opencv2/viz/types.hpp |  1 +
 modules/viz/src/types.cpp                 | 41 +++++++++++++++++++++--
 modules/viz/src/viz3d_impl.cpp            |  8 ++---
 3 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp
index d12a4df13f..54b47140eb 100644
--- a/modules/viz/include/opencv2/viz/types.hpp
+++ b/modules/viz/include/opencv2/viz/types.hpp
@@ -100,6 +100,7 @@ namespace cv
             Camera(float f_x, float f_y, float c_x, float c_y, const Size &window_size);
             Camera(const Vec2f &fov, const Size &window_size);
             Camera(const cv::Matx33f &K, const Size &window_size);
+            Camera(const cv::Matx44f &proj, const Size &window_size);
             
             inline const Vec2d & getClip() const { return clip_; }
             inline void setClip(const Vec2d &clip) { clip_ = clip; }
diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp
index 631582a646..ad9eb2731a 100644
--- a/modules/viz/src/types.cpp
+++ b/modules/viz/src/types.cpp
@@ -170,6 +170,43 @@ cv::viz::Camera::Camera(const cv::Matx33f & K, const Size &window_size)
     init(f_x, f_y, c_x, c_y, window_size);
 }
 
+cv::viz::Camera::Camera(const Matx44f &proj, const Size &window_size)
+{   
+    double near = proj(2,3) / (proj(2,2) - 1.0);
+    double far = near * (proj(2,2) - 1.0) / (proj(2,2) + 1.0);
+    double left = near * (proj(0,2)-1) / proj(0,0);
+    double right = 2.0 * near / proj(0,0) + left;
+    double bottom = near * (proj(1,2)-1) / proj(1,1);
+    double top = 2.0 * near / proj(1,1) + bottom;
+    
+    if (fabs(left-right) < std::numeric_limits<double>::epsilon()) 
+    { 
+        principal_point_[0] = -1.0f; 
+        focal_[0] = -1.0f;
+    }
+    else 
+    { 
+        principal_point_[0] = (left * static_cast<float>(window_size.width)) / (left - right); 
+        focal_[0] = - near * principal_point_[0] / left;
+    }
+    
+    if (fabs(top-bottom) < std::numeric_limits<double>::epsilon()) 
+    { 
+        principal_point_[1] = -1.0f; 
+        focal_[1] = -1.0f;
+    }
+    else 
+    { 
+        principal_point_[1] = (top * static_cast<float>(window_size.height)) / (top - bottom); 
+        focal_[1] = near * principal_point_[1] / top;
+    }
+    
+    setClip(Vec2d(near, far));
+    // Set the vertical field of view
+    fov_[1] = (atan2(principal_point_[1],focal_[1]) + atan2(window_size.height-principal_point_[1],focal_[1]));
+    setWindowSize(window_size);
+}
+
 void cv::viz::Camera::init(float f_x, float f_y, float c_x, float c_y, const Size &window_size)
 {
     CV_Assert(window_size.width > 0 && window_size.height > 0);
@@ -220,7 +257,7 @@ void cv::viz::Camera::computeProjectionMatrix(Matx44f &proj) const
     double left   = -(top * window_size_.width) / window_size_.height;
     double right  = -left;
     double bottom = -top;
-    // If principal point is defined
+    // If principal point is defined (i.e intrinsic parameters are known)
     if (principal_point_[0] > 0.0f)
     {
         top = clip_[0] * principal_point_[1] / focal_[1];
@@ -233,7 +270,7 @@ void cv::viz::Camera::computeProjectionMatrix(Matx44f &proj) const
     double temp2 = 1.0 / (right - left);
     double temp3 = 1.0 / (top - bottom);
     double temp4 = 1.0 / (clip_[0] - clip_[1]);
-
+    
     proj = Matx44d::zeros();
     proj(0,0) = temp1 * temp2;
     proj(1,1) = temp1 * temp3;
diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp
index cc732f6d7e..a236b683c9 100644
--- a/modules/viz/src/viz3d_impl.cpp
+++ b/modules/viz/src/viz3d_impl.cpp
@@ -577,7 +577,7 @@ void cv::viz::Viz3d::VizImpl::setCamera(const Camera &camera)
     // Use the intrinsic parameters of the camera to simulate more realistically
     Matx44f proj_matrix;
     camera.computeProjectionMatrix(proj_matrix);
-    Matx44f old_proj_matrix = convertToMatx(active_camera.GetProjectionTransformMatrix(((float)camera.getWindowSize().width) / camera.getWindowSize().height, -1.0f, 1.0f));
+    Matx44f old_proj_matrix = convertToMatx(active_camera.GetProjectionTransformMatrix(static_cast<float>(camera.getWindowSize().width) / static_cast<float>(camera.getWindowSize().height), -1.0f, 1.0f));
     vtkTransform * transform = vtkTransform::New();
     // This is a hack around not being able to set Projection Matrix
     transform->SetMatrix(convertToVtkMatrix(proj_matrix * old_proj_matrix.inv())); 
@@ -594,9 +594,9 @@ cv::viz::Camera cv::viz::Viz3d::VizImpl::getCamera() const
     Vec2d clip(active_camera.GetClippingRange());
     Size window_size(renderer_->GetRenderWindow()->GetSize()[0],
                      renderer_->GetRenderWindow()->GetSize()[1]);
-    
-    Camera camera(fov, window_size);
-    camera.setClip(clip);
+    Matx44f old_proj_matrix = convertToMatx(active_camera.GetProjectionTransformMatrix(((float)window_size.width) / window_size.height, -1.0f, 1.0f));
+    Camera camera(old_proj_matrix, window_size);
+//     camera.setClip(clip);
     return camera;
 }
 
-- 
2.34.1