diff --git a/modules/viz/doc/widget.rst b/modules/viz/doc/widget.rst index c42079964..159e4ba80 100644 --- 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 &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 &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> 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 2724e921d..b281635a6 100644 --- 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& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose"); - CV_EXPORTS void writeTrajectory(const std::vector& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose"); + //! takes vector> 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& 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& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose"); + //! takes vector> 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 27f211cd6..2868880ec 100644 --- 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 &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white()); - WTrajectory(const std::vector &path, int display_mode = WTrajectory::PATH, float scale = 1.f, const Color &color = Color::white()); + //! Takes vector> 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 fa6662862..490be3b3b 100644 --- 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 &_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 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 appendFilter = vtkSmartPointer::New(); + const Affine3d* dpath = _path.getMat().ptr(), *dend = dpath + _path.total(); + const Affine3f* fpath = _path.getMat().ptr(), *fend = fpath + _path.total(); + std::vector path; - // 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(); + if (_path.depth() == CV_32F) + path.assign(fpath, fend); - vtkSmartPointer points = vtkSmartPointer::New(); - points->SetDataTypeToFloat(); - points->SetNumberOfPoints(nr_points); + if (_path.depth() == CV_64F) + path.assign(dpath, dend); - vtkSmartPointer polyData = vtkSmartPointer::New(); - vtkSmartPointer polyLine = vtkSmartPointer::New(); - polyLine->GetPointIds()->SetNumberOfIds(nr_points); - - Vec3f *data_beg = vtkpoints_data(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 cells = vtkSmartPointer::New(); - cells->InsertNextCell(polyLine); - - polyData->SetPoints(points); - polyData->SetLines(cells); - - // Set the color for polyData - vtkSmartPointer colors = vtkSmartPointer::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) - { - // Create frames and transform along the path - vtkSmartPointer axes = vtkSmartPointer::New(); - axes->SetOrigin(0, 0, 0); - axes->SetScaleFactor(scale); - - vtkSmartPointer axes_colors = vtkSmartPointer::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 axes_data = axes->GetOutput(); -#if VTK_MAJOR_VERSION <= 5 - axes_data->Update(); -#else - axes->Update(); -#endif - axes_data->GetPointData()->SetScalars(axes_colors); - - vtkSmartPointer axes_tubes = vtkSmartPointer::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); - } - - vtkSmartPointer mapper = vtkSmartPointer::New(); - mapper->SetScalarModeToUsePointData(); - mapper->SetInputConnection(appendFilter->GetOutputPort()); - - vtkSmartPointer actor = vtkSmartPointer::New(); - actor->SetMapper(mapper); - - WidgetAccessor::setProp(*this, actor); -} - -cv::viz::WTrajectory::WTrajectory(const std::vector &path, int display_mode, float scale, const Color &color) -{ vtkSmartPointer appendFilter = vtkSmartPointer::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 39f34b35d..d01b947a4 100644 --- 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 - void readTrajectory(std::vector >& 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 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); + } - for(int i = start; i < end; ++i) + Mat(traj).convertTo(_traj, _traj.depth()); +} + +void cv::viz::writeTrajectory(InputArray _traj, const String& files_format, int start, const String& tag) +{ + if (_traj.kind() == _InputArray::STD_VECTOR_MAT) + { + std::vector& v = *(std::vector*)_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 - void writeTrajectory(const std::vector >& 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(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(i), tag); } -}}} + CV_Assert(!"Unsupported array kind"); +} +/////////////////////////////////////////////////////////////////////////////////////////////// +/// Computing normals for mesh +void cv::viz::computeNormals(const Mesh3d& mesh, OutputArray _normals) +{ + vtkSmartPointer prop = WidgetAccessor::getProp(WMesh(mesh)); + vtkSmartPointer mapper = vtkActor::SafeDownCast(prop)->GetMapper(); + vtkSmartPointer polydata = vtkPolyData::SafeDownCast(mapper->GetInput()); -void cv::viz::readTrajectory(std::vector& 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& 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& traj, const String& files_format, int start, const String& tag) -{ impl::writeTrajectory(traj, files_format, start, tag); } - -void cv::viz::writeTrajectory(const std::vector& traj, const String& files_format, int start, const String& tag) -{ impl::writeTrajectory(traj, files_format, start, tag); } + vtkSmartPointer normal_generator = vtkSmartPointer::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 generic_normals = normal_generator->GetOutput()->GetPointData()->GetNormals(); + if(generic_normals) + { + Mat normals(1, generic_normals->GetNumberOfTuples(), CV_64FC3); + Vec3d *optr = normals.ptr(); + 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 b183f7bb5..f311ff2b7 100644 --- 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(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(); }