fix aspect_ratio in camera position widget, add scale parameter (tentative implementation of scale)

modules/viz/include/opencv2/viz/widgets.hpp

@@ -169,7 +169,7 @@ namespace cv
         public:
             CameraPositionWidget(double scale = 1.0);
             CameraPositionWidget(const Vec3f &position, const Vec3f &look_at, const Vec3f &up_vector, double scale = 1.0);
-            CameraPositionWidget(const Matx33f &K, const Color &color = Color::white());
+            CameraPositionWidget(const Matx33f &K, double scale = 1.0, const Color &color = Color::white());
         };
 
         class CV_EXPORTS CloudWidget : public Widget3D

modules/viz/src/shape_widgets.cpp

@@ -881,22 +881,21 @@ cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec3f &position, const
     WidgetAccessor::setProp(*this, actor);
 }
 
-cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, const Color &color)
+cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, double scale, const Color &color)
 {
     vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
-    float focal_length = K(0,0);
-    float c_x = K(0,2);
+    float f_x = K(0,0);
+    float f_y = K(1,1);
     float c_y = K(1,2);
-    float aspect_ratio = c_x / c_y;
-    float img_width = c_x * 2;
-    float img_height = c_y * 2;
+    float aspect_ratio = f_y / f_x;
     // Assuming that this is an ideal camera (c_y and c_x are at the center of the image)
-    float fovy = 2.0f * atan2(c_y,focal_length) * 180 / CV_PI;
+    float fovy = 2.0f * atan2(c_y,f_y) * 180 / CV_PI;
     
     camera->SetViewAngle(fovy);
     camera->SetPosition(0.0,0.0,0.0);
     camera->SetViewUp(0.0,1.0,0.0);
     camera->SetFocalPoint(0.0,0.0,1.0);
+    camera->SetClippingRange(0.01, scale * f_y * 0.001);
     
     double planesArray[24];
     camera->GetFrustumPlanes(aspect_ratio, planesArray);

modules/viz/test/test_viz3d.cpp

@@ -144,7 +144,8 @@ TEST(Viz_viz3d, accuracy)
     
     viz::CameraPositionWidget cpw(Vec3f(0.5, 0.5, 3.0), Vec3f(0.0,0.0,0.0), Vec3f(0.0,-1.0,0.0), 0.5);
     viz::CameraPositionWidget cpw2(0.5);
-    viz::CameraPositionWidget frustum(K, viz::Color::green());
+    viz::CameraPositionWidget frustum(K, 2.0, viz::Color::green());
+    viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
     viz::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1);
     viz::Text3DWidget t3w2("Camera2", Point3f(0,0,0), 0.1);
     
@@ -153,6 +154,7 @@ TEST(Viz_viz3d, accuracy)
     viz.showWidget("camera_label", t3w1);
     viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
     viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+    viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
     
 //     viz.showWidget("CameraPositionWidget2", cpw2);
 //     viz.showWidget("CameraPositionWidget3", cpw3);