diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp
index 9d0f51636..46d0be04c 100644
--- a/modules/viz/include/opencv2/viz/viz3d.hpp
+++ b/modules/viz/include/opencv2/viz/viz3d.hpp
@@ -25,41 +25,41 @@ namespace temp_viz
 
         void setBackgroundColor(const Color& color = Color::black());
 
-        void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate");
+        void addCoordinateSystem(double scale, const Affine3f& t, const String& id = "coordinate");
 
         void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity());
         void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity());
 
-        bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud");
+        bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud");
         
-        void showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
-        void showPlane(const String &id, const Vec4f &coefs, const Color &color = Color(255,255,255));
-        void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color = Color(255,255,255));
-        void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
-        void showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color = Color(255,255,255));
-        void showCircle(const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255));
-        void showSphere (const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255));
-        void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
+        void showLine(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
+        void showPlane(const String& id, const Vec4f& coefs, const Color& color = Color::white());
+        void showPlane(const String& id, const Vec4f& coefs, const Point3f& pt, const Color& color = Color::white());
+        void showCube(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
+        void showCylinder(const String& id, const Point3f& pt_on_axis, const Point3f&  axis_direction, double radius, int num_sides, const Color& color = Color::white());
+        void showCircle(const String& id, const Point3f& pt, double radius, const Color& color = Color::white());
+        void showSphere(const String& id, const Point3f& pt, double radius, const Color& color = Color::white());
+        void showArrow(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
         
-        Affine3f getShapePose(const String &id);
-        bool setShapePose(const String &id, const Affine3f &pose);
+        Affine3f getShapePose(const String& id);
+        void setShapePose(const String& id, const Affine3f &pose);
 
-        bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane");
-        bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane");
-        bool removeCoordinateSystem (const String &id = "coordinate");
+        bool addPlane (const ModelCoefficients &coefficients, const String& id = "plane");
+        bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id = "plane");
+        bool removeCoordinateSystem (const String& id = "coordinate");
 
-        bool addPolygonMesh (const Mesh3d& mesh, const String &id = "polygon");
-        bool updatePolygonMesh (const Mesh3d& mesh, const String &id = "polygon");
+        bool addPolygonMesh (const Mesh3d& mesh, const String& id = "polygon");
+        bool updatePolygonMesh (const Mesh3d& mesh, const String& id = "polygon");
 
-        bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String &id = "polyline");
+        bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline");
 
 
-        bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String &id = "");
+        bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = "");
 
 
-        bool addPolygon(const Mat& cloud, const Color& color, const String &id = "polygon");
+        bool addPolygon(const Mat& cloud, const Color& color, const String& id = "polygon");
 
-        bool addSphere (const Point3f &center, double radius, const Color& color, const String &id = "sphere");
+        bool addSphere (const Point3f &center, double radius, const Color& color, const String& id = "sphere");
 
 
         void spin ();
diff --git a/modules/viz/src/interactor_style.cpp b/modules/viz/src/interactor_style.cpp
index 3a8abb13b..62d2d2084 100644
--- a/modules/viz/src/interactor_style.cpp
+++ b/modules/viz/src/interactor_style.cpp
@@ -154,13 +154,13 @@ temp_viz::InteractorStyle::OnKeyDown ()
 {
     if (!init_)
     {
-        std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
+        std::cout << "Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
         return;
     }
 
     if (!renderer_)
     {
-        std::cout << "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl;
+        std::cout << "No renderer given! Use SetRendererCollection () before continuing." << std::endl;
         return;
     }
 
@@ -186,21 +186,9 @@ temp_viz::InteractorStyle::OnKeyDown ()
     bool keymod = false;
     switch (modifier_)
     {
-    case KB_MOD_ALT:
-    {
-        keymod = alt;
-        break;
-    }
-    case KB_MOD_CTRL:
-    {
-        keymod = ctrl;
-        break;
-    }
-    case KB_MOD_SHIFT:
-    {
-        keymod = shift;
-        break;
-    }
+    case KB_MOD_ALT:   keymod = alt;   break;
+    case KB_MOD_CTRL:  keymod = ctrl;  break;
+    case KB_MOD_SHIFT: keymod = shift; break;
     }
 
     std::string key (Interactor->GetKeySym ());
diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp
index de7104455..bb59ca949 100644
--- a/modules/viz/src/precomp.hpp
+++ b/modules/viz/src/precomp.hpp
@@ -6,12 +6,6 @@
 #include <list>
 #include <vector>
 
-/*
-#include <boost/function.hpp>
-#include <boost/bind.hpp>
-#include <boost/signals2.hpp>
-#include <boost/thread.hpp>*/
-
 #include <Eigen/Geometry>
 
 #if defined __GNUC__
@@ -140,24 +134,13 @@
 #include <vtkImageData.h>
 
 
-
 #include <vtkPolyDataNormals.h>
 #include <vtkMapper.h>
 #include <vtkSelectionNode.h>
-
-
-
 #include <vtkAbstractPicker.h>
 #include <vtkAbstractPropPicker.h>
-
 #include <vtkMatrix4x4.h>
 #include <vtkInteractorObserver.h>
-
-
-
-
-
-
 #include <vtkMapper2D.h>
 #include <vtkLeaderActor2D.h>
 #include <vtkAlgorithmOutput.h>
@@ -168,3 +151,6 @@
 #define __DEPRECATED
 #undef __DEPRECATED_DISABLED__
 #endif
+
+
+#include <q/shapes.h>
diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h
index be83cd59e..4055cd1b2 100644
--- a/modules/viz/src/q/shapes.h
+++ b/modules/viz/src/q/shapes.h
@@ -1,22 +1,19 @@
 #pragma once
 
-#include <Eigen/Core>
 #include <opencv2/viz/types.hpp>
-#include "precomp.hpp"
 
 namespace temp_viz
 {
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2);
-    /** \brief Allocate a new unstructured grid smartpointer. For internal use only.
-      * \param[out] polydata the resultant unstructured grid. 
-      */
-    CV_EXPORTS void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
+    vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
+    vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10);
+    vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30);
+    vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs);
+    vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
+    vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
+    vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
+    vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
+    vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2);
+
+    //brief Allocate a new unstructured grid smartpointer. For internal use only.
+    void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
 }
diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp
index 008489ccd..d80881c92 100644
--- a/modules/viz/src/q/viz3d_impl.hpp
+++ b/modules/viz/src/q/viz3d_impl.hpp
@@ -17,11 +17,11 @@ class CV_EXPORTS Viz3d::VizImpl
 public:
     typedef cv::Ptr<VizImpl> Ptr;
 
-    VizImpl (const std::string &name = std::string());
+    VizImpl (const String &name = String());
 
     virtual ~VizImpl ();
     void setFullScreen (bool mode);
-    void setWindowName (const std::string &name);
+    void setWindowName (const String &name);
     
     /** \brief  Register a callback function for keyboard input
       * \param[in] callback function that will be registered as a callback for a keyboard event
@@ -68,34 +68,34 @@ public:
            *         |
            *         y
            */
-    void addCoordinateSystem (double scale, const cv::Affine3f& t, const std::string &id = "coordinate");
+    void addCoordinateSystem (double scale, const Affine3f& t, const String& id = "coordinate");
 
     /** \brief Removes a previously added 3D axes (coordinate system)
           */
-    bool removeCoordinateSystem (const std::string &id = "coordinate");
-    bool removePointCloud (const std::string &id = "cloud");
-    inline bool removePolygonMesh (const std::string &id = "polygon")
+    bool removeCoordinateSystem (const String& id = "coordinate");
+    bool removePointCloud (const String& id = "cloud");
+    inline bool removePolygonMesh (const String& id = "polygon")
     {
         // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
         return removePointCloud (id);
     }
-    bool removeShape (const std::string &id = "cloud");
+    bool removeShape (const String& id = "cloud");
 
-    bool removeText3D (const std::string &id = "cloud");
+    bool removeText3D (const String& id = "cloud");
     bool removeAllPointClouds ();
     bool removeAllShapes ();
 
     void setBackgroundColor (const Color& color);
 
-    bool addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = "");
-    bool updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = "");
+    bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = "");
+    bool updateText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = "");
 
     /** \brief Set the pose of an existing shape. Returns false if the shape doesn't exist, true if the pose was succesfully updated. */
-    bool updateShapePose (const std::string &id, const cv::Affine3f& pose);
+    bool updateShapePose (const String& id, const Affine3f& pose);
 
-    bool addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = "");
+    bool addText3D (const String &text, const Point3f &position, const Color& color, double textScale = 1.0, const String& id = "");
 
-    bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud");
+    bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud");
 
     /** \brief If the id exists, updates the point cloud; otherwise, adds a new point cloud to the scene
       * \param[in] id a variable to identify the point cloud
@@ -106,23 +106,23 @@ public:
     void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity());
     void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity());
 
-    bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon");
-    bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon");
+    bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
+    bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
 
-    bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id = "polyline");
+    bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline");
 
-    void setPointCloudColor (const Color& color, const std::string &id = "cloud");
-    bool setPointCloudRenderingProperties (int property, double value, const std::string &id = "cloud");
-    bool getPointCloudRenderingProperties (int property, double &value, const std::string &id = "cloud");
+    void setPointCloudColor (const Color& color, const String& id = "cloud");
+    bool setPointCloudRenderingProperties (int property, double value, const String& id = "cloud");
+    bool getPointCloudRenderingProperties (int property, double &value, const String& id = "cloud");
 
-    bool setShapeRenderingProperties (int property, double value, const std::string &id);
-    void setShapeColor (const Color& color, const std::string &id);
+    bool setShapeRenderingProperties (int property, double value, const String& id);
+    void setShapeColor (const Color& color, const String& id);
 
     /** \brief Set whether the point cloud is selected or not
          *  \param[in] selected whether the cloud is selected or not (true = selected)
          *  \param[in] id the point cloud object id (default: cloud)
          */
-    bool setPointCloudSelected (const bool selected, const std::string &id = "cloud" );
+    bool setPointCloudSelected (const bool selected, const String& id = "cloud" );
 
     /** \brief Returns true when the user tried to close the window */
     bool wasStopped () const { if (interactor_ != NULL) return (stopped_); else return true; }
@@ -138,27 +138,27 @@ public:
         interactor_->TerminateApp ();
     }
     
-    void showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color);
-    void showPlane (const String &id, const cv::Vec4f &coefs, const Color &color);
-    void showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color);
-    void showCube (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color);
-    void showCylinder (const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color);
-    void showCircle (const String &id, const Point3f &pt, double radius, const Color &color);
-    void showSphere (const String &id, const Point3f &pt, double radius, const Color &color);
-    void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color);
+    void showLine (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
+    void showPlane (const String& id, const cv::Vec4f &coefs, const Color& color);
+    void showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, const Color& color);
+    void showCube (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
+    void showCylinder (const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color);
+    void showCircle (const String& id, const Point3f& pt, double radius, const Color& color);
+    void showSphere (const String& id, const Point3f& pt, double radius, const Color& color);
+    void showArrow (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
     
-    Affine3f getShapePose (const String &id);
-    bool setShapePose (const String &id, const Affine3f &pose);
+    Affine3f getShapePose (const String& id);
+    void setShapePose (const String& id, const Affine3f& pose);
 
-    bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon");
-    bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow");
-    bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color_line, const Color& color_text, const std::string &id = "arrow");
+    bool addPolygon(const cv::Mat& cloud, const Color& color, const String& id = "polygon");
+    bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color, bool display_length, const String& id = "arrow");
+    bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color_line, const Color& color_text, const String& id = "arrow");
 
     // Add a vtkPolydata as a mesh
-    bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id = "PolyData");
-    bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PolyData");
-    bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel");
-    bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PLYModel");
+    bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const String& id = "PolyData");
+    bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const String& id = "PolyData");
+    bool addModelFromPLYFile (const String &filename, const String& id = "PLYModel");
+    bool addModelFromPLYFile (const String &filename, vtkSmartPointer<vtkTransform> transform, const String& id = "PLYModel");
 
     /** \brief Changes the visual representation for all actors to surface representation. */
     void setRepresentationToSurfaceForAllActors ();
@@ -187,7 +187,7 @@ public:
     /** \brief Reset the camera direction from {0, 0, 0} to the center_{x, y, z} of a given dataset.
           * \param[in] id the point cloud object id (default: cloud)
           */
-    void resetCameraViewpoint (const std::string &id = "cloud");
+    void resetCameraViewpoint (const String& id = "cloud");
 
     /** \brief Set the camera pose given by position, viewpoint and up vector
           * \param[in] pos_x the x coordinate of the camera location
@@ -217,7 +217,7 @@ public:
           * \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters
           * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters
           */
-    void setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics);
+    void setCameraParameters (const cv::Matx33f& intrinsics, const Affine3f& extrinsics);
 
     /** \brief Set the camera parameters by given a full camera data structure.
           * \param[in] camera camera structure containing all the camera parameters.
@@ -237,8 +237,8 @@ public:
     void getCameras (Camera& camera);
 
     /** \brief Get the current viewing pose. */
-    cv::Affine3f getViewerPose ();
-    void saveScreenshot (const std::string &file);
+    Affine3f getViewerPose ();
+    void saveScreenshot (const String &file);
 
     /** \brief Return a pointer to the underlying VTK Render Window used. */
     //vtkSmartPointer<vtkRenderWindow> getRenderWindow () { return (window_); }
@@ -351,6 +351,9 @@ private:
 void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
 void convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m);
 
+vtkSmartPointer<vtkMatrix4x4> convertToVtkMatrix (const cv::Matx44f &m);
+cv::Matx44f convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matrix);
+
 /** \brief Convert origin and orientation to vtkMatrix4x4
       * \param[in] origin the point cloud origin
       * \param[in] orientation the point cloud orientation
diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp
index 362b615df..7fa9a2cd8 100644
--- a/modules/viz/src/shapes.cpp
+++ b/modules/viz/src/shapes.cpp
@@ -1,9 +1,7 @@
-#include <q/shapes.h>
+#include "precomp.hpp"
 
-inline float rad2deg (float alpha)
-{ return (alpha * 57.29578f); }
-
-inline double rad2deg (double alpha){return (alpha * 57.29578);}
+inline float rad2deg(float alpha) { return (alpha * 57.29578f); }
+inline double rad2deg(double alpha) { return (alpha * 57.29578); }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides)
 {
@@ -23,21 +21,20 @@ vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const cv::Vec4f& coefs)
 {
     vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
     plane->SetNormal (coefs[0], coefs[1], coefs[2]);
-    double norm = cv::norm (cv::Vec3f (coefs[0], coefs[1], coefs[2]));
+    double norm = cv::norm(cv::Vec3f(coefs.val));
     plane->Push (-coefs[3] / norm);
-    return (plane->GetOutput ());
+    return plane->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt)
 {
     vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
-    cv::Point3f coefs3 (coefs[0], coefs[1], coefs[2]);    
+    cv::Point3f coefs3(coefs[0], coefs[1], coefs[2]);
     double norm_sqr = 1.0 / coefs3.dot (coefs3);
-    plane->SetNormal (coefs[0], coefs[1], coefs[2]);
+    plane->SetNormal(coefs[0], coefs[1], coefs[2]);
 
-    double t = coefs3.dot (pt) + coefs[3];
-    cv::Vec3f p_center;
-    p_center = pt - coefs3 * t * norm_sqr;
+    double t = coefs3.dot(pt) + coefs[3];
+    cv::Vec3f p_center = pt - coefs3 * t * norm_sqr;
     plane->SetCenter (p_center[0], p_center[1], p_center[2]);
 
     return (plane->GetOutput ());
@@ -61,14 +58,14 @@ vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const cv::Point3f& pt, dou
     tf->SetTransform (t);
     tf->SetInputConnection (disk->GetOutputPort ());
     
-    return (tf->GetOutput ());
+    return tf->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max)
 {
     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);
-    return (cube->GetOutput ());
+    return cube->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
@@ -81,7 +78,7 @@ vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double ra
     sphere->LatLongTessellationOff ();
     sphere->Update ();
     
-    return (sphere->GetOutput ());
+    return sphere->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
@@ -136,22 +133,21 @@ vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Poi
     transformPD->SetTransform(transform);
     transformPD->SetInputConnection(arrowSource->GetOutputPort());
     
-    return (transformPD->GetOutput());
+    return transformPD->GetOutput();
 }
 
-////////////////////////////////////////////////////////////////////////////////////////////
 vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2)
 {
-  vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
-  line->SetPoint1 (pt1.x, pt1.y, pt1.z);
-  line->SetPoint2 (pt2.x, pt2.y, pt2.z);
-  line->Update ();
-  return line->GetOutput ();
+    vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
+    line->SetPoint1 (pt1.x, pt1.y, pt1.z);
+    line->SetPoint2 (pt2.x, pt2.y, pt2.z);
+    line->Update ();
+    return line->GetOutput ();
 }
-//////////////////////////////////////////////////////////////////////////////////////////////
+
 void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
 {
-  polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+    polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
 }
 
 
diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp
index 5d5f51f70..3df43fab6 100644
--- a/modules/viz/src/viz3d.cpp
+++ b/modules/viz/src/viz3d.cpp
@@ -9,16 +9,15 @@ temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_nam
 
 temp_viz::Viz3d::~Viz3d()
 {
-
+    delete impl_;
 }
 
-
 void temp_viz::Viz3d::setBackgroundColor(const Color& color)
 {
     impl_->setBackgroundColor(color);
 }
 
-void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String &id)
+void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String& id)
 {
     impl_->addCoordinateSystem(scale, t, id);
 }
@@ -38,22 +37,22 @@ bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals
     return impl_->addPointCloudNormals(cloud, normals, level, scale, id);
 }
 
-bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->addPolygonMesh(mesh, Mat(), id);
 }
 
-bool temp_viz::Viz3d::updatePolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::updatePolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->updatePolygonMesh(mesh, Mat(), id);
 }
 
-bool temp_viz::Viz3d::addPolylineFromPolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->addPolylineFromPolygonMesh(mesh, id);
 }
 
-bool temp_viz::Viz3d::addText (const String &text, int xpos, int ypos, const Color& color, int fontsize, const String &id)
+bool temp_viz::Viz3d::addText (const String &text, int xpos, int ypos, const Color& color, int fontsize, const String& id)
 {
     return impl_->addText(text, xpos, ypos, color, fontsize, id);
 }
@@ -73,57 +72,57 @@ void temp_viz::Viz3d::spinOnce (int time, bool force_redraw)
     impl_->spinOnce(time, force_redraw);
 }
 
-void temp_viz::Viz3d::showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::showLine(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     impl_->showLine(id, pt1, pt2, color);
 }
 
-void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Color &color)
+void temp_viz::Viz3d::showPlane(const String& id, const Vec4f &coefs, const Color& color)
 {
     impl_->showPlane(id, coefs, color);
 }
 
-void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color)
+void temp_viz::Viz3d::showPlane(const String& id, const Vec4f &coefs, const Point3f& pt, const Color& color)
 {
     impl_->showPlane(id, coefs, pt, color);
 }
 
-void temp_viz::Viz3d::showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::showCube(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     impl_->showCube(id, pt1, pt2, color);
 }
 
-void temp_viz::Viz3d::showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color)
+void temp_viz::Viz3d::showCylinder(const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color)
 {
     impl_->showCylinder(id, pt_on_axis, axis_direction, radius, num_sides, color);
 }
 
-void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double radius, const Color &color)
+void temp_viz::Viz3d::showCircle(const String& id, const Point3f& pt, double radius, const Color& color)
 {
     impl_->showCircle(id, pt, radius, color);
 }
 
-void temp_viz::Viz3d::showSphere (const String &id, const Point3f &pt, double radius, const Color &color)
+void temp_viz::Viz3d::showSphere (const String& id, const Point3f& pt, double radius, const Color& color)
 {
     impl_->showSphere(id, pt, radius, color);
 }
 
-void temp_viz::Viz3d::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::showArrow (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     impl_->showArrow(id,pt1,pt2,color);
 }
 
-cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id)
+cv::Affine3f temp_viz::Viz3d::getShapePose(const String& id)
 {
     return impl_->getShapePose(id);
 }
 
-bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose)
+void temp_viz::Viz3d::setShapePose(const String& id, const Affine3f &pose)
 {
-    return impl_->setShapePose(id, pose);
+    impl_->setShapePose(id, pose);
 }
 
-bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)
+bool temp_viz::Viz3d::removeCoordinateSystem (const String& id)
 {
     return impl_->removeCoordinateSystem(id);
 }
diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp
index 598304ae8..7c97b20d7 100644
--- a/modules/viz/src/viz3d_impl.cpp
+++ b/modules/viz/src/viz3d_impl.cpp
@@ -53,7 +53,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 
     // If the cloud already exists, update otherwise create new one
     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-    bool exist = (am_it == cloud_actor_map_->end());
+    bool exist = am_it == cloud_actor_map_->end();
     if (exist)
     {
         // Add as new cloud
@@ -181,7 +181,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 
     // If the cloud already exists, update otherwise create new one
     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-    bool exist = (am_it == cloud_actor_map_->end());
+    bool exist = am_it == cloud_actor_map_->end();
     if (exist)
     {
         // Add as new cloud
@@ -292,7 +292,7 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
     CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3);
 
     if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
-        return (false);
+        return false;
 
     vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
     vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
@@ -380,11 +380,11 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
 }
 
 
-void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::VizImpl::showLine (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     
     // If it exists just update
     if (exists)
@@ -416,11 +416,11 @@ void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coefs, const Color &color)
+void temp_viz::Viz3d::VizImpl::showPlane (const String& id, const cv::Vec4f &coefs, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -451,11 +451,11 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color)
+void temp_viz::Viz3d::VizImpl::showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -486,11 +486,11 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coe
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::VizImpl::showCube (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -521,11 +521,11 @@ void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, c
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color)
+void temp_viz::Viz3d::VizImpl::showCylinder (const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -556,11 +556,11 @@ void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, double radius, const Color &color)
+void temp_viz::Viz3d::VizImpl::showCircle (const String& id, const Point3f& pt, double radius, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -591,11 +591,11 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt,
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt, double radius, const Color &color)
+void temp_viz::Viz3d::VizImpl::showSphere (const String& id, const Point3f& pt, double radius, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -626,11 +626,11 @@ void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt,
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+void temp_viz::Viz3d::VizImpl::showArrow (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
 {
     // Check if this Id already exists
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     
     // If it exists just update
@@ -662,49 +662,24 @@ void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1,
     }
 }
 
-cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String &id)
+cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String& id)
 {
-    // Get the shape with the id and return the pose
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
-    
-    if (!exists)
-    {
-        std::cout << "[getShapePose] A shape with id <" << id << "> does not exist!" << std::endl;
-        return Affine3f();
-    }
 
-    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
-
-    vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
-    
-    Matx44f pose_mat;
-    convertToCvMatrix(matrix, pose_mat);
-    
-    Affine3f pose;
-    pose.matrix = pose_mat;
-    return pose;
+    CV_Assert(am_it != shape_actor_map_->end());
+    vtkLODActor* actor = vtkLODActor::SafeDownCast(am_it->second);
+    return Affine3f(convertToMatx(actor->GetUserMatrix()));
 }
 
-bool temp_viz::Viz3d::VizImpl::setShapePose (const String &id, const Affine3f &pose)
+void temp_viz::Viz3d::VizImpl::setShapePose (const String& id, const Affine3f &pose)
 {
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
-    
-    if (!exists)
-    {
-        return false;
-    }
+    CV_Assert(am_it != shape_actor_map_->end());
    
     vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
-    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
-
-    convertToVtkMatrix (pose.matrix, matrix);
-
+    vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
     actor->SetUserMatrix (matrix);
     actor->Modified ();
-
-    return (true);
 }
 
 bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp
index d35a88153..4552790ab 100644
--- a/modules/viz/src/viz_main.cpp
+++ b/modules/viz/src/viz_main.cpp
@@ -97,14 +97,12 @@ void temp_viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_
 /////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::Viz3d::VizImpl::registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie)
 {
-    // Register the callback function in the interactor style
     style_->registerMouseCallback(callback, cookie);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::Viz3d::VizImpl::registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie)
 {
-    // Register the callback function in the interactor style
     style_->registerKeyboardCallback(callback, cookie);
 }
 
@@ -1205,6 +1203,15 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatr
             vtk_matrix->SetElement (i, k, m (i, k));
 }
 
+vtkSmartPointer<vtkMatrix4x4> temp_viz::convertToVtkMatrix (const cv::Matx44f &m)
+{
+    vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
+    for (int i = 0; i < 4; i++)
+        for (int k = 0; k < 4; k++)
+            vtk_matrix->SetElement(i, k, m(i, k));
+    return vtk_matrix;
+}
+
 void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m)
 {
     for (int i = 0; i < 4; i++)
@@ -1212,6 +1219,16 @@ void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matri
             m(i,k) = vtk_matrix->GetElement (i, k);
 }
 
+
+cv::Matx44f temp_viz::convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matrix)
+{
+    cv::Matx44f m;
+    for (int i = 0; i < 4; i++)
+        for (int k = 0; k < 4; k++)
+            m(i, k) = vtk_matrix->GetElement (i, k);
+    return m;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
 {