ported some trajectories functionality to InputArray style

diff --git a/modules/viz/doc/widget.rst b/modules/viz/doc/widget.rst
index c420799..159e4ba 100644 (file)
--- a/modules/viz/doc/widget.rst
+++ b/modules/viz/doc/widget.rst
@@ -732,16 +732,16 @@ This 3D Widget represents a trajectory. ::
         enum {FRAMES = 1, PATH = 2, BOTH = FRAMES + PATH};
 
         //! Displays trajectory of the given path either by coordinate frames or polyline
-        WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white(),;
+        WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white(),;
     };
 
 viz::WTrajectory::WTrajectory
 -----------------------------
 Constructs a WTrajectory.
 
-.. ocv:function:: WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white())
+.. ocv:function:: WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, 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 display_mode: Display mode. This can be PATH, FRAMES, and BOTH.
     :param scale: Scale of the frames. Polyline is not affected.
     :param color: :ocv:class:`Color` of the polyline that represents path. Frames are not affected.

diff --git a/modules/viz/include/opencv2/viz.hpp b/modules/viz/include/opencv2/viz.hpp
index 2724e92..b281635 100644 (file)
--- a/modules/viz/include/opencv2/viz.hpp
+++ b/modules/viz/include/opencv2/viz.hpp
@@ -104,11 +104,17 @@ namespace cv
         CV_EXPORTS bool readPose(const String& file, Affine3d& pose, const String& tag = "pose");
         CV_EXPORTS void writePose(const String& file, const Affine3d& pose, const String& tag = "pose");
 
-        CV_EXPORTS void writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
-        CV_EXPORTS void writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
+        //! takes vector<Affine3<T>> with T = float/dobule and writes to a sequence of files with given filename format
+        CV_EXPORTS void writeTrajectory(InputArray traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
 
-        CV_EXPORTS void readTrajectory(std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
-        CV_EXPORTS void readTrajectory(std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
+        //! takes vector<Affine3<T>> with T = float/dobule and loads poses from sequence of files
+        CV_EXPORTS void readTrajectory(OutputArray traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
+
+
+        ///////////////////////////////////////////////////////////////////////////////////////////////
+        /// Computing normals for mesh
+
+        CV_EXPORTS void computeNormals(const Mesh3d& mesh, OutputArray normals);
 
     } /* namespace viz */
 } /* namespace cv */

diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp
index 27f211c..2868880 100644 (file)
--- a/modules/viz/include/opencv2/viz/widgets.hpp
+++ b/modules/viz/include/opencv2/viz/widgets.hpp
@@ -262,9 +262,8 @@ namespace cv
         public:
             enum {FRAMES = 1, PATH = 2, BOTH = FRAMES + PATH };
 
-            //! Displays trajectory of the given path either by coordinate frames or polyline
-            WTrajectory(const std::vector<Affine3f> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
-            WTrajectory(const std::vector<Affine3d> &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
+            //! Takes vector<Affine3<T>> and displays trajectory of the given path either by coordinate frames or polyline
+            WTrajectory(InputArray path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white());
         };
 
         class CV_EXPORTS WTrajectoryFrustums : public Widget3D
@@ -309,7 +308,7 @@ namespace cv
         class CV_EXPORTS WCloudNormals : public Widget3D
         {
         public:
-            WCloudNormals(InputArray cloud, InputArray normals, int level = 100, float scale = 0.02f, const Color &color = Color::white());
+            WCloudNormals(InputArray cloud, InputArray normals, int level = 64, float scale = 0.1f, const Color &color = Color::white());
         };
 
         class CV_EXPORTS WMesh : public Widget3D

diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp
index fa66628..490be3b 100644 (file)
--- a/modules/viz/src/shapes.cpp
+++ b/modules/viz/src/shapes.cpp
@@ -1213,106 +1213,21 @@ namespace cv { namespace viz { namespace
     };
 }}}
 
-cv::viz::WTrajectory::WTrajectory(const std::vector<Affine3f> &_path, int display_mode, float scale, const Color &color)
+cv::viz::WTrajectory::WTrajectory(InputArray _path, int display_mode, float scale, const Color &color)
 {
-    std::vector<Affine3d> path(_path.begin(), _path.end());
+    CV_Assert(_path.kind() == _InputArray::STD_VECTOR || _path.kind() == _InputArray::MAT);
+    CV_Assert(_path.type() == CV_32FC(16) || _path.type() == CV_64FC(16));
 
-    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)
-    {
-        // 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);
-        }
-
-        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
-    }
+    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 ((~display_mode & 3) ^ WTrajectory::FRAMES)
-    {
-        // Create frames and transform along the path
-        vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New();
-        axes->SetOrigin(0, 0, 0);
-        axes->SetScaleFactor(scale);
-
-        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> axes_data = axes->GetOutput();
-#if VTK_MAJOR_VERSION <= 5
-        axes_data->Update();
-#else
-        axes->Update();
-#endif
-        axes_data->GetPointData()->SetScalars(axes_colors);
+    if (_path.depth() == CV_32F)
+        path.assign(fpath, fend);
 
-        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);
-    }
+    if (_path.depth() == CV_64F)
+        path.assign(dpath, dend);
 
-    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
-    mapper->SetScalarModeToUsePointData();
-    mapper->SetInputConnection(appendFilter->GetOutputPort());
-
-    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
-    actor->SetMapper(mapper);
-
-    WidgetAccessor::setProp(*this, actor);
-}
-
-cv::viz::WTrajectory::WTrajectory(const std::vector<Affine3d> &path, int display_mode, float scale, const Color &color)
-{
     vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
 
     // Bitwise and with 3 in order to limit the domain to 2 bits

diff --git a/modules/viz/src/vizcore.cpp b/modules/viz/src/vizcore.cpp
index 39f34b3..d01b947 100644 (file)
--- a/modules/viz/src/vizcore.cpp
+++ b/modules/viz/src/vizcore.cpp
@@ -243,53 +243,103 @@ void cv::viz::writePose(const String& file, const Affine3d& pose, const String&
     fs << tag << Mat(pose.matrix, false);
 }
 
-namespace cv { namespace viz { namespace impl
+void cv::viz::readTrajectory(OutputArray _traj, const String& files_format, int start, int end, const String& tag)
 {
-    template <typename _Tp>
-    void readTrajectory(std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, int end, const String& tag)
+    CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);
+
+    start = max(0, std::min(start, end));
+    end = std::max(start, end);
+
+    std::vector<Affine3d> traj;
+
+    for(int i = start; i < end; ++i)
     {
-        start = max(0, std::min(start, end));
-        end = std::max(start, end);
+        Affine3d affine;
+        bool ok = readPose(cv::format(files_format.c_str(), i), affine, tag);
+        if (!ok)
+            break;
 
-        std::vector< Affine3<_Tp> > temp;
+        traj.push_back(affine);
+    }
+
+    Mat(traj).convertTo(_traj, _traj.depth());
+}
 
-        for(int i = start; i < end; ++i)
+void cv::viz::writeTrajectory(InputArray _traj, const String& files_format, int start, const String& tag)
+{
+    if (_traj.kind() == _InputArray::STD_VECTOR_MAT)
+    {
+        std::vector<Mat>& v = *(std::vector<Mat>*)_traj.getObj();
+
+        for(size_t i = 0, index = max(0, start); i < v.size(); ++i, ++index)
         {
             Affine3d affine;
-            bool ok = readPose(cv::format(files_format.c_str(), i), affine, tag);
-            if (!ok)
-                break;
-
-            temp.push_back(affine);
+            Mat pose = v[i];
+            CV_Assert(pose.type() == CV_32FC(16) || pose.type() == CV_64FC(16));
+            pose.copyTo(affine.matrix);
+            writePose(cv::format(files_format.c_str(), index), affine, tag);
         }
-        traj.swap(temp);
+        return;
     }
 
-    template <typename _Tp>
-    void writeTrajectory(const std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, const String& tag)
+    if (_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT)
     {
-        for(size_t i = 0, index = max(0, start); i < traj.size(); ++i, ++index)
-            writePose(cv::format(files_format.c_str(), index), traj[i], tag);
-    }
-}}}
+        CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16));
 
+        Mat traj = _traj.getMat();
 
+        if (traj.depth() == CV_32F)
+            for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
+                writePose(cv::format(files_format.c_str(), index), traj.at<Affine3f>(i), tag);
 
+        if (traj.depth() == CV_64F)
+            for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
+                writePose(cv::format(files_format.c_str(), index), traj.at<Affine3d>(i), tag);
+    }
 
-void cv::viz::readTrajectory(std::vector<Affine3f>& traj, const String& files_format, int start, int end, const String& tag)
-{ impl::readTrajectory(traj, files_format, start, end, tag); }
-
-void cv::viz::readTrajectory(std::vector<Affine3d>& traj, const String& files_format, int start, int end, const String& tag)
-{ impl::readTrajectory(traj, files_format, start, end, tag); }
-
-void cv::viz::writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format, int start, const String& tag)
-{ impl::writeTrajectory(traj, files_format, start, tag); }
+    CV_Assert(!"Unsupported array kind");
+}
 
-void cv::viz::writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format, int start, const String& tag)
-{ impl::writeTrajectory(traj, files_format, start, tag); }
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// Computing normals for mesh
 
+void cv::viz::computeNormals(const Mesh3d& mesh, OutputArray _normals)
+{
+    vtkSmartPointer<vtkProp> prop = WidgetAccessor::getProp(WMesh(mesh));
+    vtkSmartPointer<vtkMapper> mapper = vtkActor::SafeDownCast(prop)->GetMapper();
+    vtkSmartPointer<vtkPolyData> polydata = vtkPolyData::SafeDownCast(mapper->GetInput());
+
+    vtkSmartPointer<vtkPolyDataNormals> normal_generator = vtkSmartPointer<vtkPolyDataNormals>::New();
+#if VTK_MAJOR_VERSION <= 5
+    normal_generator->SetInput(polydata);
+#else
+    normal_generator->SetInputData(polydata);
+#endif
+    normal_generator->ComputePointNormalsOn();
+    normal_generator->ComputeCellNormalsOff();
+
+    normal_generator->SetFeatureAngle(0.1);
+    normal_generator->SetSplitting(0);
+    normal_generator->SetConsistency(1);
+    normal_generator->SetAutoOrientNormals(0);
+    normal_generator->SetFlipNormals(0);
+    normal_generator->SetNonManifoldTraversal(1);
+    normal_generator->Update();
+
+    vtkSmartPointer<vtkDataArray> generic_normals = normal_generator->GetOutput()->GetPointData()->GetNormals();
+    if(generic_normals)
+    {
+        Mat normals(1, generic_normals->GetNumberOfTuples(), CV_64FC3);
+        Vec3d *optr = normals.ptr<Vec3d>();
 
+        for(int i = 0; i < generic_normals->GetNumberOfTuples(); ++i, ++optr)
+            generic_normals->GetTuple(i, optr->val);
 
+        normals.convertTo(_normals, mesh.cloud.type());
+    }
+    else
+        _normals.release();
+}
 
 
 

diff --git a/modules/viz/test/tests_simple.cpp b/modules/viz/test/tests_simple.cpp
index b183f7b..f311ff2 100644 (file)
--- a/modules/viz/test/tests_simple.cpp
+++ b/modules/viz/test/tests_simple.cpp
@@ -49,9 +49,11 @@ TEST(Viz, DISABLED_show_cloud_bluberry)
 {
     Mat dragon_cloud = readCloud(get_dragon_ply_file_path());
 
+    Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
+
     Viz3d viz("show_cloud_bluberry");
     viz.showWidget("coosys", WCoordinateSystem());
-    viz.showWidget("dragon", WCloud(dragon_cloud, Color::bluberry()));
+    viz.showWidget("dragon", WCloud(dragon_cloud, Color::bluberry()), pose);
     viz.spin();
 }
 
@@ -62,9 +64,11 @@ TEST(Viz, DISABLED_show_cloud_random_color)
     Mat colors(dragon_cloud.size(), CV_8UC3);
     theRNG().fill(colors, RNG::UNIFORM, 0, 255);
 
+    Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
+
     Viz3d viz("show_cloud_random_color");
     viz.showWidget("coosys", WCoordinateSystem());
-    viz.showWidget("dragon", WCloud(dragon_cloud, colors));
+    viz.showWidget("dragon", WCloud(dragon_cloud, colors), pose);
     viz.spin();
 }
 
@@ -77,9 +81,11 @@ TEST(Viz, DISABLED_show_cloud_masked)
         if (i % 15 != 0)
             dragon_cloud.at<Vec3f>(i) = qnan;
 
+    Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
+
     Viz3d viz("show_cloud_masked");
     viz.showWidget("coosys", WCoordinateSystem());
-    viz.showWidget("dragon", WCloud(dragon_cloud));
+    viz.showWidget("dragon", WCloud(dragon_cloud), pose);
     viz.spin();
 }
 
@@ -102,9 +108,11 @@ TEST(Viz, DISABLED_show_mesh)
 {
     Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());
 
+    Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
+
     Viz3d viz("show_mesh");
     viz.showWidget("coosys", WCoordinateSystem());
-    viz.showWidget("mesh", WMesh(mesh));
+    viz.showWidget("mesh", WMesh(mesh), pose);
     viz.spin();
 }
 
@@ -113,9 +121,11 @@ TEST(Viz, DISABLED_show_mesh_random_colors)
     Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());
     theRNG().fill(mesh.colors, RNG::UNIFORM, 0, 255);
 
+    Affine3d pose = Affine3d().rotate(Vec3d(0, 0.8, 0));
+
     Viz3d viz("show_mesh_random_color");
     viz.showWidget("coosys", WCoordinateSystem());
-    viz.showWidget("mesh", WMesh(mesh));
+    viz.showWidget("mesh", WMesh(mesh), pose);
     viz.setRenderingProperty("mesh", SHADING, SHADING_PHONG);
     viz.spin();
 }