From 439ba98e0a14a0f382e58720f5601854e3c1330a Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 5 Aug 2013 13:01:21 +0200 Subject: [PATCH] initial implementation of projection matrix, 3D to window coordinates, window coordinates to 3D ray (not yet tested) --- modules/viz/include/opencv2/viz/types.hpp | 2 ++ modules/viz/include/opencv2/viz/viz3d.hpp | 3 ++ modules/viz/src/types.cpp | 23 ++++++++++++ modules/viz/src/viz3d.cpp | 3 ++ modules/viz/src/viz3d_impl.cpp | 44 ++++++++++++++++++++--- modules/viz/src/viz3d_impl.hpp | 6 ++-- 6 files changed, 73 insertions(+), 8 deletions(-) diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index 33b22325f2..8dd68196b4 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/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_; diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index e173fcfca0..eff8ac000b 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/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); diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp index 827b8b3bc9..e32cf54cb1 100644 --- a/modules/viz/src/types.cpp +++ b/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 diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 8aa97419f7..2c67025809 100644 --- a/modules/viz/src/viz3d.cpp +++ b/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 diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index f764c77558..70d1e6fee3 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/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 () { diff --git a/modules/viz/src/viz3d_impl.hpp b/modules/viz/src/viz3d_impl.hpp index 5bd4ebdea6..3b3018ff28 100644 --- a/modules/viz/src/viz3d_impl.hpp +++ b/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);