shorten Trajectory widget implementation

diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp
index 48616ca..101d856 100644 (file)
--- a/modules/viz/include/opencv2/viz/widgets.hpp
+++ b/modules/viz/include/opencv2/viz/widgets.hpp
@@ -180,6 +180,9 @@ namespace cv
         public:
             TrajectoryWidget(const std::vector<Affine3f> &path, const Color &color = Color::white(), bool show_frames = false, double scale = 1.0);
             TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
+            
+        private:
+            struct ApplyPath;
         };
 
         class CV_EXPORTS CloudWidget : public Widget3D

diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp
index 4067ef5..ed07691 100644 (file)
--- a/modules/viz/src/shape_widgets.cpp
+++ b/modules/viz/src/shape_widgets.cpp
@@ -984,6 +984,36 @@ cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec2f &fov, double sca
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// trajectory widget implementation
 
+struct cv::viz::TrajectoryWidget::ApplyPath
+{
+    static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3f> &path)
+    {
+        vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+        mat_trans->Identity();
+        
+        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();
+            vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);        
+            transform->SetMatrix(mat_trans);
+            
+            vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+            filter->SetInput(new_data);
+            filter->SetTransform(transform);
+            filter->Update();
+            
+            append_filter->AddInputConnection(filter->GetOutputPort());
+        }
+    }
+};
+
 cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Color &color, bool show_frames, double scale)
 {
     vtkIdType nr_points = path.size();    
@@ -1016,49 +1046,33 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
     vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
     if (show_frames)
     {
-        vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
-        mat_trans->Identity();
+        vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New();
+        axes->SetOrigin (0, 0, 0);
+        axes->SetScaleFactor (scale);
         
-        for (vtkIdType i = 0; i < nr_points; ++i)
-        {
-            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 ();
-            axes_data->Update ();
-            axes_data->GetPointData ()->SetScalars (axes_colors);
-            
-            // Transform the default coordinate frame
-            vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
-            transform->PreMultiply();
-            vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);        
-            transform->SetMatrix(mat_trans);
-            
-            vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
-            filter->SetInput(axes_data);
-            filter->SetTransform(transform);
-            filter->Update();
-            
-            vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
-            axes_tubes->SetInput (filter->GetOutput());
-            axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
-            axes_tubes->SetNumberOfSides (6);
-            
-            appendFilter->AddInputConnection(axes_tubes->GetOutputPort());
-        }
+        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 ();
+        axes_data->Update ();
+        axes_data->GetPointData ()->SetScalars (axes_colors);
+        
+        vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
+        axes_tubes->SetInput (axes_data);
+        axes_tubes->SetRadius (axes->GetScaleFactor() / 50.0);
+        axes_tubes->SetNumberOfSides (6);
+        axes_tubes->Update();
+        
+        ApplyPath::applyPath(axes_tubes->GetOutput(), appendFilter, path);
     }
     
-    // Set the color only for polyData
+    // Set the color for polyData
     vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
     colors->SetNumberOfComponents(3);
     
@@ -1103,36 +1117,19 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
     double planesArray[24];
     camera->GetFrustumPlanes(aspect_ratio, planesArray);
     
-    vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
-    mat_trans->Identity();
+    vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
+    planes->SetFrustumPlanes(planesArray);
     
-    for (vtkIdType i = 0; i < nr_points; ++i)
-    {
-        vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
-        planes->SetFrustumPlanes(planesArray);
-        
-        vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
-        frustumSource->SetPlanes(planes);
-        frustumSource->Update();
-
-        // Extract the edges
-        vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
-        filter->SetInput(frustumSource->GetOutput());
-        filter->Update();
-        
-        // Transform the default coordinate frame
-        vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
-        transform->PreMultiply();
-        vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans);        
-        transform->SetMatrix(mat_trans);
-        
-        vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
-        transform_filter->SetInput(filter->GetOutput());
-        transform_filter->SetTransform(transform);
-        transform_filter->Update();
-        
-        appendFilter->AddInputConnection(transform_filter->GetOutputPort());
-    }
+    vtkSmartPointer<vtkFrustumSource> frustumSource = vtkSmartPointer<vtkFrustumSource>::New();
+    frustumSource->SetPlanes(planes);
+    frustumSource->Update();
+
+    // Extract the edges
+    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+    filter->SetInput(frustumSource->GetOutput());
+    filter->Update();
+    
+    ApplyPath::applyPath(filter->GetOutput(), appendFilter, path);
     
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
     mapper->SetInput(appendFilter->GetOutput());

diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp
index 2fe9ca1..66f8118 100644 (file)
--- a/modules/viz/test/test_viz3d.cpp
+++ b/modules/viz/test/test_viz3d.cpp
@@ -89,7 +89,9 @@ TEST(Viz_viz3d, accuracy)
     viz::Color color = viz::Color::black();
 
     viz::LineWidget lw(Point3f(0, 0, 0), Point3f(4.f, 4.f,4.f), viz::Color::green());
-    viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0), 5.0);
+    viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0));
+    viz::PlaneWidget pw2(Vec4f(0.0,1.0,2.0,3.0), 2.0, viz::Color::red());
+    viz::PlaneWidget pw3(Vec4f(0.0,1.0,2.0,3.0), 3.0, viz::Color::blue());
     viz::SphereWidget sw(Point3f(0, 0, 0), 0.2);
     viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), 0.01, viz::Color::red());
     viz::CircleWidget cw(Point3f(0, 0, 0), 0.5, 0.01, viz::Color::green());
@@ -101,8 +103,10 @@ TEST(Viz_viz3d, accuracy)
     viz::CloudWidget pcw2(cloud, viz::Color::magenta());
     
 //     viz.showWidget("line", lw);
-//     viz.showWidget("plane", pw);
-    viz.showWidget("sphere", sw);
+    viz.showWidget("plane", pw);
+    viz.showWidget("plane2", pw2);
+    viz.showWidget("plane3", pw3);
+//     viz.showWidget("sphere", sw);
 //     viz.showWidget("arrow", aw);
 //     viz.showWidget("circle", cw);
 //     viz.showWidget("cylinder", cyw);
@@ -145,16 +149,33 @@ TEST(Viz_viz3d, accuracy)
     viz::CameraPositionWidget cpw(Vec3f(0.5, 0.5, 3.0), Vec3f(0.0,0.0,0.0), Vec3f(0.0,-1.0,0.0), 0.5);
     viz::CameraPositionWidget cpw2(0.5);
     viz::CameraPositionWidget frustum(K, 2.0, viz::Color::green());
+//     viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
     viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
+    viz::CameraPositionWidget frustum3(Vec2f(CV_PI, CV_PI/2), 4.0);
     viz::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1);
     viz::Text3DWidget t3w2("Camera2", Point3f(0,0,0), 0.1);
     
-    viz.showWidget("CameraPositionWidget", cpw);
-    viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
-    viz.showWidget("camera_label", t3w1);
-    viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
-    viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
-    viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("CameraPositionWidget", cpw);
+//     viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("camera_label", t3w1);
+//     viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustum3", frustum3, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+    
+    std::vector<Affine3f> trajectory;
+    
+    trajectory.push_back(Affine3f().translate(Vec3f(0.5,0.5,0.5)));
+    trajectory.push_back(Affine3f().translate(Vec3f(1.0,0.0,0.0)));
+    trajectory.push_back(Affine3f().translate(Vec3f(2.0,0.5,0.0)));
+    trajectory.push_back(Affine3f(0.5, 0.0, 0.0, Vec3f(1.0,0.0,1.0)));
+//     
+    viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory, viz::Color(0,255,255), true, 0.5));
+    viz.showWidget("trajectory2", viz::TrajectoryWidget(trajectory, K, 1.0, viz::Color(255,0,255)));
+    
+    
+    
+//     viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory/*, viz::Color::yellow()*/));
     
 //     viz.showWidget("CameraPositionWidget2", cpw2);
 //     viz.showWidget("CameraPositionWidget3", cpw3);