added test for trajectories and fixed bug with float type

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

@@ -279,6 +279,7 @@ namespace cv
         class CV_EXPORTS WTrajectorySpheres: public Widget3D
         {
         public:
+            //! Takes vector<Affine3<T>> and displays trajectory of the given path
             WTrajectorySpheres(InputArray path, double line_length = 0.05, double radius = 0.007,
                                const Color &from = Color::red(), const Color &to = Color::white());
         };

modules/viz/src/precomp.hpp

@@ -221,6 +221,22 @@ namespace cv
                 return scalars;
             }
         };
+
+        inline vtkSmartPointer<vtkMatrix4x4> vtkmatrix(const cv::Matx44d &matrix)
+        {
+            vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
+            vtk_matrix->DeepCopy(matrix.val);
+            return vtk_matrix;
+        }
+
+        inline Color vtkcolor(const Color& color)
+        {
+            Color scaled_color = color * (1.0/255.0);
+            std::swap(scaled_color[0], scaled_color[2]);
+            return scaled_color;
+        }
+
+        template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/norm(v); }
     }
 }
 

modules/viz/src/shapes.cpp

@@ -70,6 +70,35 @@ template<> cv::viz::WLine cv::viz::Widget::cast<cv::viz::WLine>()
     return static_cast<WLine&>(widget);
 }
 
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// sphere widget implementation
+
+cv::viz::WSphere::WSphere(const Point3d &center, double radius, int sphere_resolution, const Color &color)
+{
+    vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New();
+    sphere->SetRadius(radius);
+    sphere->SetCenter(center.x, center.y, center.z);
+    sphere->SetPhiResolution(sphere_resolution);
+    sphere->SetThetaResolution(sphere_resolution);
+    sphere->LatLongTessellationOff();
+    sphere->Update();
+
+    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    VtkUtils::SetInputData(mapper, sphere->GetOutput());
+
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}
+
+template<> cv::viz::WSphere cv::viz::Widget::cast<cv::viz::WSphere>()
+{
+    Widget3D widget = this->cast<Widget3D>();
+    return static_cast<WSphere&>(widget);
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// plane widget implementation
 
@@ -143,51 +172,21 @@ template<> cv::viz::WPlane cv::viz::Widget::cast<cv::viz::WPlane>()
     return static_cast<WPlane&>(widget);
 }
 
-///////////////////////////////////////////////////////////////////////////////////////////////
-/// sphere widget implementation
-
-cv::viz::WSphere::WSphere(const Point3d &center, double radius, int sphere_resolution, const Color &color)
-{
-    vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New();
-    sphere->SetRadius(radius);
-    sphere->SetCenter(center.x, center.y, center.z);
-    sphere->SetPhiResolution(sphere_resolution);
-    sphere->SetThetaResolution(sphere_resolution);
-    sphere->LatLongTessellationOff();
-    sphere->Update();
-
-    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
-    VtkUtils::SetInputData(mapper, sphere->GetOutput());
-
-    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
-    actor->SetMapper(mapper);
-
-    WidgetAccessor::setProp(*this, actor);
-    setColor(color);
-}
-
-template<> cv::viz::WSphere cv::viz::Widget::cast<cv::viz::WSphere>()
-{
-    Widget3D widget = this->cast<Widget3D>();
-    return static_cast<WSphere&>(widget);
-}
-
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// arrow widget implementation
 
 cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness, const Color &color)
 {
-    vtkSmartPointer<vtkArrowSource> arrowSource = vtkSmartPointer<vtkArrowSource>::New();
-    arrowSource->SetShaftRadius(thickness);
-    // The thickness and radius of the tip are adjusted based on the thickness of the arrow
-    arrowSource->SetTipRadius(thickness * 3.0);
-    arrowSource->SetTipLength(thickness * 10.0);
+    vtkSmartPointer<vtkArrowSource> arrow_source = vtkSmartPointer<vtkArrowSource>::New();
+    arrow_source->SetShaftRadius(thickness);
+    arrow_source->SetTipRadius(thickness * 3.0);
+    arrow_source->SetTipLength(thickness * 10.0);
 
     RNG rng = theRNG();
     Vec3d arbitrary(rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0));
     Vec3d startPoint(pt1.x, pt1.y, pt1.z), endPoint(pt2.x, pt2.y, pt2.z);
 
-    double length = cv::norm(endPoint - startPoint);
+    double length = norm(endPoint - startPoint);
 
     Vec3d xvec = normalized(endPoint - startPoint);
     Vec3d zvec = normalized(xvec.cross(arbitrary));
@@ -204,7 +203,7 @@ cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness
     // Transform the polydata
     vtkSmartPointer<vtkTransformPolyDataFilter> transformPD = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
     transformPD->SetTransform(transform);
-    transformPD->SetInputConnection(arrowSource->GetOutputPort());
+    transformPD->SetInputConnection(arrow_source->GetOutputPort());
     transformPD->Update();
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
@@ -908,14 +907,14 @@ namespace  cv  { namespace viz { namespace
             actor->SetTexture(texture);
         }
 
-        static vtkSmartPointer<vtkPolyData> createFrustrum(double aspect_ratio, double fovy, double scale)
+        static vtkSmartPointer<vtkPolyData> createFrustum(double aspect_ratio, double fovy, double scale)
         {
             vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
             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);
+            camera->SetClippingRange(1e-9, scale);
 
             double planesArray[24];
             camera->GetFrustumPlanes(aspect_ratio, planesArray);
@@ -956,7 +955,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Matx33d &K, double scale, const
     double fovy = 2.0 * atan2(c_y, f_y) * 180 / CV_PI;
     double aspect_ratio = f_y / f_x;
 
-    vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale);
+    vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale);
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
     VtkUtils::SetInputData(mapper, polydata);
@@ -973,7 +972,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Vec2d &fov, double scale, const
     double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5);
     double fovy = fov[1] * 180 / CV_PI;
 
-    vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale);
+    vtkSmartPointer<vtkPolyData> polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale);
 
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
     VtkUtils::SetInputData(mapper, polydata);

modules/viz/src/vizimpl.hpp

@@ -179,34 +179,16 @@ private:
     bool removeActorFromRenderer(const vtkSmartPointer<vtkProp> &actor);
 };
 
-
 namespace cv
 {
     namespace viz
     {
-        inline vtkSmartPointer<vtkMatrix4x4> vtkmatrix(const cv::Matx44d &matrix)
-        {
-            vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
-            vtk_matrix->DeepCopy(matrix.val);
-            return vtk_matrix;
-        }
-
         struct color_tag {};
         struct gray_tag {};
 
         inline Vec3b fetchRgb(const unsigned char* color, color_tag) { return Vec3b(color[2], color[1], color[0]); }
         inline Vec3b fetchRgb(const unsigned char* color,  gray_tag) { return Vec3b(color[0], color[0], color[0]); }
 
-        inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); }
-        template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
-
-        inline Color vtkcolor(const Color& color)
-        {
-            Color scaled_color = color * (1.0/255.0);
-            std::swap(scaled_color[0], scaled_color[2]);
-            return scaled_color;
-        }
-
         struct ConvertToVtkImage
         {
             struct Impl

modules/viz/src/vtk/vtkTrajectorySource.cpp

@@ -59,9 +59,8 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj)
 
     Mat traj;
     _traj.getMat().convertTo(traj, CV_64F);
-    const Affine3d* dpath = _traj.getMat().ptr<Affine3d>();
-
-    size_t total = _traj.total();
+    const Affine3d* dpath = traj.ptr<Affine3d>();
+    size_t total = traj.total();
 
     points = vtkSmartPointer<vtkPoints>::New();
     points->SetDataType(VTK_DOUBLE);
@@ -81,7 +80,6 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj)
     }
 }
 
-
 cv::Mat cv::viz::vtkTrajectorySource::ExtractPoints(InputArray _traj)
 {
     CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT);

modules/viz/src/widget.cpp

@@ -140,14 +140,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value)
                 {
                     if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals())
                     {
+                        vtkSmartPointer<vtkPolyDataMapper> mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper());
                         vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New();
-#if VTK_MAJOR_VERSION <= 5
-                        normals->SetInput(actor->GetMapper()->GetInput());
-#else
-                        normals->SetInputData(actor->GetMapper()->GetInput());
-#endif
+                        VtkUtils::SetInputData(normals, mapper->GetInput());
                         normals->Update();
-                        vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort());
+                        VtkUtils::SetInputData(mapper, normals->GetOutput());
                     }
                     actor->GetProperty()->SetInterpolationToGouraud();
                     break;
@@ -156,14 +153,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value)
                 {
                     if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals())
                     {
+                        vtkSmartPointer<vtkPolyDataMapper> mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper());
                         vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New();
-#if VTK_MAJOR_VERSION <= 5
-                        normals->SetInput(actor->GetMapper()->GetInput());
-#else
-                        normals->SetInputData(actor->GetMapper()->GetInput());
-#endif
+                        VtkUtils::SetInputData(normals, mapper->GetInput());
                         normals->Update();
-                        vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort());
+                        VtkUtils::SetInputData(mapper, normals->GetOutput());
                     }
                     actor->GetProperty()->SetInterpolationToPhong();
                     break;

modules/viz/test/test_precomp.hpp

@@ -78,6 +78,21 @@ namespace cv
     {
         return Path::combine(cvtest::TS::ptr()->get_data_path(), "dragon.ply");
     }
+
+    template<typename _Tp>
+    inline std::vector< Affine3<_Tp> > generate_test_trajectory()
+    {
+        std::vector< Affine3<_Tp> > result;
+
+        for (int i = 0, j = 0; i <= 270; i += 3, j += 10)
+        {
+            double x = 2 * cos(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * 1.2 * CV_PI/180.0));
+            double y = 0.25 + i/270.0 + sin(j * CV_PI/180.0) * 0.2 * sin(0.6 + j * 1.5 * CV_PI/180.0);
+            double z = 2 * sin(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * CV_PI/180.0));
+            result.push_back(viz::makeCameraPose(Vec3d(x, y, z), Vec3d::all(0.0), Vec3d(0.0, 1.0, 0.0)));
+        }
+        return result;
+    }
 }
 
 #endif

modules/viz/test/test_viz3d.cpp

@@ -48,46 +48,21 @@ TEST(Viz_viz3d, develop)
     cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path());
 
     //cv::viz::Mesh3d mesh = cv::viz::Mesh3d::load(get_dragon_ply_file_path());
-
     //theRNG().fill(mesh.colors, RNG::UNIFORM, 0, 255);
 
     cv::viz::Viz3d viz("abc");
     viz.setBackgroundColor(cv::viz::Color::mlab());
-    viz.showWidget("coo", cv::viz::WCoordinateSystem(0.1));
-
-
-//    double c = cos(CV_PI/6);
-//    std::vector<Vec3d> pts;
-//    pts.push_back(Vec3d(0, 0.0, -1.0));
-//    pts.push_back(Vec3d(1,   c, -0.5));
-//    pts.push_back(Vec3d(2,   c,  0.5));
-//    pts.push_back(Vec3d(3, 0.0,  1.0));
-//    pts.push_back(Vec3d(4,  -c,  0.5));
-//    pts.push_back(Vec3d(5,  -c, -0.5));
-
-//    viz.showWidget("pl", cv::viz::WPolyLine(Mat(pts), cv::viz::Color::green()));
-
-    //viz.showWidget("pl", cv::viz::WPolyLine(cloud.colRange(0, 100), cv::viz::Color::green()));
-    //viz.spin();
-
-    //cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
+    viz.showWidget("coo", cv::viz::WCoordinateSystem(1));
 
     //viz.showWidget("h", cv::viz::Widget::fromPlyFile("d:/horse-red.ply"));
     //viz.showWidget("a", cv::viz::WArrow(cv::Point3f(0,0,0), cv::Point3f(1,1,1)));
 
-    std::vector<cv::Affine3d> gt, es;
-    cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml");
+    //---->>>>> <to_test_in_future>
+    //std::vector<cv::Affine3d> gt, es;
+    //cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml");
     //cv::viz::readTrajectory(es, "d:/Datasets/trajs/es%05d.xml");
-    gt.resize(20);
-
-    Affine3d inv = gt[0].inv();
-    for(size_t i = 0; i < gt.size(); ++i)
-        gt[i] = inv * gt[i];
-
-    //viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::PATH, 1.f, viz::Color::blue()), gt[0].inv());
-    viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::BOTH, 0.01f, viz::Color::blue()));
-
-    //viz.showWidget("tr", viz::WTrajectory(es, viz::WTrajectory::PATH, 1.f, viz::Color::red()), gt[0].inv());
+    //cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path());
+    //---->>>>> </to_test_in_future>
 
     //theRNG().fill(colors, cv::RNG::UNIFORM, 0, 255);
     //viz.showWidget("c", cv::viz::WCloud(cloud, colors));

modules/viz/test/tests_simple.cpp

@@ -156,6 +156,49 @@ TEST(Viz, DISABLED_show_sampled_normals)
     viz.spin();
 }
 
+TEST(Viz, DISABLED_show_trajectories)
+{
+    std::vector<Affine3d> path = generate_test_trajectory<double>(), sub0, sub1, sub2, sub3, sub4, sub5;
+
+    Mat(path).rowRange(0, path.size()/10+1).copyTo(sub0);
+    Mat(path).rowRange(path.size()/10, path.size()/5+1).copyTo(sub1);
+    Mat(path).rowRange(path.size()/5, 11*path.size()/12).copyTo(sub2);
+    Mat(path).rowRange(11*path.size()/12, path.size()).copyTo(sub3);
+    Mat(path).rowRange(3*path.size()/4, 33*path.size()/40).copyTo(sub4);
+    Mat(path).rowRange(33*path.size()/40, 9*path.size()/10).copyTo(sub5);
+    Matx33d K(1024.0, 0.0, 320.0, 0.0, 1024.0, 240.0, 0.0, 0.0, 1.0);
+
+    Viz3d viz("show_trajectories");
+    viz.showWidget("coos", WCoordinateSystem());
+    viz.showWidget("sub0", WTrajectorySpheres(sub0, 0.25, 0.07));
+    viz.showWidget("sub1", WTrajectory(sub1, WTrajectory::PATH, 0.2, Color::brown()));
+    viz.showWidget("sub2", WTrajectory(sub2, WTrajectory::FRAMES, 0.2));
+    viz.showWidget("sub3", WTrajectory(sub3, WTrajectory::BOTH, 0.2, Color::green()));
+    viz.showWidget("sub4", WTrajectoryFrustums(sub4, K, 0.3));
+    viz.showWidget("sub5", WTrajectoryFrustums(sub5, Vec2d(0.78, 0.78), 0.15));
+
+    int i = 0;
+    while(!viz.wasStopped())
+    {
+        double a = --i % 360;
+        Vec3d pose(sin(a * CV_PI/180), 0.7, cos(a * CV_PI/180));
+        viz.setViewerPose(makeCameraPose(pose * 7.5, Vec3d(0.0, 0.5, 0.0), Vec3d(0.0, 0.1, 0.0)));
+        viz.spinOnce(20, true);
+    }
+    //viz.spin();
+}
+
+TEST(Viz, DISABLED_show_trajectory_reposition)
+{
+    std::vector<Affine3f> path = generate_test_trajectory<float>();
+
+    Viz3d viz("show_trajectory_reposition_to_origin");
+    viz.showWidget("coos", WCoordinateSystem());
+    viz.showWidget("sub3", WTrajectory(Mat(path).rowRange(0, path.size()/3), WTrajectory::BOTH, 0.2, Color::brown()), path.front().inv());
+    viz.spin();
+}
+
+
 TEST(Viz, DISABLED_spin_twice_____________________________TODO_UI_BUG)
 {
     Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());