Merge pull request #17 from ozantonkal/implementing_widgets

Implementing widgets

modules/viz/include/opencv2/viz.hpp

@@ -64,7 +64,7 @@ namespace cv
         CV_EXPORTS Affine3f makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& axis_y, const Vec3f& axis_z, const Vec3f& origin = Vec3f::all(0));
 
         //! constructs camera pose from position, focal_point and up_vector (see gluLookAt() for more infromation
-        CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& up_vector);
+        CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir);
 
 
         //! checks float value for Nan

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <opencv2/viz/types.hpp>
+#include <common.h>
 
 
 namespace cv
@@ -70,6 +71,8 @@ namespace cv
         public:
             PlaneWidget(const Vec4f& coefs, double size = 1.0, const Color &color = Color::white());
             PlaneWidget(const Vec4f& coefs, const Point3f& pt, double size = 1.0, const Color &color = Color::white());
+        private:
+            struct SetSizeImpl;
         };
 
         class CV_EXPORTS SphereWidget : public Widget3D
@@ -81,7 +84,7 @@ namespace cv
         class CV_EXPORTS ArrowWidget : public Widget3D
         {
         public:
-            ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color = Color::white());
+            ArrowWidget(const Point3f& pt1, const Point3f& pt2, double thickness = 0.03, const Color &color = Color::white());
         };
 
         class CV_EXPORTS CircleWidget : public Widget3D
@@ -120,7 +123,12 @@ namespace cv
         class CV_EXPORTS GridWidget : public Widget3D
         {
         public:
-            GridWidget(Vec2i dimensions, Vec2d spacing, const Color &color = Color::white());
+            GridWidget(const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white());
+            GridWidget(const Vec4f &coeffs, const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white());
+            
+        private:
+            struct GridImpl;
+        
         };
 
         class CV_EXPORTS Text3DWidget : public Widget3D
@@ -140,6 +148,48 @@ namespace cv
             void setText(const String &text);
             String getText() const;
         };
+        
+        class CV_EXPORTS ImageOverlayWidget : public Widget2D
+        {
+        public:
+            ImageOverlayWidget(const Mat &image, const Rect &rect);
+            
+            void setImage(const Mat &image);
+        };
+        
+        class CV_EXPORTS Image3DWidget : public Widget3D
+        {
+        public:
+            Image3DWidget(const Mat &image, const Size &size);
+            Image3DWidget(const Vec3f &position, const Vec3f &normal, const Vec3f &up_vector, const Mat &image, const Size &size);
+            
+            void setImage(const Mat &image);
+        };
+        
+        class CV_EXPORTS CameraPositionWidget : public Widget3D
+        {
+        public:
+            CameraPositionWidget(double scale = 1.0);
+            CameraPositionWidget(const Matx33f &K, double scale = 1.0, const Color &color = Color::white());
+            CameraPositionWidget(const Vec2f &fov, double scale = 1.0, const Color &color = Color::white());
+            CameraPositionWidget(const Matx33f &K, const Mat &img, double scale = 1.0, const Color &color = Color::white());
+            
+        };
+        
+        class CV_EXPORTS TrajectoryWidget : public Widget3D
+        {
+        public:
+            enum {DISPLAY_FRAMES = 1, DISPLAY_PATH = 2};
+            
+            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, float line_length, double init_sphere_radius,
+                             double sphere_radius, const Color &line_color = Color::white(), const Color &sphere_color = Color::white());
+            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;
+        };
 
         class CV_EXPORTS CloudWidget : public Widget3D
         {
@@ -151,6 +201,18 @@ namespace cv
             struct CreateCloudWidget;
         };
 
+        class CV_EXPORTS CloudCollectionWidget : public Widget3D
+        {
+        public:
+            CloudCollectionWidget();
+            
+            void addCloud(InputArray cloud, InputArray colors, const Affine3f &pose = Affine3f::Identity());
+            void addCloud(InputArray cloud, const Color &color = Color::white(), const Affine3f &pose = Affine3f::Identity());
+            
+        private:
+            struct CreateCloudWidget;
+        };
+        
         class CV_EXPORTS CloudNormalsWidget : public Widget3D
         {
         public:
@@ -183,7 +245,12 @@ namespace cv
         template<> CV_EXPORTS GridWidget Widget::cast<GridWidget>();
         template<> CV_EXPORTS Text3DWidget Widget::cast<Text3DWidget>();
         template<> CV_EXPORTS TextWidget Widget::cast<TextWidget>();
+        template<> CV_EXPORTS ImageOverlayWidget Widget::cast<ImageOverlayWidget>();
+        template<> CV_EXPORTS Image3DWidget Widget::cast<Image3DWidget>();
+        template<> CV_EXPORTS CameraPositionWidget Widget::cast<CameraPositionWidget>();
+        template<> CV_EXPORTS TrajectoryWidget Widget::cast<TrajectoryWidget>();
         template<> CV_EXPORTS CloudWidget Widget::cast<CloudWidget>();
+        template<> CV_EXPORTS CloudCollectionWidget Widget::cast<CloudCollectionWidget>();
         template<> CV_EXPORTS CloudNormalsWidget Widget::cast<CloudNormalsWidget>();
         template<> CV_EXPORTS MeshWidget Widget::cast<MeshWidget>();
 

modules/viz/src/cloud_widgets.cpp

@@ -116,7 +116,7 @@ cv::viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
 
     // Filter colors
     Vec3b* colors_data = new Vec3b[nr_points];
-    NanFilter::copy(colors, colors_data, cloud);
+    NanFilter::copyColor(colors, colors_data, cloud);
 
     vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
     scalars->SetNumberOfComponents (3);
@@ -154,7 +154,6 @@ cv::viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color)
     Mat cloud = _cloud.getMat();
     CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
 
-
     vtkIdType nr_points;
     vtkSmartPointer<vtkPolyData> polydata = CreateCloudWidget::create(cloud, nr_points);
 
@@ -184,6 +183,227 @@ template<> cv::viz::CloudWidget cv::viz::Widget::cast<cv::viz::CloudWidget>()
     return static_cast<CloudWidget&>(widget);
 }
 
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// Cloud Collection Widget implementation
+
+struct cv::viz::CloudCollectionWidget::CreateCloudWidget
+{
+    static inline vtkSmartPointer<vtkPolyData> create(const Mat &cloud, vtkIdType &nr_points)
+    {
+        vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New ();
+        vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
+
+        polydata->SetVerts (vertices);
+
+        vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
+        vtkSmartPointer<vtkIdTypeArray> initcells;
+        nr_points = cloud.total();
+
+        if (!points)
+        {
+            points = vtkSmartPointer<vtkPoints>::New ();
+            if (cloud.depth() == CV_32F)
+                points->SetDataTypeToFloat();
+            else if (cloud.depth() == CV_64F)
+                points->SetDataTypeToDouble();
+            polydata->SetPoints (points);
+        }
+        points->SetNumberOfPoints (nr_points);
+
+        if (cloud.depth() == CV_32F)
+        {
+            // Get a pointer to the beginning of the data array
+            Vec3f *data_beg = vtkpoints_data<float>(points);
+            Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
+            nr_points = data_end - data_beg;
+        }
+        else if (cloud.depth() == CV_64F)
+        {
+            // Get a pointer to the beginning of the data array
+            Vec3d *data_beg = vtkpoints_data<double>(points);
+            Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud);
+            nr_points = data_end - data_beg;
+        }
+        points->SetNumberOfPoints (nr_points);
+
+        // Update cells
+        vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
+        // If no init cells and cells has not been initialized...
+        if (!cells)
+            cells = vtkSmartPointer<vtkIdTypeArray>::New ();
+
+        // If we have less values then we need to recreate the array
+        if (cells->GetNumberOfTuples () < nr_points)
+        {
+            cells = vtkSmartPointer<vtkIdTypeArray>::New ();
+
+            // If init cells is given, and there's enough data in it, use it
+            if (initcells && initcells->GetNumberOfTuples () >= nr_points)
+            {
+                cells->DeepCopy (initcells);
+                cells->SetNumberOfComponents (2);
+                cells->SetNumberOfTuples (nr_points);
+            }
+            else
+            {
+                // If the number of tuples is still too small, we need to recreate the array
+                cells->SetNumberOfComponents (2);
+                cells->SetNumberOfTuples (nr_points);
+                vtkIdType *cell = cells->GetPointer (0);
+                // Fill it with 1s
+                std::fill_n (cell, nr_points * 2, 1);
+                cell++;
+                for (vtkIdType i = 0; i < nr_points; ++i, cell += 2)
+                    *cell = i;
+                // Save the results in initcells
+                initcells = vtkSmartPointer<vtkIdTypeArray>::New ();
+                initcells->DeepCopy (cells);
+            }
+        }
+        else
+        {
+            // The assumption here is that the current set of cells has more data than needed
+            cells->SetNumberOfComponents (2);
+            cells->SetNumberOfTuples (nr_points);
+        }
+
+        // Set the cells and the vertices
+        vertices->SetCells (nr_points, cells);
+        return polydata;
+    }
+    
+    static void createMapper(vtkSmartPointer<vtkLODActor> actor, vtkSmartPointer<vtkPolyData> poly_data, Vec3d& minmax)
+    {
+        vtkDataSetMapper *mapper = vtkDataSetMapper::SafeDownCast(actor->GetMapper());
+        if (!mapper)
+        {
+            // This is the first cloud
+            vtkSmartPointer<vtkDataSetMapper> mapper_new = vtkSmartPointer<vtkDataSetMapper>::New ();
+            mapper_new->SetInputConnection (poly_data->GetProducerPort());
+
+            mapper_new->SetScalarRange(minmax.val);
+            mapper_new->SetScalarModeToUsePointData ();
+
+            bool interpolation = (poly_data && poly_data->GetNumberOfCells () != poly_data->GetNumberOfVerts ());
+
+            mapper_new->SetInterpolateScalarsBeforeMapping(interpolation);
+            mapper_new->ScalarVisibilityOn();
+            mapper_new->ImmediateModeRenderingOff();
+            
+            actor->SetNumberOfCloudPoints (int (std::max<vtkIdType>(1, poly_data->GetNumberOfPoints () / 10)));
+            actor->GetProperty()->SetInterpolationToFlat();
+            actor->GetProperty()->BackfaceCullingOn();
+            actor->SetMapper(mapper_new);
+            return ;
+        }
+        
+        vtkPolyData *data = vtkPolyData::SafeDownCast(mapper->GetInput());
+        CV_Assert(data);
+        
+        vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+        appendFilter->AddInputConnection(mapper->GetInput()->GetProducerPort());
+        appendFilter->AddInputConnection(poly_data->GetProducerPort());
+        mapper->SetInputConnection(appendFilter->GetOutputPort());
+        
+        // Update the number of cloud points
+        vtkIdType old_cloud_points = actor->GetNumberOfCloudPoints();
+        actor->SetNumberOfCloudPoints (int (std::max<vtkIdType>(1, old_cloud_points+poly_data->GetNumberOfPoints () / 10)));
+    }
+};
+
+cv::viz::CloudCollectionWidget::CloudCollectionWidget()
+{
+    // Just create the actor
+    vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
+    WidgetAccessor::setProp(*this, actor);
+}
+
+void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, InputArray _colors, const Affine3f &pose)
+{
+    Mat cloud = _cloud.getMat();
+    Mat colors = _colors.getMat();
+    CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
+    CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
+    
+    if (cloud.isContinuous() && colors.isContinuous())
+    {
+        cloud.reshape(cloud.channels(), 1);
+        colors.reshape(colors.channels(), 1);
+    }
+
+    vtkIdType nr_points;
+    vtkSmartPointer<vtkPolyData> polydata =  CreateCloudWidget::create(cloud, nr_points);
+
+    // Filter colors
+    Vec3b* colors_data = new Vec3b[nr_points];
+    NanFilter::copyColor(colors, colors_data, cloud);
+
+    vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+    scalars->SetNumberOfComponents (3);
+    scalars->SetNumberOfTuples (nr_points);
+    scalars->SetArray (colors_data->val, 3 * nr_points, 0);
+
+    // Assign the colors
+    polydata->GetPointData ()->SetScalars (scalars);
+    
+    // Transform the poly data based on the pose
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->SetMatrix(convertToVtkMatrix(pose.matrix));
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(polydata->GetProducerPort());
+    transform_filter->Update();
+    
+    vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this));
+    CV_Assert(actor);
+    
+    Vec3d minmax(scalars->GetRange());
+    CreateCloudWidget::createMapper(actor, transform_filter->GetOutput(), minmax);
+}
+
+void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, const Color &color, const Affine3f &pose)
+{
+    Mat cloud = _cloud.getMat();
+    CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
+
+    vtkIdType nr_points;
+    vtkSmartPointer<vtkPolyData> polydata =  CreateCloudWidget::create(cloud, nr_points);
+
+    vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+    scalars->SetNumberOfComponents (3);
+    scalars->SetNumberOfTuples (nr_points);
+    scalars->FillComponent(0, color[2]);
+    scalars->FillComponent(1, color[1]);
+    scalars->FillComponent(2, color[0]);
+
+    // Assign the colors
+    polydata->GetPointData ()->SetScalars (scalars);
+    
+    // Transform the poly data based on the pose
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->SetMatrix(convertToVtkMatrix(pose.matrix));
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(polydata->GetProducerPort());
+    transform_filter->Update();
+    
+    vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this));
+    CV_Assert(actor);
+    
+    Vec3d minmax(scalars->GetRange());
+    CreateCloudWidget::createMapper(actor, transform_filter->GetOutput(), minmax);
+}
+
+template<> cv::viz::CloudCollectionWidget cv::viz::Widget::cast<cv::viz::CloudCollectionWidget>()
+{
+    Widget3D widget = this->cast<Widget3D>();
+    return static_cast<CloudCollectionWidget&>(widget);
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// Cloud Normals Widget implementation
 
@@ -359,7 +579,8 @@ cv::viz::MeshWidget::MeshWidget(const Mesh3d &mesh)
     
     vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
     vtkIdType nr_points = mesh.cloud.total();
-    int * look_up = new int[nr_points];
+    Mat look_up_mat(1, nr_points, CV_32SC1);
+    int * look_up = look_up_mat.ptr<int>();
     points->SetNumberOfPoints (nr_points);
       
     // Copy data from cloud to vtkPoints
@@ -384,7 +605,7 @@ cv::viz::MeshWidget::MeshWidget(const Mesh3d &mesh)
     {
         Vec3b * colors_data = 0;
         colors_data = new Vec3b[nr_points];
-        NanFilter::copy(mesh.colors, colors_data, mesh.cloud);
+        NanFilter::copyColor(mesh.colors, colors_data, mesh.cloud);
         
         scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
         scalars->SetNumberOfComponents (3);

modules/viz/src/precomp.hpp

@@ -43,6 +43,7 @@
 #include <vtkPlaneSource.h>
 #include <vtkSphereSource.h>
 #include <vtkArrowSource.h>
+#include <vtkOutlineSource.h>
 #include <vtkIdentityTransform.h>
 #include <vtkTransform.h>
 #include <vtkTransformPolyDataFilter.h>
@@ -107,6 +108,7 @@
 #include <vtkImageCanvasSource2D.h>
 #include <vtkImageBlend.h>
 #include <vtkImageStencilData.h>
+#include <vtkImageActor.h>
 
 #include <vtkRenderWindowInteractor.h>
 #include <vtkChartXY.h>
@@ -131,7 +133,9 @@
 #include <vtkImageReader2Factory.h>
 #include <vtkImageReader2.h>
 #include <vtkImageData.h>
-
+#include <vtkExtractEdges.h>
+#include <vtkFrustumSource.h>
+#include <vtkTextureMapToPlane.h>
 
 #include <vtkPolyDataNormals.h>
 #include <vtkMapper.h>

modules/viz/src/shape_widgets.cpp

@@ -50,6 +50,26 @@ template<> cv::viz::LineWidget cv::viz::Widget::cast<cv::viz::LineWidget>()
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// plane widget implementation
 
+struct cv::viz::PlaneWidget::SetSizeImpl
+{
+    template<typename _Tp>
+    static vtkSmartPointer<vtkPolyData> setSize(const Vec<_Tp, 3> &center, vtkSmartPointer<vtkPolyData> poly_data, double size)
+    {
+        vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+        transform->PreMultiply();
+        transform->Translate(center[0], center[1], center[2]);
+        transform->Scale(size, size, size);
+        transform->Translate(-center[0], -center[1], -center[2]);
+        
+        vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+        transform_filter->SetInput(poly_data);
+        transform_filter->SetTransform(transform);
+        transform_filter->Update();
+        
+        return transform_filter->GetOutput();
+    }
+};
+
 cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, double size, const Color &color)
 {    
     vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
@@ -57,12 +77,14 @@ cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, double size, const Color &
     double norm = cv::norm(Vec3f(coefs.val));
     plane->Push (-coefs[3] / norm);
     
+    Vec3d p_center;
+    plane->GetOrigin(p_center.val);
+    
     vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
-    mapper->SetInput(plane->GetOutput ());
+    mapper->SetInput(SetSizeImpl::setSize(p_center, plane->GetOutput(), size));
     
     vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
     actor->SetMapper(mapper);
-    actor->SetScale(size);
     
     WidgetAccessor::setProp(*this, actor);
     setColor(color);
@@ -80,11 +102,10 @@ cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, const Point3f& pt, double
     plane->SetCenter (p_center[0], p_center[1], p_center[2]);
     
     vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
-    mapper->SetInput(plane->GetOutput ());
+    mapper->SetInput(SetSizeImpl::setSize(p_center, plane->GetOutput(), size));
 
     vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
     actor->SetMapper(mapper);
-    actor->SetScale(size);
     
     WidgetAccessor::setProp(*this, actor);
     setColor(color);
@@ -128,9 +149,13 @@ template<> cv::viz::SphereWidget cv::viz::Widget::cast<cv::viz::SphereWidget>()
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// arrow widget implementation
 
-cv::viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color)
+cv::viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& 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);
     
     float startPoint[3], endPoint[3];
     startPoint[0] = pt1.x;
@@ -265,19 +290,24 @@ template<> cv::viz::CylinderWidget cv::viz::Widget::cast<cv::viz::CylinderWidget
 /// cylinder widget implementation
 
 cv::viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, bool wire_frame, const Color &color)
-{
-    vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
-    cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z);
+{   
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();   
+    if (wire_frame)
+    {
+        vtkSmartPointer<vtkOutlineSource> cube = vtkSmartPointer<vtkOutlineSource>::New();
+        cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z);
+        mapper->SetInput(cube->GetOutput ());
+    }
+    else
+    {
+        vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+        cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z);
+        mapper->SetInput(cube->GetOutput ());
+    }
     
-    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
-    mapper->SetInput(cube->GetOutput ());
-
     vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
     actor->SetMapper(mapper);
     
-    if (wire_frame)
-        actor->GetProperty ()->SetRepresentationToWireframe ();
-    
     WidgetAccessor::setProp(*this, actor);
     setColor(color);
 }
@@ -410,26 +440,88 @@ template<> cv::viz::PolyLineWidget cv::viz::Widget::cast<cv::viz::PolyLineWidget
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// grid widget implementation
 
-cv::viz::GridWidget::GridWidget(Vec2i dimensions, Vec2d spacing, const Color &color)
+struct cv::viz::GridWidget::GridImpl
 {
-    // Create the grid using image data
-    vtkSmartPointer<vtkImageData> grid = vtkSmartPointer<vtkImageData>::New();
+    static vtkSmartPointer<vtkPolyData> createGrid(const Vec2i &dimensions, const Vec2d &spacing)
+    {
+        // Create the grid using image data
+        vtkSmartPointer<vtkImageData> grid = vtkSmartPointer<vtkImageData>::New();
+        
+        // Add 1 to dimensions because in ImageData dimensions is the number of lines
+        // - however here it means number of cells
+        grid->SetDimensions(dimensions[0]+1, dimensions[1]+1, 1);
+        grid->SetSpacing(spacing[0], spacing[1], 0.);
+        
+        // Set origin of the grid to be the middle of the grid
+        grid->SetOrigin(dimensions[0] * spacing[0] * (-0.5), dimensions[1] * spacing[1] * (-0.5), 0);
+        
+        // Extract the edges so we have the grid
+        vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+        filter->SetInputConnection(grid->GetProducerPort());
+        filter->Update();
+        return filter->GetOutput();
+    }
+};
+
+cv::viz::GridWidget::GridWidget(const Vec2i &dimensions, const Vec2d &spacing, const Color &color)
+{
+    vtkSmartPointer<vtkPolyData> grid = GridImpl::createGrid(dimensions, spacing);
+    
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
+    mapper->SetInputConnection(grid->GetProducerPort());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}
+
+cv::viz::GridWidget::GridWidget(const Vec4f &coefs, const Vec2i &dimensions, const Vec2d &spacing, const Color &color)
+{
+    vtkSmartPointer<vtkPolyData> grid = GridImpl::createGrid(dimensions, spacing);
+    
+    // Estimate the transform to set the normal based on the coefficients
+    Vec3f normal(coefs[0], coefs[1], coefs[2]);
+    Vec3f up_vector(0.0f, 1.0f, 0.0f); // Just set as default
+    double push_distance = -coefs[3]/cv::norm(Vec3f(coefs.val));
+    Vec3f u,v,n;
+    n = normalize(normal);
+    u = normalize(up_vector.cross(n));
+    v = n.cross(u);
+    
+    vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+    mat_trans->SetElement(0,0,u[0]);
+    mat_trans->SetElement(0,1,u[1]);
+    mat_trans->SetElement(0,2,u[2]);
+    mat_trans->SetElement(1,0,v[0]);
+    mat_trans->SetElement(1,1,v[1]);
+    mat_trans->SetElement(1,2,v[2]);
+    mat_trans->SetElement(2,0,n[0]);
+    mat_trans->SetElement(2,1,n[1]);
+    mat_trans->SetElement(2,2,n[2]);
+    // Inverse rotation (orthogonal, so just take transpose)
+    mat_trans->Transpose();
+    mat_trans->SetElement(0,3,n[0] * push_distance);
+    mat_trans->SetElement(1,3,n[1] * push_distance);
+    mat_trans->SetElement(2,3,n[2] * push_distance);
+    mat_trans->SetElement(3,3,1);
     
-    // Add 1 to dimensions because in ImageData dimensions is the number of lines
-    // - however here it means number of cells
-    grid->SetDimensions(dimensions[0]+1, dimensions[1]+1, 1);
-    grid->SetSpacing(spacing[0], spacing[1], 0.);
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->SetMatrix(mat_trans);
     
-    // Set origin of the grid to be the middle of the grid
-    grid->SetOrigin(dimensions[0] * spacing[0] * (-0.5), dimensions[1] * spacing[1] * (-0.5), 0);
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(grid->GetProducerPort());
+    transform_filter->Update();
     
     vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
-    mapper->SetInput(grid);
+    mapper->SetInputConnection(transform_filter->GetOutputPort());
+    
     vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
     actor->SetMapper(mapper);
     
-    // Show it as wireframe
-    actor->GetProperty ()->SetRepresentationToWireframe ();
     WidgetAccessor::setProp(*this, actor);
     setColor(color);
 }
@@ -533,3 +625,729 @@ cv::String cv::viz::TextWidget::getText() const
     CV_Assert(actor);
     return actor->GetInput();
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// image overlay widget implementation
+
+cv::viz::ImageOverlayWidget::ImageOverlayWidget(const Mat &image, const Rect &rect)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    // Create the vtk image and set its parameters based on input image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    // Scale the image based on the Rect
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->Scale(double(image.cols)/rect.width,double(image.rows)/rect.height,1.0);
+    
+    vtkSmartPointer<vtkImageReslice> image_reslice = vtkSmartPointer<vtkImageReslice>::New();
+    image_reslice->SetResliceTransform(transform);
+    image_reslice->SetInputConnection(flipFilter->GetOutputPort());
+    image_reslice->SetOutputDimensionality(2);
+    image_reslice->InterpolateOn();
+    image_reslice->AutoCropOutputOn(); 
+    
+    vtkSmartPointer<vtkImageMapper> imageMapper = vtkSmartPointer<vtkImageMapper>::New();
+    imageMapper->SetInputConnection(image_reslice->GetOutputPort());
+    imageMapper->SetColorWindow(255); // OpenCV color
+    imageMapper->SetColorLevel(127.5);  
+    
+    vtkSmartPointer<vtkActor2D> actor = vtkSmartPointer<vtkActor2D>::New();
+    actor->SetMapper(imageMapper);
+    actor->SetPosition(rect.x, rect.y);
+    
+    WidgetAccessor::setProp(*this, actor);
+}
+
+void cv::viz::ImageOverlayWidget::setImage(const Mat &image)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    vtkActor2D *actor = vtkActor2D::SafeDownCast(WidgetAccessor::getProp(*this));
+    CV_Assert(actor);
+    
+    vtkImageMapper *mapper = vtkImageMapper::SafeDownCast(actor->GetMapper());
+    CV_Assert(mapper);
+    
+    // Create the vtk image and set its parameters based on input image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    mapper->SetInputConnection(flipFilter->GetOutputPort());
+}
+
+template<> cv::viz::ImageOverlayWidget cv::viz::Widget::cast<cv::viz::ImageOverlayWidget>()
+{
+    Widget2D widget = this->cast<Widget2D>();
+    return static_cast<ImageOverlayWidget&>(widget);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// image 3D widget implementation
+
+cv::viz::Image3DWidget::Image3DWidget(const Mat &image, const Size &size)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    // Create the vtk image and set its parameters based on input image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    Vec3d plane_center(size.width * 0.5, size.height * 0.5, 0.0);
+    
+    vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();
+    plane->SetCenter(plane_center[0], plane_center[1], plane_center[2]);
+    plane->SetNormal(0.0, 0.0, 1.0);
+    
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->Translate(plane_center[0], plane_center[1], plane_center[2]);
+    transform->Scale(size.width, size.height, 1.0);
+    transform->Translate(-plane_center[0], -plane_center[1], -plane_center[2]);
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(plane->GetOutputPort());
+    transform_filter->Update();
+    
+    // Apply the texture
+    vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New();
+    texture->SetInputConnection(flipFilter->GetOutputPort());
+    
+    vtkSmartPointer<vtkTextureMapToPlane> texturePlane = vtkSmartPointer<vtkTextureMapToPlane>::New();
+    texturePlane->SetInputConnection(transform_filter->GetOutputPort());
+    
+    vtkSmartPointer<vtkPolyDataMapper> planeMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    planeMapper->SetInputConnection(texturePlane->GetOutputPort());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(planeMapper);
+    actor->SetTexture(texture);
+     
+    WidgetAccessor::setProp(*this, actor);
+}
+
+cv::viz::Image3DWidget::Image3DWidget(const Vec3f &position, const Vec3f &normal, const Vec3f &up_vector, const Mat &image, const Size &size)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    // Create the vtk image and set its parameters based on input image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();
+    plane->SetCenter(0.0, 0.0, 0.0);
+    plane->SetNormal(0.0, 0.0, 1.0);    
+    
+    // Compute the transformation matrix for drawing the camera frame in a scene
+    Vec3f u,v,n;
+    n = normalize(normal);
+    u = normalize(up_vector.cross(n));
+    v = n.cross(u);
+    
+    vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+    mat_trans->SetElement(0,0,u[0]);
+    mat_trans->SetElement(0,1,u[1]);
+    mat_trans->SetElement(0,2,u[2]);
+    mat_trans->SetElement(1,0,v[0]);
+    mat_trans->SetElement(1,1,v[1]);
+    mat_trans->SetElement(1,2,v[2]);
+    mat_trans->SetElement(2,0,n[0]);
+    mat_trans->SetElement(2,1,n[1]);
+    mat_trans->SetElement(2,2,n[2]);
+    // Inverse rotation (orthogonal, so just take transpose)
+    mat_trans->Transpose(); 
+    // Then translate the coordinate frame to camera position
+    mat_trans->SetElement(0,3,position[0]);
+    mat_trans->SetElement(1,3,position[1]);
+    mat_trans->SetElement(2,3,position[2]);
+    mat_trans->SetElement(3,3,1);
+    
+    // Apply the texture
+    vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New();
+    texture->SetInputConnection(flipFilter->GetOutputPort());
+    
+    vtkSmartPointer<vtkTextureMapToPlane> texturePlane = vtkSmartPointer<vtkTextureMapToPlane>::New();
+    texturePlane->SetInputConnection(plane->GetOutputPort());
+    
+    // Apply the transform after texture mapping
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->SetMatrix(mat_trans);
+    transform->Scale(size.width, size.height, 1.0);
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(texturePlane->GetOutputPort());
+    transform_filter->Update();
+    
+    vtkSmartPointer<vtkPolyDataMapper> planeMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    planeMapper->SetInputConnection(transform_filter->GetOutputPort());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(planeMapper);
+    actor->SetTexture(texture);
+     
+    WidgetAccessor::setProp(*this, actor);
+}
+
+void cv::viz::Image3DWidget::setImage(const Mat &image)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    vtkActor *actor = vtkActor::SafeDownCast(WidgetAccessor::getProp(*this));
+    CV_Assert(actor);
+    
+    // Create the vtk image and set its parameters based on input image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    // Apply the texture
+    vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New();
+    texture->SetInputConnection(flipFilter->GetOutputPort());
+    
+    actor->SetTexture(texture);
+}
+
+template<> cv::viz::Image3DWidget cv::viz::Widget::cast<cv::viz::Image3DWidget>()
+{
+    Widget3D widget = this->cast<Widget3D>();
+    return static_cast<Image3DWidget&>(widget);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// camera position widget implementation
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(double scale)
+{
+    vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
+    axes->SetOrigin (0, 0, 0);
+    axes->SetScaleFactor (scale);
+    
+    vtkSmartPointer<vtkFloatArray> axes_colors = vtkSmartPointer<vtkFloatArray>::New ();
+    axes_colors->Allocate (6);
+    axes_colors->InsertNextValue (0.0);
+    axes_colors->InsertNextValue (0.0);
+    axes_colors->InsertNextValue (0.5);
+    axes_colors->InsertNextValue (0.5);
+    axes_colors->InsertNextValue (1.0);
+    axes_colors->InsertNextValue (1.0);
+
+    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);
+    
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetScalarModeToUsePointData ();
+    mapper->SetInput(axes_tubes->GetOutput ());
+
+    vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+}
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, double scale, const Color &color)
+{
+    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<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
+    planes->SetFrustumPlanes(planesArray);
+    
+    vtkSmartPointer<vtkFrustumSource> frustumSource =
+    vtkSmartPointer<vtkFrustumSource>::New();
+    frustumSource->SetPlanes(planes);
+    frustumSource->Update();
+
+    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+    filter->SetInput(frustumSource->GetOutput());
+    filter->Update();
+    
+    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    mapper->SetInput(filter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}
+
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(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 so we have the grid
+    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+    filter->SetInput(frustumSource->GetOutput());
+    filter->Update();    
+    
+    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    mapper->SetInput(filter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, const Mat &image, double scale, const Color &color)
+{
+    CV_Assert(!image.empty() && image.depth() == CV_8U);
+    
+    // Create a camera
+    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 = float(image.cols)/float(image.rows);
+    // 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;
+    float far_end_height = 2.0f * c_y * scale / f_y;
+    
+    // Create the vtk image
+    vtkSmartPointer<vtkImageData> vtk_image = vtkSmartPointer<vtkImageData>::New();
+    ConvertToVtkImage::convert(image, vtk_image);
+    
+    // Adjust a pixel of the vtk_image
+    vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 0, color[2]);
+    vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 1, color[1]);
+    vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 2, color[0]);
+    
+    // Need to flip the image as the coordinates are different in OpenCV and VTK
+    vtkSmartPointer<vtkImageFlip> flipFilter = vtkSmartPointer<vtkImageFlip>::New();
+    flipFilter->SetFilteredAxis(1); // Vertical flip
+    flipFilter->SetInputConnection(vtk_image->GetProducerPort());
+    flipFilter->Update();
+    
+    Vec3d plane_center(0.0, 0.0, scale);
+    
+    vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();
+    plane->SetCenter(plane_center[0], plane_center[1], plane_center[2]);
+    plane->SetNormal(0.0, 0.0, 1.0);
+    
+    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
+    transform->PreMultiply();
+    transform->Translate(plane_center[0], plane_center[1], plane_center[2]);
+    transform->Scale(far_end_height*aspect_ratio, far_end_height, 1.0);
+    transform->RotateY(180.0);
+    transform->Translate(-plane_center[0], -plane_center[1], -plane_center[2]);
+    
+    // Apply the texture
+    vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New();
+    texture->SetInputConnection(flipFilter->GetOutputPort());
+    
+    vtkSmartPointer<vtkTextureMapToPlane> texturePlane = vtkSmartPointer<vtkTextureMapToPlane>::New();
+    texturePlane->SetInputConnection(plane->GetOutputPort());
+    
+    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transform_filter->SetTransform(transform);
+    transform_filter->SetInputConnection(texturePlane->GetOutputPort());
+    transform_filter->Update();
+    
+    // Create frustum
+    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<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
+    planes->SetFrustumPlanes(planesArray);
+    
+    vtkSmartPointer<vtkFrustumSource> frustumSource =
+    vtkSmartPointer<vtkFrustumSource>::New();
+    frustumSource->SetPlanes(planes);
+    frustumSource->Update();
+        
+    vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+    filter->SetInput(frustumSource->GetOutput());
+    filter->Update();
+    
+    // Frustum needs to be textured or else it can't be combined with image
+    vtkSmartPointer<vtkTextureMapToPlane> frustum_texture = vtkSmartPointer<vtkTextureMapToPlane>::New();
+    frustum_texture->SetInputConnection(filter->GetOutputPort());
+    // Texture mapping with only one pixel from the image to have constant color
+    frustum_texture->SetSRange(0.0, 0.0); 
+    frustum_texture->SetTRange(0.0, 0.0);
+    
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+    appendFilter->AddInputConnection(frustum_texture->GetOutputPort());
+    appendFilter->AddInputConnection(transform_filter->GetOutputPort());
+    
+    vtkSmartPointer<vtkPolyDataMapper> planeMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    planeMapper->SetInputConnection(appendFilter->GetOutputPort());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(planeMapper);
+    actor->SetTexture(texture);
+     
+    WidgetAccessor::setProp(*this, actor);
+}
+
+template<> cv::viz::CameraPositionWidget cv::viz::Widget::cast<cv::viz::CameraPositionWidget>()
+{
+    Widget3D widget = this->cast<Widget3D>();
+    return static_cast<CameraPositionWidget&>(widget);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// trajectory widget implementation
+
+struct cv::viz::TrajectoryWidget::ApplyPath
+{
+    static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3f> &path)
+    {   
+        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();
+            vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
+            mat_trans = convertToVtkMatrix(path[i].matrix);
+            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, int display_mode, const Color &color, double scale)
+{
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+    
+    // Bitwise and with 3 in order to limit the domain to 2 bits
+    if ((~display_mode & 3) ^ TrajectoryWidget::DISPLAY_PATH)
+    {
+        // Create a poly line along the path
+        vtkIdType nr_points = path.size();    
+    
+        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+        vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
+        vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+        
+        points->SetDataTypeToFloat();
+        points->SetNumberOfPoints(nr_points);
+        polyLine->GetPointIds()->SetNumberOfIds(nr_points);
+        
+        Vec3f *data_beg = vtkpoints_data<float>(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<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
+        cells->InsertNextCell(polyLine);
+        
+        polyData->SetPoints(points);
+        polyData->SetLines(cells);
+        
+        // Set the color for polyData
+        vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::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);
+        appendFilter->AddInputConnection(polyData->GetProducerPort());
+    }
+    
+    if ((~display_mode & 3) ^ TrajectoryWidget::DISPLAY_FRAMES)
+    {
+        // Create frames and transform along the path
+        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);
+        
+        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);
+    }
+    
+    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    mapper->SetScalarModeToUsePointData ();
+    mapper->SetInput(appendFilter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+}
+
+cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, float line_length, double init_sphere_radius,
+                                            double sphere_radius, const Color &line_color, const Color &sphere_color)
+{
+    vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
+    vtkIdType nr_poses = path.size();
+    
+    // Create color arrays
+    vtkSmartPointer<vtkUnsignedCharArray> line_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
+    line_scalars->SetNumberOfComponents(3);
+    line_scalars->InsertNextTuple3(line_color[2], line_color[1], line_color[0]);
+    
+    // Create color array for sphere
+    vtkSphereSource * dummy_sphere = vtkSphereSource::New();
+    // Create the array for big sphere
+    dummy_sphere->SetRadius(init_sphere_radius);
+    dummy_sphere->Update();
+    vtkIdType nr_points = dummy_sphere->GetOutput()->GetNumberOfCells();
+    vtkSmartPointer<vtkUnsignedCharArray> sphere_scalars_init = vtkSmartPointer<vtkUnsignedCharArray>::New();
+    sphere_scalars_init->SetNumberOfComponents(3);
+    sphere_scalars_init->SetNumberOfTuples(nr_points);
+    sphere_scalars_init->FillComponent(0, sphere_color[2]);
+    sphere_scalars_init->FillComponent(1, sphere_color[1]);
+    sphere_scalars_init->FillComponent(2, sphere_color[0]);
+    // Create the array for small sphere
+    dummy_sphere->SetRadius(sphere_radius);
+    dummy_sphere->Update();
+    nr_points = dummy_sphere->GetOutput()->GetNumberOfCells();
+    vtkSmartPointer<vtkUnsignedCharArray> sphere_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
+    sphere_scalars->SetNumberOfComponents(3);
+    sphere_scalars->SetNumberOfTuples(nr_points);
+    sphere_scalars->FillComponent(0, sphere_color[2]);
+    sphere_scalars->FillComponent(1, sphere_color[1]);
+    sphere_scalars->FillComponent(2, sphere_color[0]);
+    dummy_sphere->Delete();    
+        
+    for (vtkIdType i = 0; i < nr_poses; ++i)
+    {
+        Point3f new_pos = path[i].translation();
+        
+        vtkSmartPointer<vtkSphereSource> sphere_source = vtkSmartPointer<vtkSphereSource>::New();
+        sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z);
+        if (i == 0)
+        {
+            sphere_source->SetRadius(init_sphere_radius);
+            sphere_source->Update();
+            sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init);
+            appendFilter->AddInputConnection(sphere_source->GetOutputPort());
+            continue;
+        }
+        else
+        {
+            sphere_source->SetRadius(sphere_radius);
+            sphere_source->Update();
+            sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars);
+            appendFilter->AddInputConnection(sphere_source->GetOutputPort());
+        }
+        
+        
+        Affine3f relativeAffine = path[i].inv() * path[i-1];
+        Vec3f v = path[i].rotation() * relativeAffine.translation();
+        v = normalize(v) * line_length;
+        
+        vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::New();
+        line_source->SetPoint1(new_pos.x + v[0], new_pos.y + v[1], new_pos.z + v[2]);
+        line_source->SetPoint2(new_pos.x, new_pos.y, new_pos.z);
+        line_source->Update();
+        line_source->GetOutput()->GetCellData()->SetScalars(line_scalars);
+        
+        appendFilter->AddInputConnection(line_source->GetOutputPort());
+    }
+    
+    vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+    mapper->SetScalarModeToUseCellData();
+    mapper->SetInput(appendFilter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+}
+
+cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale, const Color &color)
+{   
+    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<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();
+    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();
+    mapper->SetInput(appendFilter->GetOutput());
+    
+    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+    actor->SetMapper(mapper);
+    
+    WidgetAccessor::setProp(*this, actor);
+    setColor(color);
+}
+
+template<> cv::viz::TrajectoryWidget cv::viz::Widget::cast<cv::viz::TrajectoryWidget>()
+{
+    Widget3D widget = this->cast<Widget3D>();
+    return static_cast<TrajectoryWidget&>(widget);
+}

modules/viz/src/types.cpp

@@ -111,7 +111,7 @@ struct cv::viz::Mesh3d::loadMeshImpl
                 Vec3b point_color;
                 poly_colors->GetTupleValue (i, point_color.val);
 
-                //RGB or BGR? should we swap channels????
+                std::swap(point_color[0], point_color[2]); // RGB -> BGR
                 mesh_colors[i] = point_color;
             }
         }

modules/viz/src/viz.cpp

@@ -19,6 +19,32 @@ cv::Affine3f cv::viz::makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& ax
     return Affine3f(R, origin);
 }
 
+cv::Affine3f cv::viz::makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir)
+{
+    // Compute the transformation matrix for drawing the camera frame in a scene
+    Vec3f u,v,n;
+    n = normalize(focal_point - position);
+    u = normalize(y_dir.cross(n));
+    v = n.cross(u);
+    
+    Matx44f pose_mat;
+    pose_mat.zeros();
+    pose_mat(0,0) = u[0];
+    pose_mat(0,1) = u[1];
+    pose_mat(0,2) = u[2];
+    pose_mat(1,0) = v[0];
+    pose_mat(1,1) = v[1];
+    pose_mat(1,2) = v[2];
+    pose_mat(2,0) = n[0];
+    pose_mat(2,1) = n[1];
+    pose_mat(2,2) = n[2];
+    pose_mat(3,0) = position[0];
+    pose_mat(3,1) = position[1];
+    pose_mat(3,2) = position[2];
+    pose_mat(3,3) = 1.0f;
+    pose_mat = pose_mat.t();
+    return pose_mat;
+}
 
 vtkSmartPointer<vtkMatrix4x4> cv::viz::convertToVtkMatrix (const cv::Matx44f &m)
 {

modules/viz/src/viz3d_impl.cpp

@@ -847,7 +847,7 @@ bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename,
     return (true);
 }
 
-bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& /*mesh*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
 // 
@@ -1017,7 +1017,7 @@ void cv::viz::Viz3d::VizImpl::setWindowName (const std::string &name)
 void cv::viz::Viz3d::VizImpl::setWindowPosition (int x, int y) { window_->SetPosition (x, y); }
 void cv::viz::Viz3d::VizImpl::setWindowSize (int xw, int yw) { window_->SetSize (xw, yw); }
 
-bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& /*mesh*/, const Mat& /*mask*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
 //     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
@@ -1205,7 +1205,7 @@ return true;
 }
 
 
-bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& /*mesh*/, const cv::Mat& /*mask*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
 //     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));

modules/viz/src/viz3d_impl.hpp

@@ -327,6 +327,31 @@ namespace cv
                     }
                     return output;
                 }
+                
+                static _Out* copyColor(const Mat& source, _Out* output, const Mat& nan_mask)
+                {
+                    CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
+                    CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
+                    CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
+
+                    int s_chs = source.channels();
+                    int m_chs = nan_mask.channels();
+
+                    for(int y = 0; y < source.rows; ++y)
+                    {
+                        const _Tp* srow = source.ptr<_Tp>(y);
+                        const _Msk* mrow = nan_mask.ptr<_Msk>(y);
+
+                        for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
+                            if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
+                            {
+                                *output = _Out(srow);
+                                std::swap((*output)[0], (*output)[2]); // BGR -> RGB
+                                ++output;
+                            }
+                    }
+                    return output;
+                }
             };
 
             template<typename _Tp>
@@ -339,6 +364,17 @@ namespace cv
 
                 return table[nan_mask.depth() - 5](source, output, nan_mask);
             }
+            
+            template<typename _Tp>
+            static inline Vec<_Tp, 3>* copyColor(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
+            {
+                CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
+
+                typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
+                const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copyColor, &NanFilter::Impl<_Tp, double>::copyColor };
+
+                return table[nan_mask.depth() - 5](source, output, nan_mask);
+            }
         };
 
         struct ApplyAffine
@@ -374,6 +410,63 @@ namespace cv
 
         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); }
+        
+        struct ConvertToVtkImage
+        {
+            struct Impl
+            {
+                static void copyImageMultiChannel(const Mat &image, vtkSmartPointer<vtkImageData> output)
+                {
+                    int i_chs = image.channels();
+            
+                    for (int i = 0; i < image.rows; ++i)
+                    {
+                        const unsigned char * irows = image.ptr<unsigned char>(i);
+                        for (int j = 0; j < image.cols; ++j, irows += i_chs)
+                        {
+                            unsigned char * vrows = static_cast<unsigned char *>(output->GetScalarPointer(j,i,0));
+                            memcpy(vrows, irows, i_chs);
+                            std::swap(vrows[0], vrows[2]); // BGR -> RGB
+                        }
+                    }
+                    output->Modified();
+                }
+                
+                static void copyImageSingleChannel(const Mat &image, vtkSmartPointer<vtkImageData> output)
+                {
+                    for (int i = 0; i < image.rows; ++i)
+                    {
+                        const unsigned char * irows = image.ptr<unsigned char>(i);
+                        for (int j = 0; j < image.cols; ++j, ++irows)
+                        {
+                            unsigned char * vrows = static_cast<unsigned char *>(output->GetScalarPointer(j,i,0));
+                            *vrows = *irows;
+                        }
+                    }
+                    output->Modified();
+                }
+            };
+            
+            static void convert(const Mat &image, vtkSmartPointer<vtkImageData> output)
+            {
+                // Create the vtk image
+                output->SetDimensions(image.cols, image.rows, 1);
+                output->SetNumberOfScalarComponents(image.channels());
+                output->SetScalarTypeToUnsignedChar();
+                output->AllocateScalars();
+                
+                int i_chs = image.channels();
+                if (i_chs > 1)
+                {
+                    // Multi channel images are handled differently because of BGR <-> RGB
+                    Impl::copyImageMultiChannel(image, output);
+                }
+                else
+                {
+                    Impl::copyImageSingleChannel(image, output);
+                }
+            }
+        };
     }
 
 }

modules/viz/src/widget.cpp

@@ -142,10 +142,6 @@ void cv::viz::Widget3D::setColor(const Color &color)
     actor->GetMapper ()->ScalarVisibilityOff ();
     actor->GetProperty ()->SetColor (c.val);
     actor->GetProperty ()->SetEdgeColor (c.val);
-    actor->GetProperty ()->SetAmbient (0.8);
-    actor->GetProperty ()->SetDiffuse (0.8);
-    actor->GetProperty ()->SetSpecular (0.8);
-    actor->GetProperty ()->SetLighting (0);
     actor->Modified ();
 }
 

modules/viz/test/test_viz3d.cpp

@@ -43,6 +43,7 @@
 #include <opencv2/viz.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
 
 #include <fstream>
 #include <string>
@@ -88,9 +89,11 @@ 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::SphereWidget sw(Point3f(0, 0, 0), 0.5);
-    viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), viz::Color::red());
+    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());
     viz::CylinderWidget cyw(Point3f(0, 0, 0), Point3f(-1, -1, -1), 0.5, 30, viz::Color::green());
     viz::CubeWidget cuw(Point3f(-2, -2, -2), Point3f(-1, -1, -1));
@@ -99,18 +102,20 @@ TEST(Viz_viz3d, accuracy)
     viz::CloudWidget pcw(cloud, colors);
     viz::CloudWidget pcw2(cloud, viz::Color::magenta());
     
-    viz.showWidget("line", lw);
+//     viz.showWidget("line", lw);
     viz.showWidget("plane", pw);
-    viz.showWidget("sphere", sw);
-    viz.showWidget("arrow", aw);
-    viz.showWidget("circle", cw);
-    viz.showWidget("cylinder", cyw);
-    viz.showWidget("cube", cuw);
+    viz.showWidget("plane2", pw2);
+    viz.showWidget("plane3", pw3);
+//     viz.showWidget("sphere", sw);
+//     viz.showWidget("arrow", aw);
+//     viz.showWidget("circle", cw);
+//     viz.showWidget("cylinder", cyw);
+//     viz.showWidget("cube", cuw);
     viz.showWidget("coordinateSystem", csw);
-    viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
-    viz.showWidget("text",tw);
-    viz.showWidget("pcw",pcw);
-    viz.showWidget("pcw2",pcw2);
+//     viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
+//     viz.showWidget("text",tw);
+//     viz.showWidget("pcw",pcw);
+//     viz.showWidget("pcw2",pcw2);
     
 //     viz::LineWidget lw2 = lw;
 //     v.showPointCloud("cld",cloud, colors);
@@ -125,13 +130,55 @@ TEST(Viz_viz3d, accuracy)
     
     
     viz::PolyLineWidget plw(points, viz::Color::green());
-    viz.showWidget("polyline", plw);
+//     viz.showWidget("polyline", plw);
 //     lw = v.getWidget("polyline").cast<viz::LineWidget>();
     
-    viz::Mesh3d::Ptr mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
+    viz::Mesh3d mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
     
-    viz::MeshWidget mw(*mesh);
-    viz.showWidget("mesh", mw);
+    viz::MeshWidget mw(mesh);
+//     viz.showWidget("mesh", mw);
+    
+    Mat img = imread("opencv.png");
+//     resize(img, img, Size(50,50));
+//     viz.showWidget("img", viz::ImageOverlayWidget(img, Point2i(50,50)));
+    
+    Matx33f K(657, 0, 320, 
+              0, 657, 240, 
+              0, 0, 1);
+    
+    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("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);
     
     viz.spin();
 
@@ -156,13 +203,16 @@ TEST(Viz_viz3d, accuracy)
         
         //plw.setColor(viz::Color(col_blue, col_green, col_red));
         
-        sw.setPose(cloudPosition);
+//         sw.setPose(cloudPosition);
 //         pw.setPose(cloudPosition);
         aw.setPose(cloudPosition);
         cw.setPose(cloudPosition);
         cyw.setPose(cloudPosition);
+        
+        frustum.setPose(cloudPosition);
 //         lw.setPose(cloudPosition);
-        cuw.setPose(cloudPosition);
+//         cpw.updatePose(Affine3f(0.1,0.0,0.0, cv::Vec3f(0.0,0.0,0.0)));
+//         cpw.setPose(cloudPosition);
 //         cnw.setPose(cloudPosition);
 //         v.showWidget("pcw",pcw, cloudPosition);
 //         v.showWidget("pcw2",pcw2, cloudPosition2);