refactored frustum trajectory to use glyphs

modules/viz/doc/widget.rst

@@ -762,9 +762,9 @@ This 3D Widget represents a trajectory. ::
     {
     public:
         //! Displays trajectory of the given path by frustums
-        WTrajectoryFrustums(const std::vector<Affine3d> &path, const Matx33d &K, double scale = 1.0, const Color &color = Color::white());
+        WTrajectoryFrustums(InputArray path, const Matx33d &K, double scale = 1.0, const Color &color = Color::white());
         //! Displays trajectory of the given path by frustums
-        WTrajectoryFrustums(const std::vector<Affine3d> &path, const Vec2d &fov, double scale = 1.0, const Color &color = Color::white());
+        WTrajectoryFrustums(InputArray path, const Vec2d &fov, double scale = 1.0, const Color &color = Color::white());
     };
 
 viz::WTrajectoryFrustums::WTrajectoryFrustums
@@ -773,7 +773,7 @@ Constructs a WTrajectoryFrustums.
 
 .. ocv:function:: WTrajectoryFrustums(const std::vector<Affine3d> &path, const Matx33d &K, double scale = 1.0, const Color &color = Color::white())
 
-    :param path: List of poses on a trajectory.
+    :param path: List of poses on a trajectory. Takes std::vector<Affine<T>> with T == [float | double]
     :param K: Intrinsic matrix of the camera.
     :param scale: Scale of the frustums.
     :param color: :ocv:class:`Color` of the frustums.
@@ -782,7 +782,7 @@ Constructs a WTrajectoryFrustums.
 
 .. ocv:function:: WTrajectoryFrustums(const std::vector<Affine3d> &path, const Vec2d &fov, double scale = 1.0, const Color &color = Color::white())
 
-    :param path: List of poses on a trajectory.
+    :param path: List of poses on a trajectory. Takes std::vector<Affine<T>> with T == [float | double]
     :param fov: Field of view of the camera (horizontal, vertical).
     :param scale: Scale of the frustums.
     :param color: :ocv:class:`Color` of the frustums.

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

@@ -269,10 +269,11 @@ namespace cv
         class CV_EXPORTS WTrajectoryFrustums : public Widget3D
         {
         public:
-            //! Displays trajectory of the given path by frustums
-            WTrajectoryFrustums(const std::vector<Affine3d> &path, const Matx33d &K, double scale = 1., const Color &color = Color::white());
-            //! Displays trajectory of the given path by frustums
-            WTrajectoryFrustums(const std::vector<Affine3d> &path, const Vec2d &fov, double scale = 1., const Color &color = Color::white());
+            //! Takes vector<Affine3<T>> and displays trajectory of the given path by frustums
+            WTrajectoryFrustums(InputArray path, const Matx33d &K, double scale = 1., const Color &color = Color::white());
+
+            //! Takes vector<Affine3<T>> and displays trajectory of the given path by frustums
+            WTrajectoryFrustums(InputArray path, const Vec2d &fov, double scale = 1., const Color &color = Color::white());
         };
 
         class CV_EXPORTS WTrajectorySpheres: public Widget3D

modules/viz/src/shapes.cpp

@@ -1056,41 +1056,6 @@ template<> cv::viz::WCameraPosition cv::viz::Widget::cast<cv::viz::WCameraPositi
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// trajectory widget implementation
 
-namespace cv { namespace viz { namespace
-{
-    struct TrajectoryUtils
-    {
-        static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3d> &path)
-        {
-            vtkIdType nr_points = path.size();
-
-            for (vtkIdType i = 0; i < nr_points; ++i)
-            {
-                vtkSmartPointer<vtkPolyData> new_data = vtkSmartPointer<vtkPolyData>::New();
-                new_data->DeepCopy(poly_data);
-
-                // Transform the default coordinate frame
-                vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
-                transform->PreMultiply();
-                vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
-                mat_trans = vtkmatrix(path[i].matrix);
-                transform->SetMatrix(mat_trans);
-
-                vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
-#if VTK_MAJOR_VERSION <= 5
-                filter->SetInput(new_data);
-#else
-                filter->SetInputData(new_data);
-#endif
-                filter->SetTransform(transform);
-                filter->Update();
-
-                append_filter->AddInputConnection(filter->GetOutputPort());
-            }
-        }
-    };
-}}}
-
 cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, double scale, const Color &color)
 {
     vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
@@ -1103,7 +1068,6 @@ cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, double sca
         appendFilter->AddInputConnection(polydata->GetProducerPort());
     }
 
-    vtkSmartPointer<vtkTensorGlyph> tensor_glyph;
     if (display_mode & WTrajectory::FRAMES)
     {
         vtkSmartPointer<vtkTrajectorySource> source = vtkSmartPointer<vtkTrajectorySource>::New();
@@ -1111,7 +1075,7 @@ cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, double sca
 
         vtkSmartPointer<vtkPolyData> glyph = getPolyData(WCoordinateSystem(scale));
 
-        tensor_glyph = vtkSmartPointer<vtkTensorGlyph>::New();
+        vtkSmartPointer<vtkTensorGlyph> tensor_glyph = vtkSmartPointer<vtkTensorGlyph>::New();
         tensor_glyph->SetInputConnection(source->GetOutputPort());
         tensor_glyph->SetSourceConnection(glyph->GetProducerPort());
         tensor_glyph->ExtractEigenvaluesOff();  // Treat as a rotation matrix, not as something with eigenvalues
@@ -1142,42 +1106,26 @@ template<> cv::viz::WTrajectory cv::viz::Widget::cast<cv::viz::WTrajectory>()
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// WTrajectoryFrustums widget implementation
 
-cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(const std::vector<Affine3d> &path, const Matx33d &K, double scale, const Color &color)
+cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(InputArray _path, const Matx33d &K, double scale, const Color &color)
 {
-    vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
-    double f_x = K(0,0);
-    double f_y = K(1,1);
-    double c_y = K(1,2);
-    double 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)
-    double fovy = 2.0 * 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);
-
-    double planesArray[24];
-    camera->GetFrustumPlanes(aspect_ratio, planesArray);
+    vtkSmartPointer<vtkTrajectorySource> source = vtkSmartPointer<vtkTrajectorySource>::New();
+    source->SetTrajectory(_path);
 
-    vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
-    planes->SetFrustumPlanes(planesArray);
+    vtkSmartPointer<vtkPolyData> glyph = getPolyData(WCameraPosition(K, scale));
 
-    vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
-    frustumSource->SetPlanes(planes);
-    frustumSource->Update();
+    vtkSmartPointer<vtkTensorGlyph> tensor_glyph = vtkSmartPointer<vtkTensorGlyph>::New();
+    tensor_glyph->SetInputConnection(source->GetOutputPort());
+    tensor_glyph->SetSourceConnection(glyph->GetProducerPort());
+    tensor_glyph->ExtractEigenvaluesOff();  // Treat as a rotation matrix, not as something with eigenvalues
+    tensor_glyph->ThreeGlyphsOff();
+    tensor_glyph->SymmetricOff();
+    tensor_glyph->ColorGlyphsOff();
+    tensor_glyph->Update();
 
-    // Extract the edges
-    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
-    filter->SetInputConnection(frustumSource->GetOutputPort());
-    filter->Update();
-
-    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
-    TrajectoryUtils::applyPath(filter->GetOutput(), appendFilter, path);
+    vtkSmartPointer<vtkPolyData> polydata = tensor_glyph->GetOutput();
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
-    mapper->SetInputConnection(appendFilter->GetOutputPort());
+    mapper->SetInputConnection(polydata->GetProducerPort());
 
     vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
     actor->SetMapper(mapper);
@@ -1186,38 +1134,26 @@ cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(const std::vector<Affine3d> &p
     setColor(color);
 }
 
-cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(const std::vector<Affine3d> &path, const Vec2d &fov, double scale, const Color &color)
+cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(InputArray _path, const Vec2d &fov, double scale, const Color &color)
 {
-    vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
+    vtkSmartPointer<vtkTrajectorySource> source = vtkSmartPointer<vtkTrajectorySource>::New();
+    source->SetTrajectory(_path);
 
-    camera->SetViewAngle(fov[1] * 180 / CV_PI); // Vertical field of view
-    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);
+    vtkSmartPointer<vtkPolyData> glyph = getPolyData(WCameraPosition(fov, scale));
 
-    double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5);
-
-    double planesArray[24];
-    camera->GetFrustumPlanes(aspect_ratio, planesArray);
-
-    vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
-    planes->SetFrustumPlanes(planesArray);
-
-    vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
-    frustumSource->SetPlanes(planes);
-    frustumSource->Update();
+    vtkSmartPointer<vtkTensorGlyph> tensor_glyph = vtkSmartPointer<vtkTensorGlyph>::New();
+    tensor_glyph->SetInputConnection(source->GetOutputPort());
+    tensor_glyph->SetSourceConnection(glyph->GetProducerPort());
+    tensor_glyph->ExtractEigenvaluesOff();  // Treat as a rotation matrix, not as something with eigenvalues
+    tensor_glyph->ThreeGlyphsOff();
+    tensor_glyph->SymmetricOff();
+    tensor_glyph->ColorGlyphsOff();
+    tensor_glyph->Update();
 
-    // Extract the edges
-    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
-    filter->SetInputConnection(frustumSource->GetOutputPort());
-    filter->Update();
-
-    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
-    TrajectoryUtils::applyPath(filter->GetOutput(), appendFilter, path);
+    vtkSmartPointer<vtkPolyData> polydata = tensor_glyph->GetOutput();
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
-    mapper->SetInputConnection(appendFilter->GetOutputPort());
+    mapper->SetInputConnection(polydata->GetProducerPort());
 
     vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
     actor->SetMapper(mapper);

modules/viz/src/types.cpp

@@ -245,6 +245,6 @@ void cv::viz::Camera::computeProjectionMatrix(Matx44d &proj) const
 
 cv::viz::Camera cv::viz::Camera::KinectCamera(const Size &window_size)
 {
-    Matx33d K(525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0) ;
+    Matx33d K(525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
     return Camera(K, window_size);
 }