From a76cc9ef88a3876da371faa4224281ccff6cf1e9 Mon Sep 17 00:00:00 2001
From: ozantonkal <ozantonkal@gmail.com>
Date: Fri, 2 Aug 2013 16:33:30 +0200
Subject: [PATCH] initial camera implementation (camera2), fix bug (zeros
 method)

---
 modules/viz/include/opencv2/viz/types.hpp | 25 +++++++++++
 modules/viz/src/types.cpp                 | 54 +++++++++++++++++++++++
 2 files changed, 79 insertions(+)

diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp
index b0e3efda3d..4ef05a68a2 100644
--- a/modules/viz/include/opencv2/viz/types.hpp
+++ b/modules/viz/include/opencv2/viz/types.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <string>
+#include <boost/concept_check.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/core/affine.hpp>
 
@@ -93,6 +94,30 @@ namespace cv
             Point pointer;
             unsigned int key_state;
         };
+        
+        class CV_EXPORTS Camera2
+        {
+        public:
+            Camera2(float f_x, float f_y, float c_x, float c_y, const Size &window_size);
+            Camera2(const Vec2f &fov, const Size &window_size);
+            Camera2(const cv::Mat &K, const Size &window_size);
+            
+            inline const Vec2d & getClip() const { return clip_; }
+            inline void setClip(const Vec2d &clip) { clip_ = clip; }
+            
+            inline const Size & getWindowSize() const { return window_size_; }
+            void setWindowSize(const Size &window_size);
+            
+            inline const Vec2f & getFov() const { return fov_; }
+            inline void setFov(const Vec2f & fov) { fov_ = fov; }
+            
+        private:
+            Vec2d clip_;
+            Vec2f fov_;
+            Size window_size_;
+            Vec2f principal_point_;
+            Vec2f focal_;
+        };
 
     } /* namespace viz */
 } /* namespace cv */
diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp
index 871c2dbcb6..4ea1b32d37 100644
--- a/modules/viz/src/types.cpp
+++ b/modules/viz/src/types.cpp
@@ -142,3 +142,57 @@ cv::viz::Mesh3d cv::viz::Mesh3d::loadMesh(const String& file)
 {
     return loadMeshImpl::loadMesh(file);
 }
+
+////////////////////////////////////////////////////////////////////
+/// Camera implementation
+
+cv::viz::Camera2::Camera2(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);
+    setClip(Vec2d(0.01, 1000.01));// Default clipping
+    
+    fov_[0] = (atan2(c_x,f_x) + atan2(window_size.width-c_x,f_x)) * 180 / CV_PI;
+    fov_[1] = (atan2(c_y,f_y) + atan2(window_size.height-c_y,f_y)) * 180 / CV_PI;
+    
+    principal_point_[0] = c_x;
+    principal_point_[1] = c_y;
+    
+    focal_[0] = f_x;
+    focal_[1] = f_y;
+}
+
+cv::viz::Camera2::Camera2(const Vec2f &fov, const Size &window_size)
+{
+    CV_Assert(window_size.width > 0 && window_size.height > 0);
+    setClip(Vec2d(0.01, 1000.01)); // Default clipping
+    window_size_ = window_size;
+    fov_ = fov;
+    principal_point_ = Vec2f(-1.0f, -1.0f); // Symmetric lens
+    focal_ = Vec2f(-1.0f, -1.0f);
+}
+
+cv::viz::Camera2::Camera2(const cv::Mat & K, const Size &window_size)
+{
+    CV_Assert(K.rows == 3 && K.cols == 3);
+    CV_Assert(window_size.width > 0 && window_size.height > 0);
+    
+    float f_x = K.at<float>(0,0);
+    float f_y = K.at<float>(1,1);
+    float c_x = K.at<float>(0,2);
+    float c_y = K.at<float>(1,2);
+    Camera2(f_x, f_y, c_x, c_y, window_size);
+}
+
+void cv::viz::Camera2::setWindowSize(const Size &window_size)
+{
+    CV_Assert(window_size.width > 0 && window_size.height > 0);
+    
+    // Vertical field of view is fixed! 
+    // Horizontal field of view is expandable based on the aspect ratio
+    float aspect_ratio_new = window_size.width / window_size.height;
+    
+    if (principal_point_[0] < 0.0f)
+        fov_[0] = 2 * atan2(tan(fov_[0] * 0.5), aspect_ratio_new); // This assumes that the lens is symmetric!
+    else
+        fov_[0] = (atan2(principal_point_[0],focal_[0]) + atan2(window_size.width-principal_point_[0],focal_[0])) * 180 / CV_PI;
+}