initial setViewerPose implementation

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

@@ -38,6 +38,9 @@ namespace cv
             void setWidgetPose(const String &id, const Affine3f &pose);
             void updateWidgetPose(const String &id, const Affine3f &pose);
             Affine3f getWidgetPose(const String &id) const;
+            
+            Affine3f getViewerPose();
+            void setViewerPose(const Affine3f &pose);
 
             void spin();
             void spinOnce(int time = 1, bool force_redraw = false);

modules/viz/src/viz3d.cpp

@@ -45,3 +45,6 @@ cv::viz::Widget cv::viz::Viz3d::getWidget(const String &id) const { return impl_
 void cv::viz::Viz3d::setWidgetPose(const String &id, const Affine3f &pose) { impl_->setWidgetPose(id, pose); }
 void cv::viz::Viz3d::updateWidgetPose(const String &id, const Affine3f &pose) { impl_->updateWidgetPose(id, pose); }
 cv::Affine3f cv::viz::Viz3d::getWidgetPose(const String &id) const { return impl_->getWidgetPose(id); }
+
+void cv::viz::Viz3d::setViewerPose(const Affine3f &pose) { impl_->setViewerPose(pose); }
+cv::Affine3f cv::viz::Viz3d::getViewerPose() { return impl_->getViewerPose(); }

modules/viz/src/viz3d_impl.cpp

@@ -591,6 +591,28 @@ void cv::viz::Viz3d::VizImpl::getCameras (cv::viz::Camera& camera)
     camera.window_pos = cv::Vec2d::all(0);
 }
 
+/////////////////////////////////////////////////////////////////////////////////////////////
+void cv::viz::Viz3d::VizImpl::setViewerPose(const Affine3f &pose)
+{
+    vtkCamera& camera = *renderer_->GetActiveCamera ();
+        
+    // Position = extrinsic translation
+    cv::Vec3f pos_vec = pose.translation();
+
+    // Rotate the view vector
+    cv::Matx33f rotation = pose.rotation();
+    cv::Vec3f y_axis (0.f, 1.f, 0.f);
+    cv::Vec3f up_vec (rotation * y_axis);
+
+    // Compute the new focal point
+    cv::Vec3f z_axis (0.f, 0.f, 1.f);
+    cv::Vec3f focal_vec = pos_vec + rotation * z_axis;
+    
+    camera.SetPosition(pos_vec[0], pos_vec[1], pos_vec[2]);
+    camera.SetFocalPoint(focal_vec[0], focal_vec[1], focal_vec[2]);
+    camera.SetViewUp(up_vec[0], up_vec[1], up_vec[2]);
+}
+
 /////////////////////////////////////////////////////////////////////////////////////////////
 cv::Affine3f cv::viz::Viz3d::VizImpl::getViewerPose ()
 {

modules/viz/src/viz3d_impl.hpp

@@ -140,6 +140,7 @@ public:
     void getCameras (Camera& camera);
 
     //to implement Viz3d set/getViewerPose()
+    void setViewerPose(const Affine3f &pose);
     Affine3f getViewerPose();