From 16c8c98895d3a29ba1b20904c40ed4363972568b Mon Sep 17 00:00:00 2001
From: ozantonkal <ozantonkal@gmail.com>
Date: Mon, 22 Jul 2013 14:11:06 +0200
Subject: [PATCH] trajectory widget with frustums

---
 modules/viz/include/opencv2/viz/widgets.hpp |  3 +-
 modules/viz/src/shape_widgets.cpp           | 63 ++++++++++++++++++++-
 2 files changed, 63 insertions(+), 3 deletions(-)

diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp
index 2a7b67834..465517b7d 100644
--- a/modules/viz/include/opencv2/viz/widgets.hpp
+++ b/modules/viz/include/opencv2/viz/widgets.hpp
@@ -177,8 +177,7 @@ 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, double scale = 1.0);
-            TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K); // Camera frustums
+            TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
         };
 
         class CV_EXPORTS CloudWidget : public Widget3D
diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp
index 90a0cf7b2..13241073e 100644
--- a/modules/viz/src/shape_widgets.cpp
+++ b/modules/viz/src/shape_widgets.cpp
@@ -908,7 +908,6 @@ cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, double sca
     frustumSource->SetPlanes(planes);
     frustumSource->Update();
 
-    // Extract the edges so we have the grid
     vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
     filter->SetInput(frustumSource->GetOutput());
     filter->Update();
@@ -1060,4 +1059,66 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
     WidgetAccessor::setProp(*this, actor);
 }
 
+cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale, const Color &color)
+{
+    vtkIdType nr_points = path.size();
+    
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+    
+    vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
+    float f_x = K(0,0);
+    float f_y = K(1,1);
+    float c_y = K(1,2);
+    float 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)
+    float fovy = 2.0f * 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->SetClippingRange(0.01, scale);
+    
+    double planesArray[24];
+    camera->GetFrustumPlanes(aspect_ratio, planesArray);
+    
+    vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+    mat_trans->Identity();
+    
+    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<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    mapper->SetInput(appendFilter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}