refactored simple trajectory to use TensorGlyph

modules/viz/src/precomp.hpp

@@ -132,10 +132,11 @@
 # include <io.h> /* unlink */
 #endif
 
-#include <vtk/vtkCloudMatSource.h>
-#include <vtk/vtkCloudMatSink.h>
 #include <vtk/vtkOBJWriter.h>
 #include <vtk/vtkXYZWriter.h>
+#include <vtk/vtkCloudMatSink.h>
+#include <vtk/vtkCloudMatSource.h>
+#include <vtk/vtkTrajectorySource.h>
 
 #include <opencv2/core.hpp>
 #include <opencv2/viz.hpp>

modules/viz/src/shapes.cpp

@@ -1145,106 +1145,39 @@ namespace cv { namespace viz { namespace
 
 cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, double scale, const Color &color)
 {
-    CV_Assert(_path.kind() == _InputArray::STD_VECTOR || _path.kind() == _InputArray::MAT);
-    CV_Assert(_path.type() == CV_32FC(16) || _path.type() == CV_64FC(16));
-
-    const Affine3d* dpath = _path.getMat().ptr<Affine3d>(), *dend = dpath + _path.total();
-    const Affine3f* fpath = _path.getMat().ptr<Affine3f>(), *fend = fpath + _path.total();
-    std::vector<Affine3d> path;
-
-    if (_path.depth() == CV_32F)
-        path.assign(fpath, fend);
-
-    if (_path.depth() == CV_64F)
-        path.assign(dpath, dend);
-
     vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
 
     // Bitwise and with 3 in order to limit the domain to 2 bits
-    if ((~display_mode & 3) ^ WTrajectory::PATH)
+    if (display_mode & WTrajectory::PATH)
     {
-        // Create a poly line along the path
-        vtkIdType nr_points = path.size();
-
-        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
-        points->SetDataTypeToFloat();
-        points->SetNumberOfPoints(nr_points);
-
-        vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
-        vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
-        polyLine->GetPointIds()->SetNumberOfIds(nr_points);
-
-        Vec3f *data_beg = vtkpoints_data<float>(points);
-
-        for (vtkIdType i = 0; i < nr_points; ++i)
-        {
-            Vec3f cam_pose = path[i].translation();
-            *data_beg++ = cam_pose;
-            polyLine->GetPointIds()->SetId(i,i);
+        Mat points = vtkTrajectorySource::ExtractPoints(_path);
+        vtkSmartPointer<vtkPolyData> polydata = getPolyData(WPolyLine(points, color));
+        appendFilter->AddInputConnection(polydata->GetProducerPort());
     }
 
-        vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
-        cells->InsertNextCell(polyLine);
-
-        polyData->SetPoints(points);
-        polyData->SetLines(cells);
-
-        // Set the color for polyData
-        vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
-        colors->SetNumberOfComponents(3);
-        colors->SetNumberOfTuples(nr_points);
-        colors->FillComponent(0, color[2]);
-        colors->FillComponent(1, color[1]);
-        colors->FillComponent(2, color[0]);
-
-        polyData->GetPointData()->SetScalars(colors);
-#if VTK_MAJOR_VERSION <= 5
-        appendFilter->AddInputConnection(polyData->GetProducerPort());
-#else
-        appendFilter->AddInputData(polyData);
-#endif
-    }
-
-    if ((~display_mode & 3) ^ WTrajectory::FRAMES)
+    vtkSmartPointer<vtkTensorGlyph> tensor_glyph;
+    if (display_mode & WTrajectory::FRAMES)
     {
-        // Create frames and transform along the path
-        vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New();
-        axes->SetOrigin(0, 0, 0);
-        axes->SetScaleFactor(scale);
+        vtkSmartPointer<vtkTrajectorySource> source = vtkSmartPointer<vtkTrajectorySource>::New();
+        source->SetTrajectory(_path);
 
-        vtkSmartPointer<vtkUnsignedCharArray> axes_colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
-        axes_colors->SetNumberOfComponents(3);
-        axes_colors->InsertNextTuple3(255,0,0);
-        axes_colors->InsertNextTuple3(255,0,0);
-        axes_colors->InsertNextTuple3(0,255,0);
-        axes_colors->InsertNextTuple3(0,255,0);
-        axes_colors->InsertNextTuple3(0,0,255);
-        axes_colors->InsertNextTuple3(0,0,255);
+        vtkSmartPointer<vtkPolyData> glyph = getPolyData(WCoordinateSystem(scale));
 
-        vtkSmartPointer<vtkPolyData> axes_data = axes->GetOutput();
-#if VTK_MAJOR_VERSION <= 5
-        axes_data->Update();
-#else
-        axes->Update();
-#endif
-        axes_data->GetPointData()->SetScalars(axes_colors);
+        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();
 
-        vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New();
-#if VTK_MAJOR_VERSION <= 5
-        axes_tubes->SetInput(axes_data);
-#else
-        axes_tubes->SetInputData(axes_data);
-#endif
-        axes_tubes->SetRadius(axes->GetScaleFactor() / 50.0);
-        axes_tubes->SetNumberOfSides(6);
-        axes_tubes->Update();
-
-        TrajectoryUtils::applyPath(axes_tubes->GetOutput(), appendFilter, path);
+        appendFilter->AddInputConnection(tensor_glyph->GetOutputPort());
     }
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    VtkUtils::SetInputData(mapper, appendFilter->GetOutput());
     mapper->SetScalarModeToUsePointData();
-    mapper->SetInputConnection(appendFilter->GetOutputPort());
+    mapper->SetScalarRange(0, 255);
 
     vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
     actor->SetMapper(mapper);
@@ -1269,12 +1202,12 @@ cv::viz::WTrajectoryFrustums::WTrajectoryFrustums(const std::vector<Affine3d> &p
     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;
+    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->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];

modules/viz/src/vtk/vtkCloudMatSink.h

@@ -45,6 +45,9 @@
 #ifndef __vtkCloudMatSink_h
 #define __vtkCloudMatSink_h
 
+#include <opencv2/core.hpp>
+#include <vtkPolyDataWriter.h>
+
 namespace cv
 {
     namespace viz

modules/viz/src/vtk/vtkCloudMatSource.h

@@ -45,7 +45,7 @@
 #ifndef __vtkCloudMatSource_h
 #define __vtkCloudMatSource_h
 
-#include <opencv2/core/mat.hpp>
+#include <opencv2/core.hpp>
 #include <vtkPolyDataAlgorithm.h>
 #include <vtkSmartPointer.h>
 #include <vtkPoints.h>

modules/viz/src/vtk/vtkTrajectorySource.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+// Authors:
+//  * Anatoly Baksheev, Itseez Inc.  myname.mysurname <> mycompany.com
+//
+//M*/
+
+#include "precomp.hpp"
+
+namespace cv { namespace viz
+{
+    vtkStandardNewMacro(vtkTrajectorySource);
+}}
+
+cv::viz::vtkTrajectorySource::vtkTrajectorySource() { SetNumberOfInputPorts(0); }
+cv::viz::vtkTrajectorySource::~vtkTrajectorySource() {}
+
+void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj)
+{
+    CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);
+    CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16));
+
+    Mat traj;
+    _traj.getMat().convertTo(traj, CV_64F);
+    const Affine3d* dpath = _traj.getMat().ptr<Affine3d>();
+
+    size_t total = _traj.total();
+
+    points = vtkSmartPointer<vtkPoints>::New();
+    points->SetDataType(VTK_DOUBLE);
+    points->SetNumberOfPoints(total);
+
+    tensors = vtkSmartPointer<vtkDoubleArray>::New();
+    tensors->SetNumberOfComponents(9);
+    tensors->SetNumberOfTuples(total);
+
+    for(size_t i = 0; i < total; ++i, ++dpath)
+    {
+        Matx33d R = dpath->rotation().t();  // transposed because of
+        tensors->SetTuple(i, R.val);        // column major order
+
+        Vec3d p = dpath->translation();
+        points->SetPoint(i, p.val);
+    }
+}
+
+
+cv::Mat cv::viz::vtkTrajectorySource::ExtractPoints(InputArray _traj)
+{
+    CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);
+    CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16));
+
+    Mat points(1, _traj.total(), CV_MAKETYPE(_traj.depth(), 3));
+    const Affine3d* dpath = _traj.getMat().ptr<Affine3d>();
+    const Affine3f* fpath = _traj.getMat().ptr<Affine3f>();
+
+    if (_traj.depth() == CV_32F)
+        for(int i = 0; i < points.cols; ++i)
+            points.at<Vec3f>(i) = fpath[i].translation();
+
+    if (_traj.depth() == CV_64F)
+        for(int i = 0; i < points.cols; ++i)
+            points.at<Vec3d>(i) = dpath[i].translation();
+
+    return points;
+}
+
+int cv::viz::vtkTrajectorySource::RequestData(vtkInformation *vtkNotUsed(request), vtkInformationVector **vtkNotUsed(inputVector), vtkInformationVector *outputVector)
+{
+    vtkInformation *outInfo = outputVector->GetInformationObject(0);
+    vtkPolyData *output = vtkPolyData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));
+    output->SetPoints(points);
+    output->GetPointData()->SetTensors(tensors);
+    return 1;
+}

modules/viz/src/vtk/vtkTrajectorySource.h

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+// Authors:
+//  * Anatoly Baksheev, Itseez Inc.  myname.mysurname <> mycompany.com
+//
+//M*/
+
+#ifndef __vtkTrajectorySource_h
+#define __vtkTrajectorySource_h
+
+#include <opencv2/core/mat.hpp>
+#include <vtkPolyDataAlgorithm.h>
+#include <vtkSmartPointer.h>
+#include <vtkPoints.h>
+#include <vtkCellArray.h>
+
+namespace cv
+{
+    namespace viz
+    {
+        class vtkTrajectorySource : public vtkPolyDataAlgorithm
+        {
+        public:
+            static vtkTrajectorySource *New();
+            vtkTypeMacro(vtkTrajectorySource,vtkPolyDataAlgorithm);
+
+            virtual void SetTrajectory(InputArray trajectory);
+
+            static Mat ExtractPoints(InputArray trajectory);
+
+        protected:
+            vtkTrajectorySource();
+            ~vtkTrajectorySource();
+
+            vtkSmartPointer<vtkPoints> points;
+            vtkSmartPointer<vtkDoubleArray> tensors;
+
+            int RequestData(vtkInformation *, vtkInformationVector **, vtkInformationVector *);
+        private:
+            vtkTrajectorySource(const vtkTrajectorySource&);  // Not implemented.
+            void operator=(const vtkTrajectorySource&);  // Not implemented.
+
+        };
+    }
+}
+
+#endif