initial CameraPositionWidget implementation using eye, look_at, up_vector

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

 #pragma once
 
 #include <opencv2/viz/types.hpp>
+#include <common.h>
 
 
 namespace cv
@@ -162,6 +163,13 @@ namespace cv
         private:
             struct CopyImpl;
         };
+        
+        class CV_EXPORTS CameraPositionWidget : public Widget3D
+        {
+        public:
+            CameraPositionWidget(const Vec3f &position, const Vec3f &look_at, const Vec3f &up_vector, double scale = 1.0);
+            
+        };
 
         class CV_EXPORTS CloudWidget : public Widget3D
         {

modules/viz/src/shape_widgets.cpp

@@ -774,4 +774,74 @@ template<> cv::viz::Image3DWidget cv::viz::Widget::cast<cv::viz::Image3DWidget>(
 {
     Widget3D widget = this->cast<Widget3D>();
     return static_cast<Image3DWidget&>(widget);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// camera position widget implementation
+cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec3f &position, const Vec3f &look_at, const Vec3f &up_vector, double scale)
+{
+    vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
+    axes->SetOrigin (0, 0, 0);
+    axes->SetScaleFactor (scale);
+    
+    // Compute the transformation matrix for drawing the camera frame in a scene
+    Vec3f u,v,n;
+    n = normalize(look_at - position);
+    u = normalize(up_vector.cross(n));
+    v = n.cross(u);
+    
+    vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+    mat_trans->SetElement(0,0,u[0]);
+    mat_trans->SetElement(0,1,u[1]);
+    mat_trans->SetElement(0,2,u[2]);
+    mat_trans->SetElement(1,0,v[0]);
+    mat_trans->SetElement(1,1,v[1]);
+    mat_trans->SetElement(1,2,v[2]);
+    mat_trans->SetElement(2,0,n[0]);
+    mat_trans->SetElement(2,1,n[1]);
+    mat_trans->SetElement(2,2,n[2]);
+    // Inverse rotation (orthogonal, so just take transpose)
+    mat_trans->Transpose(); 
+    // Then translate the coordinate frame to camera position
+    mat_trans->SetElement(0,3,position[0]);
+    mat_trans->SetElement(1,3,position[1]);
+    mat_trans->SetElement(2,3,position[2]);
+    mat_trans->SetElement(3,3,1);
+
+    vtkSmartPointer<vtkFloatArray> axes_colors = vtkSmartPointer<vtkFloatArray>::New ();
+    axes_colors->Allocate (6);
+    axes_colors->InsertNextValue (0.0);
+    axes_colors->InsertNextValue (0.0);
+    axes_colors->InsertNextValue (0.5);
+    axes_colors->InsertNextValue (0.5);
+    axes_colors->InsertNextValue (1.0);
+    axes_colors->InsertNextValue (1.0);
+
+    vtkSmartPointer<vtkPolyData> axes_data = axes->GetOutput ();
+    axes_data->Update ();
+    axes_data->GetPointData ()->SetScalars (axes_colors);
+    
+    // Transform the default coordinate frame
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->SetMatrix(mat_trans);
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    filter->SetInput(axes_data);
+    filter->SetTransform(transform);
+    filter->Update();
+    
+    vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
+    axes_tubes->SetInput (filter->GetOutput());
+    axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
+    axes_tubes->SetNumberOfSides (6);
+    
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetScalarModeToUsePointData ();
+    mapper->SetInput(axes_tubes->GetOutput ());
+
+    vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
 }
\ No newline at end of file

modules/viz/test/test_viz3d.cpp

@@ -43,6 +43,7 @@
 #include <opencv2/viz.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
 
 #include <fstream>
 #include <string>
@@ -89,7 +90,7 @@ TEST(Viz_viz3d, accuracy)
 
     viz::LineWidget lw(Point3f(0, 0, 0), Point3f(4.f, 4.f,4.f), viz::Color::green());
     viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0), 5.0);
-    viz::SphereWidget sw(Point3f(0, 0, 0), 0.5);
+    viz::SphereWidget sw(Point3f(0, 0, 0), 0.2);
     viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), 0.01, viz::Color::red());
     viz::CircleWidget cw(Point3f(0, 0, 0), 0.5, 0.01, viz::Color::green());
     viz::CylinderWidget cyw(Point3f(0, 0, 0), Point3f(-1, -1, -1), 0.5, 30, viz::Color::green());
@@ -99,18 +100,18 @@ TEST(Viz_viz3d, accuracy)
     viz::CloudWidget pcw(cloud, colors);
     viz::CloudWidget pcw2(cloud, viz::Color::magenta());
     
-    viz.showWidget("line", lw);
-    viz.showWidget("plane", pw);
+//     viz.showWidget("line", lw);
+//     viz.showWidget("plane", pw);
     viz.showWidget("sphere", sw);
-    viz.showWidget("arrow", aw);
-    viz.showWidget("circle", cw);
-    viz.showWidget("cylinder", cyw);
-    viz.showWidget("cube", cuw);
+//     viz.showWidget("arrow", aw);
+//     viz.showWidget("circle", cw);
+//     viz.showWidget("cylinder", cyw);
+//     viz.showWidget("cube", cuw);
     viz.showWidget("coordinateSystem", csw);
-    viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
-    viz.showWidget("text",tw);
-    viz.showWidget("pcw",pcw);
-    viz.showWidget("pcw2",pcw2);
+//     viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
+//     viz.showWidget("text",tw);
+//     viz.showWidget("pcw",pcw);
+//     viz.showWidget("pcw2",pcw2);
     
 //     viz::LineWidget lw2 = lw;
 //     v.showPointCloud("cld",cloud, colors);
@@ -125,13 +126,26 @@ TEST(Viz_viz3d, accuracy)
     
     
     viz::PolyLineWidget plw(points, viz::Color::green());
-    viz.showWidget("polyline", plw);
+//     viz.showWidget("polyline", plw);
 //     lw = v.getWidget("polyline").cast<viz::LineWidget>();
     
-    viz::Mesh3d::Ptr mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
+    viz::Mesh3d mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
     
-    viz::MeshWidget mw(*mesh);
-    viz.showWidget("mesh", mw);
+    viz::MeshWidget mw(mesh);
+//     viz.showWidget("mesh", mw);
+    
+    Mat img = imread("opencv.png");
+//     resize(img, img, Size(50,50));
+//     viz.showWidget("img", viz::ImageOverlayWidget(img, Point2i(50,50)));
+    
+    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::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1);
+    
+    
+    viz.showWidget("CameraPositionWidget", cpw);
+    viz.showWidget("camera_label", t3w1);
+//     viz.showWidget("CameraPositionWidget2", cpw2);
+//     viz.showWidget("CameraPositionWidget3", cpw3);
     
     viz.spin();
 
@@ -156,13 +170,14 @@ TEST(Viz_viz3d, accuracy)
         
         //plw.setColor(viz::Color(col_blue, col_green, col_red));
         
-        sw.setPose(cloudPosition);
+//         sw.setPose(cloudPosition);
 //         pw.setPose(cloudPosition);
         aw.setPose(cloudPosition);
         cw.setPose(cloudPosition);
         cyw.setPose(cloudPosition);
 //         lw.setPose(cloudPosition);
-        cuw.setPose(cloudPosition);
+//         cpw.updatePose(Affine3f(0.1,0.0,0.0, cv::Vec3f(0.0,0.0,0.0)));
+//         cpw.setPose(cloudPosition);
 //         cnw.setPose(cloudPosition);
 //         v.showWidget("pcw",pcw, cloudPosition);
 //         v.showWidget("pcw2",pcw2, cloudPosition2);