initial implementation of projection matrix, 3D to window coordinates, window…

initial implementation of projection matrix, 3D to window coordinates, window coordinates to 3D ray (not yet tested)

modules/viz/include/opencv2/viz/types.hpp

@@ -110,6 +110,8 @@ namespace cv
             inline const Vec2f & getFov() const { return fov_; }
             inline void setFov(const Vec2f & fov) { fov_ = fov; }
             
+            void computeProjectionMatrix(Matx44f &proj) const;
+            
         private:
             Vec2d clip_;
             Vec2f fov_;

modules/viz/include/opencv2/viz/viz3d.hpp

@@ -42,6 +42,9 @@ namespace cv
             void setCamera(const Camera2 &camera);
             Affine3f getViewerPose();
             void setViewerPose(const Affine3f &pose);
+            
+            void convertToWindowCoordinates(const Point3f &pt, Point3f &window_coord);
+            void converTo3DRay(const Point3f &window_coord, Point3f &origin, Vec3f &direction);
 
             void spin();
             void spinOnce(int time = 1, bool force_redraw = false);

modules/viz/src/types.cpp

@@ -196,3 +196,26 @@ void cv::viz::Camera2::setWindowSize(const Size &window_size)
     else
         fov_[0] = (atan2(principal_point_[0],focal_[0]) + atan2(window_size.width-principal_point_[0],focal_[0])) * 180 / CV_PI;
 }
+
+void cv::viz::Camera2::computeProjectionMatrix(Matx44f &proj) const
+{
+    double top    = clip_[0] * tan (0.5 * fov_[1]);
+    double left   = -(top * window_size_.width) / window_size_.height;
+    double right  = -left;
+    double bottom = -top;
+
+    double temp1 = 2.0 * clip_[0];
+    double temp2 = 1.0 / (right - left);
+    double temp3 = 1.0 / (top - bottom);
+    double temp4 = 1.0 / clip_[1] - clip_[0];
+
+    proj = Matx44d::zeros();
+
+    proj(0,0) = temp1 * temp2;
+    proj(1,1) = temp1 * temp3;
+    proj(0,2) = (right + left) * temp2;
+    proj(1,2) = (top + bottom) * temp3;
+    proj(2,2) = (-clip_[1] - clip_[0]) * temp4;
+    proj(3,2) = -1.0;
+    proj(2,3) = (-temp1 * clip_[1]) * temp4;
+}
\ No newline at end of file

modules/viz/src/viz3d.cpp

@@ -49,3 +49,6 @@ cv::Affine3f cv::viz::Viz3d::getWidgetPose(const String &id) const { return impl
 void cv::viz::Viz3d::setCamera(const Camera2 &camera) { impl_->setCamera(camera); }
 void cv::viz::Viz3d::setViewerPose(const Affine3f &pose) { impl_->setViewerPose(pose); }
 cv::Affine3f cv::viz::Viz3d::getViewerPose() { return impl_->getViewerPose(); }
+
+void cv::viz::Viz3d::convertToWindowCoordinates(const Point3f &pt, Point3f &window_coord) { impl_->convertToWindowCoordinates(pt, window_coord); }
+void cv::viz::Viz3d::converTo3DRay(const Point3f &window_coord, Point3f &origin, Vec3f &direction) { impl_->converTo3DRay(window_coord, origin, direction); }
\ No newline at end of file

modules/viz/src/viz3d_impl.cpp

@@ -604,13 +604,18 @@ void cv::viz::Viz3d::VizImpl::setCamera(const Camera2 &camera)
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////
-void cv::viz::Viz3d::VizImpl::getCamera(viz::Camera2 &camera)
+cv::viz::Camera2 cv::viz::Viz3d::VizImpl::getCamera() const
 {
     vtkCamera& active_camera = *renderer_->GetActiveCamera();
-    camera.setFov(Vec2f(0.0, active_camera.GetViewAngle() * CV_PI / 180.0f));
-    camera.setClip(Vec2d(active_camera.GetClippingRange()));
-    camera.setWindowSize(Size(renderer_->GetRenderWindow()->GetSize()[0],
-                              renderer_->GetRenderWindow()->GetSize()[1]));
+    
+    Vec2f fov(0.0, active_camera.GetViewAngle() * CV_PI / 180.0f);
+    Vec2d clip(active_camera.GetClippingRange());
+    Size window_size(renderer_->GetRenderWindow()->GetSize()[0],
+                     renderer_->GetRenderWindow()->GetSize()[1]);
+    
+    Camera2 camera(fov, window_size);
+    camera.setClip(clip);
+    return camera;
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////
@@ -664,6 +669,35 @@ cv::Affine3f cv::viz::Viz3d::VizImpl::getViewerPose ()
     return cv::Affine3f(R, pos);
 }
 
+/////////////////////////////////////////////////////////////////////////////////////////////
+void cv::viz::Viz3d::VizImpl::convertToWindowCoordinates(const Point3f &pt, Point3f &window_coord)
+{
+    // Use the built in function of renderer
+    double point_wcs[3] = {pt.x, pt.y, pt.z};
+    renderer_->WorldToView(point_wcs[0], point_wcs[1], point_wcs[2]);
+    window_coord.x = point_wcs[0];
+    window_coord.y = point_wcs[1];
+    window_coord.z = point_wcs[2];
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void cv::viz::Viz3d::VizImpl::converTo3DRay(const Point3f &window_coord, Point3f &origin, Vec3f &direction)
+{
+    // Use the built in function of renderer
+    double point_view[3] = {window_coord.x, window_coord.y, window_coord.z};
+    renderer_->ViewToWorld(point_view[0], point_view[1], point_view[2]);
+    
+    vtkCamera &active_camera = *renderer_->GetActiveCamera();
+    double *cam_pos = active_camera.GetPosition();
+    origin.x = cam_pos[0];
+    origin.y = cam_pos[1];
+    origin.z = cam_pos[2];
+    direction[0] = point_view[0] - cam_pos[0];
+    direction[1] = point_view[1] - cam_pos[1];
+    direction[2] = point_view[2] - cam_pos[2];
+    normalize(direction);
+}
+
 /////////////////////////////////////////////////////////////////////////////////////////////
 void cv::viz::Viz3d::VizImpl::resetCamera ()
 {

modules/viz/src/viz3d_impl.hpp

@@ -111,7 +111,7 @@ public:
     // and 'Camera' class itself with various constructors/fields
     
     void setCamera(const Camera2 &camera);
-    void getCamera(Camera2 &camera);
+    Camera2 getCamera() const;
 
     void initCameraParameters (); /** \brief Initialize camera parameters with some default values. */
     bool cameraParamsSet () const; /** \brief Checks whether the camera parameters were manually loaded from file.*/
@@ -146,8 +146,8 @@ public:
     void setViewerPose(const Affine3f &pose);
     Affine3f getViewerPose();
 
-
-
+    void convertToWindowCoordinates(const Point3f &pt, Point3f &window_coord);
+    void converTo3DRay(const Point3f &window_coord, Point3f &origin, Vec3f &direction);