From 64cdd821466b6440b6c93b0093c321b82b3b3a70 Mon Sep 17 00:00:00 2001
From: ozantonkal <ozantonkal@gmail.com>
Date: Thu, 25 Jul 2013 15:08:45 +0200
Subject: [PATCH] trajectory widget constructor with field of view

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

diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp
index c63c54f68..2def82747 100644
--- a/modules/viz/include/opencv2/viz/widgets.hpp
+++ b/modules/viz/include/opencv2/viz/widgets.hpp
@@ -184,6 +184,7 @@ namespace cv
             
             TrajectoryWidget(const std::vector<Affine3f> &path, int display_mode = TrajectoryWidget::DISPLAY_PATH, const Color &color = Color::white(), double scale = 1.0);
             TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
+            TrajectoryWidget(const std::vector<Affine3f> &path, const Vec2f &fov, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
             
         private:
             struct ApplyPath;
diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp
index fc13e0f3e..8dea213f9 100644
--- a/modules/viz/src/shape_widgets.cpp
+++ b/modules/viz/src/shape_widgets.cpp
@@ -1248,8 +1248,6 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, i
 
 cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale, const Color &color)
 {   
-    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
-    
     vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
     float f_x = K(0,0);
     float f_y = K(1,1);
@@ -1279,6 +1277,47 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, c
     filter->SetInput(frustumSource->GetOutput());
     filter->Update();
     
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+    ApplyPath::applyPath(filter->GetOutput(), appendFilter, path);
+    
+    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);
+}
+
+cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Vec2f &fov, double scale, const Color &color)
+{
+    vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
+    
+    camera->SetViewAngle(fov[1] * 180 / CV_PI); // Vertical field of view
+    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 aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5);
+    
+    double planesArray[24];
+    camera->GetFrustumPlanes(aspect_ratio, planesArray);
+    
+    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();
+    
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
     ApplyPath::applyPath(filter->GetOutput(), appendFilter, path);
     
     vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();