First version of 'viz' module

cmake/OpenCVModule.cmake

@@ -432,6 +432,8 @@ macro(ocv_glob_module_sources)
   file(GLOB lib_hdrs     "include/opencv2/*.hpp" "include/opencv2/${name}/*.hpp" "include/opencv2/${name}/*.h")
   file(GLOB lib_hdrs_detail "include/opencv2/${name}/detail/*.hpp" "include/opencv2/${name}/detail/*.h")
 
+  file(GLOB_RECURSE qq "src/q/*.h*")
+
   file(GLOB lib_device_srcs "src/cuda/*.cu")
   set(device_objs "")
   set(lib_device_hdrs "")
@@ -445,7 +447,7 @@ macro(ocv_glob_module_sources)
   endif()
 
   ocv_set_module_sources(${ARGN} HEADERS ${lib_hdrs} ${lib_hdrs_detail}
-                                 SOURCES ${lib_srcs} ${lib_int_hdrs} ${device_objs} ${lib_device_srcs} ${lib_device_hdrs})
+                                 SOURCES ${lib_srcs} ${lib_int_hdrs} ${device_objs} ${lib_device_srcs} ${lib_device_hdrs} ${qq})
 
   source_group("Src" FILES ${lib_srcs} ${lib_int_hdrs})
   source_group("Include" FILES ${lib_hdrs})

modules/core/include/opencv2/core/affine.hpp

@@ -109,6 +109,8 @@ namespace cv
 
         template <typename Y> operator Affine3<Y>() const;
 
+        operator cv::Mat();
+
         Mat4 matrix;
 
 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
@@ -314,6 +316,8 @@ template<typename T> template <typename Y> inline cv::Affine3<T>::operator Affin
     return Affine3<Y>(matrix);
 }
 
+template<typename T> inline cv::Affine3<T>::operator cv::Mat() { return cv::Mat(matrix, false); }
+
 template<typename T> inline cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
 {
     return affine2.concatenate(affine1);
@@ -350,7 +354,7 @@ inline cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
     return r;
 }
 
-#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
+#if (defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H) || defined CV_AFFINE_FORCE_EIGEN_PLUGIN
 
 template<typename T> inline cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
 {

modules/viz/CMakeLists.txt

+###############################################################################
+# Find qvtk
+# This sets the following variables:
+# QVTK_FOUND - True if QVTK was found.
+# QVTK_INCLUDE_DIR - Directory containing the QVTK include files.
+# QVTK_LIBRARY - QVTK library.
+# if QVTK_FOUND then QVTK_INCLUDE_DIR is appended to VTK_INCLUDE_DIRS and QVTK_LIBRARY is appended to QVTK_LIBRARY_DIR
+macro(find_qvtk)
+  find_library (QVTK_LIBRARY QVTK HINTS ${VTK_DIR} ${VTK_DIR}/bin)
+  find_path (QVTK_INCLUDE_DIR QVTKWidget.h HINT ${VTK_INCLUDE_DIRS})
+  find_package_handle_standard_args(QVTK DEFAULT_MSG QVTK_LIBRARY QVTK_INCLUDE_DIR)
+
+  if(NOT QVTK_FOUND)
+    set (VTK_USE_QVTK OFF)
+  else(NOT QVTK_FOUND)
+    get_filename_component (QVTK_LIBRARY_DIR ${QVTK_LIBRARY} PATH)
+    set (VTK_LIBRARY_DIRS ${VTK_LIBRARY_DIRS} ${QVTK_LIBRARY_DIR})
+    set (VTK_INCLUDE_DIRS ${VTK_INCLUDE_DIRS} ${QVTK_INCLUDE_DIR})
+    set (VTK_USE_QVTK ON)
+  endif()
+endmacro()
+
+macro(find_vtk)
+  find_package(VTK 5.8.0 REQUIRED)
+  if(VTK_FOUND)
+    if (BUILD_SHARED_LIBS OR (NOT BUILD_SHARED_LIBS AND NOT VTK_BUILD_SHARED_LIBS))
+      find_qvtk()
+      message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, lib: ${VTK_LIBRARY_DIRS})")
+      link_directories(${VTK_LIBRARY_DIRS})
+      include_directories(SYSTEM ${VTK_INCLUDE_DIRS})
+      set(HAVE_VTK ON)    
+    else ()
+      set(HAVE_VTK OFF)
+      message ("Warning: VTK disabled. You are to build OpenCV in STATIC but VTK is SHARED!")
+    endif ()
+  endif()
+endmacro()
+
+macro(find_boost)
+  # Disable the config mode of find_package(Boost)
+  set(Boost_NO_BOOST_CMAKE ON)
+  set(Boost_USE_STATIC_LIBS ON)
+
+  find_package(Boost 1.49.0 REQUIRED COMPONENTS system thread)
+
+  if(Boost_FOUND)
+    set(HAVE_BOOST ON)
+
+    # Obtain diagnostic information about Boost's automatic linking outputted during compilation time.
+    add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
+    include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
+    link_directories(${Boost_LIBRARY_DIRS})
+    message(STATUS "Boost found (include: ${Boost_INCLUDE_DIRS})")
+  endif()
+endmacro()
+
+find_vtk()
+find_boost()
+
+
+find_package(OpenGL)
+if (OPENGL_FOUND)
+  if(OPENGL_INCLUDE_DIR)
+    include_directories("${OPENGL_INCLUDE_DIR}")
+  endif()
+  if(OPENGL_DEFINITIONS)
+    add_definitions("${OPENGL_DEFINITIONS}")
+  endif()
+endif()
+
+
+if(NOT HAVE_VTK)
+  set(DEFAULT FALSE)
+  set(REASON "VTK was not found.")
+else()
+  set(DEFAULT TRUE)
+  set(REASON)
+  set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE")
+  include (${VTK_USE_FILE})
+  include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
+endif()
+
+
+add_definitions(-DHAVE_VTK)
+
+
+set(BUILD_opencv_viz_INIT OFF)
+
+include_directories(src)
+set(the_description "Viz")
+ocv_define_module(viz opencv_core opencv_calib3d)
+#${PCL_LIBRARIES}
+
+target_link_libraries(opencv_viz vtkCommon vtkWidgets vtkHybrid vtkCharts vtkFiltering vtkRendering ${OPENGL_LIBRARIES})
+
+if(APPLE)
+    target_link_libraries(opencv_viz "-framework Cocoa")
+endif()
+

modules/viz/include/opencv2/viz.hpp

+#pragma once
+
+#include <opencv2/viz/viz3d.hpp>

modules/viz/include/opencv2/viz/events.hpp

+#pragma once
+
+#include <string>
+#include <opencv2/viz/types.hpp>
+
+namespace cv
+{
+    class KeyboardEvent
+    {
+    public:
+        static const unsigned int Alt   = 1;
+        static const unsigned int Ctrl  = 2;
+        static const unsigned int Shift = 4;
+
+        /** \brief Constructor
+              * \param[in] action    true for key was pressed, false for released
+              * \param[in] key_sym   the key-name that caused the action
+              * \param[in] key       the key code that caused the action
+              * \param[in] alt       whether the alt key was pressed at the time where this event was triggered
+              * \param[in] ctrl      whether the ctrl was pressed at the time where this event was triggered
+              * \param[in] shift     whether the shift was pressed at the time where this event was triggered
+              */
+        KeyboardEvent (bool action, const std::string& key_sym, unsigned char key, bool alt, bool ctrl, bool shift);
+
+        bool isAltPressed () const;
+        bool isCtrlPressed () const;
+        bool isShiftPressed () const;
+
+        unsigned char getKeyCode () const;
+
+        const std::string& getKeySym () const;
+        bool keyDown () const;
+        bool keyUp () const;
+
+    protected:
+
+        bool action_;
+        unsigned int modifiers_;
+        unsigned char key_code_;
+        std::string key_sym_;
+    };
+
+    class MouseEvent
+    {
+    public:
+        enum Type
+        {
+            MouseMove = 1,
+            MouseButtonPress,
+            MouseButtonRelease,
+            MouseScrollDown,
+            MouseScrollUp,
+            MouseDblClick
+        } ;
+
+        enum MouseButton
+        {
+            NoButton      = 0,
+            LeftButton,
+            MiddleButton,
+            RightButton,
+            VScroll /*other buttons, scroll wheels etc. may follow*/
+        } ;
+
+        MouseEvent (const Type& type, const MouseButton& button, const Point& p, bool alt, bool ctrl, bool shift);
+
+
+        Type type;
+        MouseButton button;
+        Point pointer;
+        unsigned int key_state;
+    };
+}
+
+////////////////////////////////////////////////////////////////////
+/// Implementation
+
+inline cv::KeyboardEvent::KeyboardEvent (bool _action, const std::string& _key_sym, unsigned char key, bool alt, bool ctrl, bool shift)
+  : action_ (_action), modifiers_ (0), key_code_(key), key_sym_ (_key_sym)
+{
+  if (alt)
+    modifiers_ = Alt;
+
+  if (ctrl)
+    modifiers_ |= Ctrl;
+
+  if (shift)
+    modifiers_ |= Shift;
+}
+
+inline bool cv::KeyboardEvent::isAltPressed () const { return (modifiers_ & Alt) != 0; }
+inline bool cv::KeyboardEvent::isCtrlPressed () const { return (modifiers_ & Ctrl) != 0; }
+inline bool cv::KeyboardEvent::isShiftPressed () const { return (modifiers_ & Shift) != 0; }
+inline unsigned char cv::KeyboardEvent::getKeyCode () const { return key_code_; }
+inline const std::string& cv::KeyboardEvent::getKeySym () const { return (key_sym_); }
+inline bool cv::KeyboardEvent::keyDown () const { return action_; }
+inline bool cv::KeyboardEvent::keyUp () const { return !action_; }
+
+inline cv::MouseEvent::MouseEvent (const Type& _type, const MouseButton& _button, const Point& _p,  bool alt, bool ctrl, bool shift)
+    : type(_type), button(_button), pointer(_p), key_state(0)
+{
+    if (alt)
+        key_state = KeyboardEvent::Alt;
+
+    if (ctrl)
+        key_state |= KeyboardEvent::Ctrl;
+
+    if (shift)
+        key_state |= KeyboardEvent::Shift;
+}

modules/viz/include/opencv2/viz/mesh_load.hpp

+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/viz/types.hpp>
+#include <vector>
+
+namespace temp_viz
+{
+    CV_EXPORTS Mesh3d::Ptr mesh_load(const String& file);
+}

modules/viz/include/opencv2/viz/types.hpp

+#pragma once
+
+#include <vector>
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+#include <opencv2/core/affine.hpp>
+
+namespace temp_viz
+{
+    //qt creator hack
+    typedef cv::Scalar Scalar;
+    typedef cv::Mat Mat;
+    typedef std::string String;
+
+    typedef cv::Vec3d Vec3d;
+    typedef cv::Vec4d Vec4d;
+    typedef cv::Vec2d Vec2d;
+    typedef cv::Vec2i Vec2i;
+    typedef cv::Matx33d Matx33d;
+    typedef cv::Affine3f Affine3f;
+    typedef cv::Affine3d Affine3d;
+    typedef cv::Point3f Point3f;
+    typedef cv::Matx44d Matx44d;
+    typedef cv::Matx44f Matx44f;
+    typedef cv::Size Size;
+    typedef cv::Point Point;
+
+
+
+    struct CV_EXPORTS ModelCoefficients
+    {
+        std::vector<float> values;
+    };
+
+
+    class CV_EXPORTS Color : public Scalar
+    {
+    public:
+        Color();
+        Color(double gray);
+        Color(double blue, double green, double red);
+
+        Color(const Scalar& color);
+
+        static Color black();
+        static Color blue();
+        static Color green();
+        static Color cyan();
+
+        static Color red();
+        static Color magenta();
+        static Color yellow();
+        static Color white();
+
+        static Color gray();
+    };
+
+
+    struct CV_EXPORTS Vertices
+    {
+        std::vector<unsigned int> vertices;
+    };
+
+    class CV_EXPORTS Mesh3d
+    {
+    public:
+        typedef cv::Ptr<Mesh3d> Ptr;
+
+        Mat cloud, colors;
+        std::vector<Vertices> polygons;
+    };
+
+
+    inline Color vtkcolor(const Color& color)
+    {
+        Color scaled_color = color * (1.0/255.0);
+        std::swap(scaled_color[0], scaled_color[2]);
+        return scaled_color;
+    }
+
+    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); }
+}

modules/viz/include/opencv2/viz/viz3d.hpp

+#pragma once
+
+#if !defined YES_I_AGREE_THAT_VIZ_API_IS_NOT_STABLE_NOW_AND_BINARY_COMPARTIBILITY_WONT_BE_SUPPORTED
+    //#error "Viz is in beta state now. Please define macro above to use it"
+#endif
+
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+
+
+#include <string>
+#include <opencv2/viz/types.hpp>
+
+namespace temp_viz
+{
+    class CV_EXPORTS Viz3d
+    {
+    public:
+
+        typedef cv::Ptr<Viz3d> Ptr;
+
+        Viz3d(const String& window_name = String());
+        ~Viz3d();
+
+        void setBackgroundColor(const Color& color = Color::black());
+
+        void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate");
+
+        void addPointCloud(const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat());
+
+        bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud");
+
+
+
+
+        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 updatePointCloud (const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat());
+
+
+        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 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 addSphere (const Point3f &center, double radius, const Color& color, const String &id = "sphere");
+
+
+        void spin ();
+        void spinOnce (int time = 1, bool force_redraw = false);
+
+    private:
+        Viz3d(const Viz3d&);
+        Viz3d& operator=(const Viz3d&);
+
+        struct VizImpl;
+        VizImpl* impl_;
+    };
+}
+
+
+

modules/viz/src/common.cpp

+#include <q/common.h>
+#include <cstdlib>
+#include <opencv2/viz/types.hpp>
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+//Eigen::Matrix4d temp_viz::vtkToEigen (vtkMatrix4x4* vtk_matrix)
+//{
+//    Eigen::Matrix4d eigen_matrix = Eigen::Matrix4d::Identity ();
+//    for (int i=0; i < 4; i++)
+//        for (int j=0; j < 4; j++)
+//            eigen_matrix (i, j) = vtk_matrix->GetElement (i, j);
+
+//    return eigen_matrix;
+//}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+//Eigen::Vector2i temp_viz::worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
+//{
+//    // Transform world to clipping coordinates
+//    Eigen::Vector4d world (view_projection_matrix * world_pt);
+//    // Normalize w-component
+//    world /= world.w ();
+
+//    // X/Y screen space coordinate
+//    int screen_x = int (floor (double (((world.x () + 1) / 2.0) * width) + 0.5));
+//    int screen_y = int (floor (double (((world.y () + 1) / 2.0) * height) + 0.5));
+
+//    // Calculate -world_pt.y () because the screen Y axis is oriented top->down, ie 0 is top-left
+//    //int winY = (int) floor ( (double) (((1 - world_pt.y ()) / 2.0) * height) + 0.5); // top left
+
+//    return (Eigen::Vector2i (screen_x, screen_y));
+//}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+//void temp_viz::getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24])
+//{
+//    // Set up the normals
+//    Eigen::Vector4d normals[6];
+//    for (int i=0; i < 6; i++)
+//    {
+//        normals[i] = Eigen::Vector4d (0.0, 0.0, 0.0, 1.0);
+
+//        // if i is even set to -1, if odd set to +1
+//        normals[i] (i/2) = 1 - (i%2)*2;
+//    }
+
+//    // Transpose the matrix for use with normals
+//    Eigen::Matrix4d view_matrix = view_projection_matrix.transpose ();
+
+//    // Transform the normals to world coordinates
+//    for (int i=0; i < 6; i++)
+//    {
+//        normals[i] = view_matrix * normals[i];
+
+//        double f = 1.0/sqrt (normals[i].x () * normals[i].x () +
+//                             normals[i].y () * normals[i].y () +
+//                             normals[i].z () * normals[i].z ());
+
+//        planes[4*i + 0] = normals[i].x ()*f;
+//        planes[4*i + 1] = normals[i].y ()*f;
+//        planes[4*i + 2] = normals[i].z ()*f;
+//        planes[4*i + 3] = normals[i].w ()*f;
+//    }
+//}
+
+//int temp_viz::cullFrustum (double frustum[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb)
+//{
+//    int result = PCL_INSIDE_FRUSTUM;
+
+//    for(int i =0; i < 6; i++){
+//        double a = frustum[(i*4)];
+//        double b = frustum[(i*4)+1];
+//        double c = frustum[(i*4)+2];
+//        double d = frustum[(i*4)+3];
+
+//        //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;
+
+//        //  Basic VFC algorithm
+//        Eigen::Vector3d center ((max_bb.x () - min_bb.x ()) / 2 + min_bb.x (),
+//                                (max_bb.y () - min_bb.y ()) / 2 + min_bb.y (),
+//                                (max_bb.z () - min_bb.z ()) / 2 + min_bb.z ());
+
+//        Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
+//                                fabs (static_cast<double> (max_bb.y () - center.y ())),
+//                                fabs (static_cast<double> (max_bb.z () - center.z ())));
+
+//        double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
+//        double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
+
+//        if (m + n < 0){
+//            result = PCL_OUTSIDE_FRUSTUM;
+//            break;
+//        }
+
+//        if (m - n < 0)
+//        {
+//            result = PCL_INTERSECT_FRUSTUM;
+//        }
+//    }
+
+//    return result;
+//}
+
+//void
+//temp_viz::getModelViewPosition (Eigen::Matrix4d model_view_matrix, Eigen::Vector3d &position)
+//{
+//  //Compute eye or position from model view matrix
+//  Eigen::Matrix4d inverse_model_view_matrix = model_view_matrix.inverse();
+//  for (int i=0; i < 3; i++)
+//  {
+//    position(i) = inverse_model_view_matrix(i, 3);
+//  }
+//}
+
+// Lookup table of max 6 bounding box vertices, followed by number of vertices, ie {v0, v1, v2, v3, v4, v5, nv}
+//
+//      3--------2
+//     /|       /|       Y      0 = xmin, ymin, zmin
+//    / |      / |       |      6 = xmax, ymax. zmax
+//   7--------6  |       |
+//   |  |     |  |       |
+//   |  0-----|--1       +------X
+//   | /      | /       /
+//   |/       |/       /
+//   4--------5       Z
+
+int hull_vertex_table[43][7] = {
+    { 0, 0, 0, 0, 0, 0, 0 }, // inside
+    { 0, 4, 7, 3, 0, 0, 4 }, // left
+    { 1, 2, 6, 5, 0, 0, 4 }, // right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 1, 5, 4, 0, 0, 4 }, // bottom
+    { 0, 1, 5, 4, 7, 3, 6 }, // bottom, left
+    { 0, 1, 2, 6, 5, 4, 6 }, // bottom, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 2, 3, 7, 6, 0, 0, 4 }, // top
+    { 4, 7, 6, 2, 3, 0, 6 }, // top, left
+    { 2, 3, 7, 6, 5, 1, 6 }, // top, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 3, 2, 1, 0, 0, 4 }, // front
+    { 0, 4, 7, 3, 2, 1, 6 }, // front, left
+    { 0, 3, 2, 6, 5, 1, 6 }, // front, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 3, 2, 1, 5, 4, 6 }, // front, bottom
+    { 2, 1, 5, 4, 7, 3, 6 }, // front, bottom, left
+    { 0, 3, 2, 6, 5, 4, 6 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 3, 7, 6, 2, 1, 6 }, // front, top
+    { 0, 4, 7, 6, 2, 1, 6 }, // front, top, left
+    { 0, 3, 7, 6, 5, 1, 6 }, // front, top, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 4, 5, 6, 7, 0, 0, 4 }, // back
+    { 4, 5, 6, 7, 3, 0, 6 }, // back, left
+    { 1, 2, 6, 7, 4, 5, 6 }, // back, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 0, 1, 5, 6, 7, 4, 6 }, // back, bottom
+    { 0, 1, 5, 6, 7, 3, 6 }, // back, bottom, left
+    { 0, 1, 2, 6, 7, 4, 6 }, // back, bottom, right
+    { 0, 0, 0, 0, 0, 0, 0 },
+    { 2, 3, 7, 4, 5, 6, 6 }, // back, top
+    { 0, 4, 5, 6, 2, 3, 6 }, // back, top, left
+    { 1, 2, 3, 7, 4, 5, 6 }  // back, top, right
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+//float
+//temp_viz::viewScreenArea (
+//        const Eigen::Vector3d &eye,
+//        const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb,
+//        const Eigen::Matrix4d &view_projection_matrix, int width, int height)
+//{
+//    Eigen::Vector4d bounding_box[8];
+//    bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
+//    bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
+//    bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
+//    bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
+//    bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
+//    bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
+//    bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
+//    bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
+
+//    // Compute 6-bit code to classify eye with respect to the 6 defining planes
+//    int pos = ((eye.x () < bounding_box[0].x ()) )  // 1 = left
+//            + ((eye.x () > bounding_box[6].x ()) << 1)  // 2 = right
+//            + ((eye.y () < bounding_box[0].y ()) << 2)  // 4 = bottom
+//            + ((eye.y () > bounding_box[6].y ()) << 3)  // 8 = top
+//            + ((eye.z () < bounding_box[0].z ()) << 4)  // 16 = front
+//            + ((eye.z () > bounding_box[6].z ()) << 5); // 32 = back
+
+//    // Look up number of vertices
+//    int num = hull_vertex_table[pos][6];
+//    if (num == 0)
+//    {
+//        return (float (width * height));
+//    }
+//    //return 0.0;
+
+
+//    //  cout << "eye: " << eye.x() << " " << eye.y() << " " << eye.z() << endl;
+//    //  cout << "min: " << bounding_box[0].x() << " " << bounding_box[0].y() << " " << bounding_box[0].z() << endl;
+//    //
+//    //  cout << "pos: " << pos << " ";
+//    //  switch(pos){
+//    //    case 0:  cout << "inside" << endl; break;
+//    //    case 1:  cout << "left" << endl; break;
+//    //    case 2:  cout << "right" << endl; break;
+//    //    case 3:
+//    //    case 4:  cout << "bottom" << endl; break;
+//    //    case 5:  cout << "bottom, left" << endl; break;
+//    //    case 6:  cout << "bottom, right" << endl; break;
+//    //    case 7:
+//    //    case 8:  cout << "top" << endl; break;
+//    //    case 9:  cout << "top, left" << endl; break;
+//    //    case 10:  cout << "top, right" << endl; break;
+//    //    case 11:
+//    //    case 12:
+//    //    case 13:
+//    //    case 14:
+//    //    case 15:
+//    //    case 16:  cout << "front" << endl; break;
+//    //    case 17:  cout << "front, left" << endl; break;
+//    //    case 18:  cout << "front, right" << endl; break;
+//    //    case 19:
+//    //    case 20:  cout << "front, bottom" << endl; break;
+//    //    case 21:  cout << "front, bottom, left" << endl; break;
+//    //    case 22:
+//    //    case 23:
+//    //    case 24:  cout << "front, top" << endl; break;
+//    //    case 25:  cout << "front, top, left" << endl; break;
+//    //    case 26:  cout << "front, top, right" << endl; break;
+//    //    case 27:
+//    //    case 28:
+//    //    case 29:
+//    //    case 30:
+//    //    case 31:
+//    //    case 32:  cout << "back" << endl; break;
+//    //    case 33:  cout << "back, left" << endl; break;
+//    //    case 34:  cout << "back, right" << endl; break;
+//    //    case 35:
+//    //    case 36:  cout << "back, bottom" << endl; break;
+//    //    case 37:  cout << "back, bottom, left" << endl; break;
+//    //    case 38:  cout << "back, bottom, right" << endl; break;
+//    //    case 39:
+//    //    case 40:  cout << "back, top" << endl; break;
+//    //    case 41:  cout << "back, top, left" << endl; break;
+//    //    case 42:  cout << "back, top, right" << endl; break;
+//    //  }
+
+//    //return -1 if inside
+//    Eigen::Vector2d dst[8];
+//    for (int i = 0; i < num; i++)
+//    {
+//        Eigen::Vector4d world_pt = bounding_box[hull_vertex_table[pos][i]];
+//        Eigen::Vector2i screen_pt = temp_viz::worldToView(world_pt, view_projection_matrix, width, height);
+//        //    cout << "point[" << i << "]: " << screen_pt.x() << " " << screen_pt.y() << endl;
+//        dst[i] = Eigen::Vector2d(screen_pt.x (), screen_pt.y ());
+//    }
+
+//    double sum = 0.0;
+//    for (int i = 0; i < num; ++i)
+//    {
+//        sum += (dst[i].x () - dst[(i+1) % num].x ()) * (dst[i].y () + dst[(i+1) % num].y ());
+//    }
+
+//    return (fabsf (float (sum * 0.5f)));
+//}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Camera::computeViewMatrix (Affine3d& view_mat) const
+{
+    //constructs view matrix from camera pos, view up, and the point it is looking at
+    //this code is based off of gluLookAt http://www.opengl.org/wiki/GluLookAt_code
+
+    Vec3d zAxis = normalized(focal - pos);
+    Vec3d xAxis = normalized(zAxis.cross(view_up));
+    Vec3d yAxis = xAxis.cross (zAxis);
+
+    Matx33d R;
+
+    R(0, 0) =  xAxis[0];  R(0, 1) =  xAxis[1];  R(0, 2) =  xAxis[2];
+    R(1, 0) =  yAxis[0];  R(1, 1) =  yAxis[1];  R(1, 2) =  yAxis[2];
+    R(1, 0) = -zAxis[0];  R(2, 1) = -zAxis[1];  R(2, 2) = -zAxis[2];
+
+    Vec3d t = R * (-pos);
+
+    view_mat = Affine3d(R, t);
+}
+
+///////////////////////////////////////////////////////////////////////
+void temp_viz::Camera::computeProjectionMatrix (Matx44d& proj) const
+{
+    double top    = clip[0] * tan (0.5 * fovy);
+    double left   = -(top * window_size[0]) / window_size[1];
+    double right  = -left;
+    double bottom = -top;
+
+    double temp1 = 2.0 * clip[0];
+    double temp2 = 1.0 / (right - left);
+    double temp3 = 1.0 / (top - bottom);
+    double temp4 = 1.0 / clip[1] - clip[0];
+
+    proj = Matx44d::zeros();
+
+    proj(0,0) = temp1 * temp2;
+    proj(1,1) = temp1 * temp3;
+    proj(0,2) = (right + left) * temp2;
+    proj(1,2) = (top + bottom) * temp3;
+    proj(2,2) = (-clip[1] - clip[0]) * temp4;
+    proj(3,2) = -1.0;
+    proj(2,3) = (-temp1 * clip[1]) * temp4;
+}

modules/viz/src/interactor_style.cpp

+#include <list>
+#include <q/interactor_style.h>
+#include <vtkPolyData.h>
+#include <vtkMapper.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkPointData.h>
+#include <vtkCellArray.h>
+#include <vtkAppendPolyData.h>
+#include <vtkTextProperty.h>
+#include <vtkAbstractPicker.h>
+#include <vtkAbstractPropPicker.h>
+#include <vtkPlanes.h>
+#include <vtkPointPicker.h>
+#include <vtkMatrix4x4.h>
+#include <vtkInteractorObserver.h>
+#include <vtkCamera.h>
+
+//#include <q/visualization/vtk/vtkVertexBufferObjectMapper.h>
+
+using namespace cv;
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::Initialize ()
+{
+    modifier_ = temp_viz::InteractorStyle::KB_MOD_ALT;
+    // Set windows size (width, height) to unknown (-1)
+    win_size_ = Vec2i(-1, -1);
+    win_pos_ = Vec2i(0, 0);
+    max_win_size_ = Vec2i(-1, -1);
+
+    // Create the image filter and PNG writer objects
+    wif_ = vtkSmartPointer<vtkWindowToImageFilter>::New ();
+    snapshot_writer_ = vtkSmartPointer<vtkPNGWriter>::New ();
+    snapshot_writer_->SetInputConnection (wif_->GetOutputPort ());
+
+    init_ = true;
+    stereo_anaglyph_mask_default_ = true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::saveScreenshot (const std::string &file)
+{
+    FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+    wif_->SetInput (Interactor->GetRenderWindow ());
+    wif_->Modified ();      // Update the WindowToImageFilter
+    snapshot_writer_->Modified ();
+    snapshot_writer_->SetFileName (file.c_str ());
+    snapshot_writer_->Write ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::zoomIn ()
+{
+    FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+    // Zoom in
+    StartDolly ();
+    double factor = 10.0 * 0.2 * .5;
+    Dolly (pow (1.1, factor));
+    EndDolly ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::zoomOut ()
+{
+    FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+    // Zoom out
+    StartDolly ();
+    double factor = 10.0 * -0.2 * .5;
+    Dolly (pow (1.1, factor));
+    EndDolly ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnChar ()
+{
+    // Make sure we ignore the same events we handle in OnKeyDown to avoid calling things twice
+    FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+    if (Interactor->GetKeyCode () >= '0' && Interactor->GetKeyCode () <= '9')
+        return;
+    std::string key (Interactor->GetKeySym ());
+    if (key.find ("XF86ZoomIn") != std::string::npos)
+        zoomIn ();
+    else if (key.find ("XF86ZoomOut") != std::string::npos)
+        zoomOut ();
+
+    bool keymod = false;
+    switch (modifier_)
+    {
+    case KB_MOD_ALT:
+    {
+        keymod = Interactor->GetAltKey ();
+        break;
+    }
+    case KB_MOD_CTRL:
+    {
+        keymod = Interactor->GetControlKey ();
+        break;
+    }
+    case KB_MOD_SHIFT:
+    {
+        keymod = Interactor->GetShiftKey ();
+        break;
+    }
+    }
+
+    switch (Interactor->GetKeyCode ())
+    {
+    // All of the options below simply exit
+    case 'h': case 'H':
+    case 'l': case 'L':
+    case 'p': case 'P':
+    case 'j': case 'J':
+    case 'c': case 'C':
+    case 43:        // KEY_PLUS
+    case 45:        // KEY_MINUS
+    case 'f': case 'F':
+    case 'g': case 'G':
+    case 'o': case 'O':
+    case 'u': case 'U':
+    case 'q': case 'Q':
+    {
+        break;
+    }
+        // S and R have a special !ALT case
+    case 'r': case 'R':
+    case 's': case 'S':
+    {
+        if (!keymod)
+            Superclass::OnChar ();
+        break;
+    }
+    default:
+    {
+        Superclass::OnChar ();
+        break;
+    }
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::InteractorStyle::registerMouseCallback (boost::function<void (const cv::MouseEvent&)> callback)
+{
+    return (mouse_signal_.connect (callback));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::InteractorStyle::registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> callback)
+{
+    return (keyboard_signal_.connect (callback));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+temp_viz::InteractorStyle::OnKeyDown ()
+{
+    if (!init_)
+    {
+        std::cout << "[PCLVisualizerInteractorStyle] 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;
+        return;
+    }
+
+    FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+
+    if (wif_->GetInput () == NULL)
+    {
+        wif_->SetInput (Interactor->GetRenderWindow ());
+        wif_->Modified ();
+        snapshot_writer_->Modified ();
+    }
+
+    // Save the initial windows width/height
+    if (win_size_[0] == -1 || win_size_[1] == -1)
+        win_size_ = Vec2i(Interactor->GetRenderWindow ()->GetSize ());
+
+
+    // Get the status of special keys (Cltr+Alt+Shift)
+    bool shift = Interactor->GetShiftKey   ();
+    bool ctrl  = Interactor->GetControlKey ();
+    bool alt   = Interactor->GetAltKey ();
+
+    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;
+    }
+    }
+
+    std::string key (Interactor->GetKeySym ());
+    if (key.find ("XF86ZoomIn") != std::string::npos)
+        zoomIn ();
+    else if (key.find ("XF86ZoomOut") != std::string::npos)
+        zoomOut ();
+
+    switch (Interactor->GetKeyCode ())
+    {
+    case 'h': case 'H':
+    {
+        std::cout << "| Help:\n"
+                     "-------\n"
+                     "          p, P   : switch to a point-based representation\n"
+                     "          w, W   : switch to a wireframe-based representation (where available)\n"
+                     "          s, S   : switch to a surface-based representation (where available)\n"
+                     "\n"
+                     "          j, J   : take a .PNG snapshot of the current window view\n"
+                     "          c, C   : display current camera/window parameters\n"
+                     "          f, F   : fly to point mode\n"
+                     "\n"
+                     "          e, E   : exit the interactor\n"
+                     "          q, Q   : stop and call VTK's TerminateApp\n"
+                     "\n"
+                     "           +/-   : increment/decrement overall point size\n"
+                     "     +/- [+ ALT] : zoom in/out \n"
+                     "\n"
+                     "    r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]\n"
+                     "\n"
+                     "    ALT + s, S   : turn stereo mode on/off\n"
+                     "    ALT + f, F   : switch between maximized window mode and original size\n"
+                     "\n"
+                     "    SHIFT + left click   : select a point\n"
+                     << std::endl;
+        break;
+    }
+
+        // Switch representation to points
+    case 'p': case 'P':
+    {
+        vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+        vtkCollectionSimpleIterator ait;
+        for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+        {
+            for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+            {
+                vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ());
+                apart->GetProperty ()->SetRepresentationToPoints ();
+            }
+        }
+        break;
+    }
+        // Save a PNG snapshot with the current screen
+    case 'j': case 'J':
+    {
+        char cam_fn[80], snapshot_fn[80];
+        unsigned t = static_cast<unsigned> (time (0));
+        sprintf (snapshot_fn, "screenshot-%d.png" , t);
+        saveScreenshot (snapshot_fn);
+
+        sprintf (cam_fn, "screenshot-%d.cam", t);
+        ofstream ofs_cam;
+        ofs_cam.open (cam_fn);
+        vtkSmartPointer<vtkCamera> cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
+        double clip[2], focal[3], pos[3], view[3];
+        cam->GetClippingRange (clip);
+        cam->GetFocalPoint (focal);
+        cam->GetPosition (pos);
+        cam->GetViewUp (view);
+#ifndef M_PI
+        # define M_PI   3.14159265358979323846     // pi
+#endif
+
+        int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
+        int *win_size = Interactor->GetRenderWindow ()->GetSize ();
+        ofs_cam << clip[0]  << "," << clip[1]  << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
+                               pos[0]   << "," << pos[1]   << "," << pos[2]   << "/" << view[0]  << "," << view[1]  << "," << view[2] << "/" <<
+                               cam->GetViewAngle () / 180.0 * M_PI  << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
+                            << endl;
+        ofs_cam.close ();
+
+        std::cout << "Screenshot (" << snapshot_fn << ") and camera information (" << cam_fn << ") successfully captured." << std::endl;
+        break;
+    }
+        // display current camera settings/parameters
+    case 'c': case 'C':
+    {
+        vtkSmartPointer<vtkCamera> cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
+        double clip[2], focal[3], pos[3], view[3];
+        cam->GetClippingRange (clip);
+        cam->GetFocalPoint (focal);
+        cam->GetPosition (pos);
+        cam->GetViewUp (view);
+        int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
+        int *win_size = Interactor->GetRenderWindow ()->GetSize ();
+        std::cerr << clip[0]  << "," << clip[1]  << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
+                                 pos[0]   << "," << pos[1]   << "," << pos[2]   << "/" << view[0]  << "," << view[1]  << "," << view[2] << "/" <<
+                                 cam->GetViewAngle () / 180.0 * M_PI  << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
+                              << endl;
+        break;
+    }
+    case '=':
+    {
+        zoomIn();
+        break;
+    }
+    case 43:        // KEY_PLUS
+    {
+        if(alt)
+            zoomIn ();
+        else
+        {
+            vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+            vtkCollectionSimpleIterator ait;
+            for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+            {
+                for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+                {
+                    vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ());
+                    float psize = apart->GetProperty ()->GetPointSize ();
+                    if (psize < 63.0f)
+                        apart->GetProperty ()->SetPointSize (psize + 1.0f);
+                }
+            }
+        }
+        break;
+    }
+    case 45:        // KEY_MINUS
+    {
+        if(alt)
+            zoomOut ();
+        else
+        {
+            vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+            vtkCollectionSimpleIterator ait;
+            for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+            {
+                for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+                {
+                    vtkSmartPointer<vtkActor> apart = static_cast<vtkActor*> (path->GetLastNode ()->GetViewProp ());
+                    float psize = apart->GetProperty ()->GetPointSize ();
+                    if (psize > 1.0f)
+                        apart->GetProperty ()->SetPointSize (psize - 1.0f);
+                }
+            }
+        }
+        break;
+    }
+        // Switch between maximize and original window size
+    case 'f': case 'F':
+    {
+        if (keymod)
+        {
+            Vec2i screen_size(Interactor->GetRenderWindow ()->GetScreenSize ());
+            Vec2i win_size(Interactor->GetRenderWindow ()->GetSize ());
+
+            // Is window size = max?
+            if (win_size == max_win_size_)
+            {
+                Interactor->GetRenderWindow ()->SetSize (win_size_.val);
+                Interactor->GetRenderWindow ()->SetPosition (win_pos_.val);
+                Interactor->GetRenderWindow ()->Render ();
+                Interactor->Render ();
+            }
+            // Set to max
+            else
+            {
+                win_pos_ = Vec2i(Interactor->GetRenderWindow ()->GetPosition ());
+                win_size_ = win_size;
+
+                Interactor->GetRenderWindow ()->SetSize (screen_size.val);
+                Interactor->GetRenderWindow ()->Render ();
+                Interactor->Render ();
+                max_win_size_ = Vec2i(Interactor->GetRenderWindow ()->GetSize ());
+            }
+        }
+        else
+        {
+            AnimState = VTKIS_ANIM_ON;
+            vtkAssemblyPath *path = NULL;
+            Interactor->GetPicker ()->Pick (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1], 0.0, CurrentRenderer);
+            vtkAbstractPropPicker *picker;
+            if ((picker = vtkAbstractPropPicker::SafeDownCast (Interactor->GetPicker ())))
+                path = picker->GetPath ();
+            if (path != NULL)
+                Interactor->FlyTo (CurrentRenderer, picker->GetPickPosition ());
+            AnimState = VTKIS_ANIM_OFF;
+        }
+        break;
+    }
+        // 's'/'S' w/out ALT
+    case 's': case 'S':
+    {
+        if (keymod)
+        {
+            int stereo_render = Interactor->GetRenderWindow ()->GetStereoRender ();
+            if (!stereo_render)
+            {
+                if (stereo_anaglyph_mask_default_)
+                {
+                    Interactor->GetRenderWindow ()->SetAnaglyphColorMask (4, 3);
+                    stereo_anaglyph_mask_default_ = false;
+                }
+                else
+                {
+                    Interactor->GetRenderWindow ()->SetAnaglyphColorMask (2, 5);
+                    stereo_anaglyph_mask_default_ = true;
+                }
+            }
+            Interactor->GetRenderWindow ()->SetStereoRender (!stereo_render);
+            Interactor->GetRenderWindow ()->Render ();
+            Interactor->Render ();
+        }
+        else
+            Superclass::OnKeyDown ();
+        break;
+    }
+
+    case 'o': case 'O':
+    {
+        vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+        int flag = cam->GetParallelProjection ();
+        cam->SetParallelProjection (!flag);
+
+        CurrentRenderer->SetActiveCamera (cam);
+        CurrentRenderer->Render ();
+        break;
+    }
+
+    // Overwrite the camera reset
+    case 'r': case 'R':
+    {
+        if (!keymod)
+        {
+            Superclass::OnKeyDown ();
+            break;
+        }
+
+        vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+
+        static CloudActorMap::iterator it = actors_->begin ();
+        // it might be that some actors don't have a valid transformation set -> we skip them to avoid a seg fault.
+        bool found_transformation = false;
+
+        for (size_t idx = 0; idx < actors_->size (); ++idx, ++it)
+        {
+            if (it == actors_->end ())
+                it = actors_->begin ();
+
+            const CloudActor& actor = it->second;
+            if (actor.viewpoint_transformation_.GetPointer ())
+            {
+                found_transformation = true;
+                break;
+            }
+        }
+
+        // if a valid transformation was found, use it otherwise fall back to default view point.
+        if (found_transformation)
+        {
+            const CloudActor& actor = it->second;
+            cam->SetPosition (actor.viewpoint_transformation_->GetElement (0, 3),
+                              actor.viewpoint_transformation_->GetElement (1, 3),
+                              actor.viewpoint_transformation_->GetElement (2, 3));
+
+            cam->SetFocalPoint (actor.viewpoint_transformation_->GetElement (0, 3) - actor.viewpoint_transformation_->GetElement (0, 2),
+                                actor.viewpoint_transformation_->GetElement (1, 3) - actor.viewpoint_transformation_->GetElement (1, 2),
+                                actor.viewpoint_transformation_->GetElement (2, 3) - actor.viewpoint_transformation_->GetElement (2, 2));
+
+            cam->SetViewUp (actor.viewpoint_transformation_->GetElement (0, 1),
+                            actor.viewpoint_transformation_->GetElement (1, 1),
+                            actor.viewpoint_transformation_->GetElement (2, 1));
+        }
+        else
+        {
+            cam->SetPosition (0, 0, 0);
+            cam->SetFocalPoint (0, 0, 1);
+            cam->SetViewUp (0, -1, 0);
+        }
+
+        // go to the next actor for the next key-press event.
+        if (it != actors_->end ())
+            ++it;
+        else
+            it = actors_->begin ();
+
+        CurrentRenderer->SetActiveCamera (cam);
+        CurrentRenderer->ResetCameraClippingRange ();
+        CurrentRenderer->Render ();
+        break;
+    }
+
+    case 'q': case 'Q':
+    {
+        Interactor->ExitCallback ();
+        return;
+    }
+    default:
+    {
+        Superclass::OnKeyDown ();
+        break;
+    }
+    }
+
+    KeyboardEvent event (true, Interactor->GetKeySym (), Interactor->GetKeyCode (), Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    keyboard_signal_ (event);
+
+    renderer_->Render ();
+    Interactor->Render ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnKeyUp ()
+{
+    KeyboardEvent event (false, Interactor->GetKeySym (), Interactor->GetKeyCode (), Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    keyboard_signal_ (event);
+    Superclass::OnKeyUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseMove ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseMove, MouseEvent::NoButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnMouseMove ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnLeftButtonDown ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+    MouseEvent event (type, MouseEvent::LeftButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnLeftButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnLeftButtonUp ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::LeftButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnLeftButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMiddleButtonDown ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+
+    MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+    MouseEvent event (type, MouseEvent::MiddleButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnMiddleButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMiddleButtonUp ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::MiddleButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnMiddleButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnRightButtonDown ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+
+    MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+    MouseEvent event (type, MouseEvent::RightButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnRightButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnRightButtonUp ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::RightButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    Superclass::OnRightButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseWheelForward ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseScrollUp, MouseEvent::VScroll, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    if (Interactor->GetRepeatCount ())
+        mouse_signal_ (event);
+
+    if (Interactor->GetAltKey ())
+    {
+        // zoom
+        vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+        double opening_angle = cam->GetViewAngle ();
+        if (opening_angle > 15.0)
+            opening_angle -= 1.0;
+
+        cam->SetViewAngle (opening_angle);
+        cam->Modified ();
+        CurrentRenderer->SetActiveCamera (cam);
+        CurrentRenderer->ResetCameraClippingRange ();
+        CurrentRenderer->Modified ();
+        CurrentRenderer->Render ();
+        renderer_->Render ();
+        Interactor->Render ();
+    }
+    else
+        Superclass::OnMouseWheelForward ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseWheelBackward ()
+{
+    Vec2i p(Interactor->GetEventPosition());
+    MouseEvent event (MouseEvent::MouseScrollDown, MouseEvent::VScroll, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+    mouse_signal_ (event);
+    if (Interactor->GetRepeatCount ())
+        mouse_signal_ (event);
+
+    if (Interactor->GetAltKey ())
+    {
+        // zoom
+        vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+        double opening_angle = cam->GetViewAngle ();
+        if (opening_angle < 170.0)
+            opening_angle += 1.0;
+
+        cam->SetViewAngle (opening_angle);
+        cam->Modified ();
+        CurrentRenderer->SetActiveCamera (cam);
+        CurrentRenderer->ResetCameraClippingRange ();
+        CurrentRenderer->Modified ();
+        CurrentRenderer->Render ();
+        renderer_->Render ();
+        Interactor->Render ();
+    }
+    else
+        Superclass::OnMouseWheelBackward ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnTimer ()
+{
+    if (!init_)
+    {
+        std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing.\n" << std::endl;
+        return;
+    }
+
+    if (!renderer_)
+    {
+        std::cout <<  "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl;
+        return;
+    }
+    renderer_->Render ();
+    Interactor->Render ();
+}
+
+
+namespace temp_viz
+{
+    // Standard VTK macro for *New ()
+    vtkStandardNewMacro (InteractorStyle);
+
+}
+

modules/viz/src/mesh_load.cpp

+#include <opencv2/viz/mesh_load.hpp>
+
+#include "precomp.hpp"
+
+#include <vtkPLYReader.h>
+#include <vtkSmartPointer.h>
+#include <vtkPolyData.h>
+#include <vtkPointData.h>
+#include <vtkCellArray.h>
+
+temp_viz::Mesh3d::Ptr temp_viz::mesh_load(const String& file)
+{
+    Mesh3d::Ptr mesh = new Mesh3d();
+
+    vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New();
+    reader->SetFileName(file.c_str());
+    reader->Update();
+    vtkSmartPointer<vtkPolyData> poly_data = reader->GetOutput ();
+
+    typedef unsigned int uint32_t;
+    mesh->polygons.clear();
+
+    vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
+    vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
+    vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
+
+    mesh->cloud.create(1, nr_points, CV_32FC3);
+
+    double point_xyz[3];
+    for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
+    {
+        mesh_points->GetPoint (i, &point_xyz[0]);
+        mesh->cloud.ptr<cv::Point3f>()[i] = cv::Point3d(point_xyz[0], point_xyz[1], point_xyz[2]);;
+    }
+
+    // Then the color information, if any
+    vtkUnsignedCharArray* poly_colors = NULL;
+    if (poly_data->GetPointData() != NULL)
+        poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
+
+    // some applications do not save the name of scalars (including PCL's native vtk_io)
+    if (!poly_colors && poly_data->GetPointData () != NULL)
+        poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
+
+    if (!poly_colors && poly_data->GetPointData () != NULL)
+        poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
+
+    // TODO: currently only handles rgb values with 3 components
+    if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
+    {
+        mesh->colors.create(1, nr_points, CV_8UC3);
+        unsigned char point_color[3];
+
+        for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
+        {
+            poly_colors->GetTupleValue (i, &point_color[0]);
+
+            //RGB or BGR?????
+            mesh->colors.ptr<cv::Vec3b>()[i] = cv::Vec3b(point_color[0], point_color[1], point_color[2]);
+        }
+    }
+    else
+        mesh->colors.release();
+
+    // Now handle the polygons
+    mesh->polygons.resize (nr_polygons);
+    vtkIdType* cell_points;
+    vtkIdType nr_cell_points;
+    vtkCellArray * mesh_polygons = poly_data->GetPolys ();
+    mesh_polygons->InitTraversal ();
+    int id_poly = 0;
+    while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
+    {
+        mesh->polygons[id_poly].vertices.resize (nr_cell_points);
+        for (int i = 0; i < nr_cell_points; ++i)
+            mesh->polygons[id_poly].vertices[i] = static_cast<int> (cell_points[i]);
+        ++id_poly;
+    }
+
+    return mesh;
+}

modules/viz/src/precomp.cpp

+#include "precomp.hpp"
\ No newline at end of file

modules/viz/src/precomp.hpp

+#pragma once
+
+#include <opencv2/core.hpp>
+#include <map>
+#include <vector>
+
+
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/signals2.hpp>
+#include <boost/thread.hpp>
+
+#include <Eigen/Geometry>
+
+#if defined __GNUC__
+#pragma GCC system_header
+#ifdef __DEPRECATED
+#undef __DEPRECATED
+#define __DEPRECATED_DISABLED__
+#endif
+#endif
+
+#include <vtkAppendPolyData.h>
+#include <vtkAssemblyPath.h>
+#include <vtkAxesActor.h>
+#include <vtkActor.h>
+#include <vtkBoxRepresentation.h>
+#include <vtkBoxWidget.h>
+#include <vtkBoxWidget2.h>
+#include <vtkCellData.h>
+#include <vtkMath.h>
+#include <vtkLoopSubdivisionFilter.h>
+#include <vtkLineSource.h>
+#include <vtkLegendScaleActor.h>
+#include <vtkLightKit.h>
+#include <vtkPlatonicSolidSource.h>
+#include <vtkPropPicker.h>
+#include <vtkGeneralTransform.h>
+#include <vtkSmartPointer.h>
+#include <vtkDataSet.h>
+#include <vtkDataSetSurfaceFilter.h>
+#include <vtkExecutive.h>
+#include <vtkPolygon.h>
+#include <vtkPointPicker.h>
+#include <vtkUnstructuredGrid.h>
+#include <vtkConeSource.h>
+#include <vtkDiskSource.h>
+#include <vtkPlaneSource.h>
+#include <vtkSphereSource.h>
+#include <vtkIdentityTransform.h>
+#include <vtkTransform.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkTubeFilter.h>
+#include <vtkCubeSource.h>
+#include <vtkAxes.h>
+#include <vtkFloatArray.h>
+#include <vtkPointData.h>
+#include <vtkPolyData.h>
+#include <vtkPolyDataReader.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkDataSetMapper.h>
+#include <vtkCellArray.h>
+#include <vtkCommand.h>
+#include <vtkCellLocator.h>
+#include <vtkPLYReader.h>
+#include <vtkTransformFilter.h>
+#include <vtkPolyLine.h>
+#include <vtkVectorText.h>
+#include <vtkFollower.h>
+#include <vtkCallbackCommand.h>
+#include <vtkInteractorStyle.h>
+#include <vtkInformationVector.h>
+#include <vtkDataArray.h>
+#include <vtkUnsignedCharArray.h>
+#include <vtkPoints.h>
+#include <vtkRendererCollection.h>
+#include <vtkPNGWriter.h>
+#include <vtkWindowToImageFilter.h>
+#include <vtkInteractorStyleTrackballCamera.h>
+#include <vtkProperty.h>
+#include <vtkCamera.h>
+#include <vtkObjectFactory.h>
+#include <vtkScalarBarActor.h>
+#include <vtkScalarsToColors.h>
+#include <vtkClipPolyData.h>
+#include <vtkPlanes.h>
+#include <vtkImageImport.h>
+#include <vtkImageViewer.h>
+#include <vtkInteractorStyleImage.h>
+#include <vtkImageFlip.h>
+#include <vtkTIFFWriter.h>
+#include <vtkBMPWriter.h>
+#include <vtkJPEGWriter.h>
+#include <vtkImageViewer2.h>
+#include <vtkRenderWindow.h>
+#include <vtkXYPlotActor.h>
+#include <vtkTextProperty.h>
+#include <vtkProperty2D.h>
+#include <vtkFieldData.h>
+#include <vtkDoubleArray.h>
+#include <vtkLODActor.h>
+#include <vtkPolyDataWriter.h>
+#include <vtkTextActor.h>
+#include <vtkCleanPolyData.h>
+#include <vtkRenderer.h>
+#include <vtkObject.h>
+#include <vtkOrientationMarkerWidget.h>
+#include <vtkImageReslice.h>
+#include <vtkImageChangeInformation.h>
+#include <vtkImageCanvasSource2D.h>
+#include <vtkImageBlend.h>
+#include <vtkImageStencilData.h>
+
+#include <vtkRenderWindowInteractor.h>
+#include <vtkChartXY.h>
+#include <vtkPlot.h>
+#include <vtkTable.h>
+#include <vtkContextView.h>
+#include <vtkContextScene.h>
+#include <vtkColorSeries.h>
+#include <vtkAxis.h>
+#include <vtkSelection.h>
+#include <vtkHardwareSelector.h>
+#include <vtkTriangle.h>
+#include <vtkWorldPointPicker.h>
+#include <vtkInteractorStyleRubberBandPick.h>
+#include <vtkInteractorStyleTrackballActor.h>
+#include <vtkAreaPicker.h>
+#include <vtkExtractGeometry.h>
+#include <vtkExtractPolyDataGeometry.h>
+#include <vtkVertexGlyphFilter.h>
+#include <vtkIdFilter.h>
+#include <vtkIdTypeArray.h>
+#include <vtkImageReader2Factory.h>
+#include <vtkImageReader2.h>
+#include <vtkImageData.h>
+
+
+
+#include <vtkPolyDataNormals.h>
+#include <vtkMapper.h>
+#include <vtkSelectionNode.h>
+
+
+
+#include <vtkAbstractPicker.h>
+#include <vtkAbstractPropPicker.h>
+#include <vtkPointPicker.h>
+#include <vtkMatrix4x4.h>
+#include <vtkInteractorObserver.h>
+
+
+
+#if defined __GNUC__ && defined __DEPRECATED_DISABLED__
+#define __DEPRECATED
+#undef __DEPRECATED_DISABLED__
+#endif

modules/viz/src/q/common.h

+#pragma once
+
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+#include <opencv2/viz/types.hpp>
+//#include <vtkMatrix4x4.h>
+
+namespace temp_viz
+{
+    //CV_EXPORTS Eigen::Matrix4d vtkToEigen (vtkMatrix4x4* vtk_matrix);
+    //CV_EXPORTS Eigen::Vector2i worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height);
+    //CV_EXPORTS void getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24]);
+
+//    enum FrustumCull
+//    {
+//        PCL_INSIDE_FRUSTUM,
+//        PCL_INTERSECT_FRUSTUM,
+//        PCL_OUTSIDE_FRUSTUM
+//    };
+
+    //CV_EXPORTS int cullFrustum (double planes[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb);
+    //CV_EXPORTS float viewScreenArea (const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height);
+
+    enum RenderingProperties
+    {
+        VIZ_POINT_SIZE,
+        VIZ_OPACITY,
+        VIZ_LINE_WIDTH,
+        VIZ_FONT_SIZE,
+        VIZ_COLOR,
+        VIZ_REPRESENTATION,
+        VIZ_IMMEDIATE_RENDERING,
+        VIZ_SHADING
+    };
+
+    enum RenderingRepresentationProperties
+    {
+        REPRESENTATION_POINTS,
+        REPRESENTATION_WIREFRAME,
+        REPRESENTATION_SURFACE
+    };
+
+    enum ShadingRepresentationProperties
+    {
+        SHADING_FLAT,
+        SHADING_GOURAUD,
+        SHADING_PHONG
+    };
+
+    class CV_EXPORTS Camera
+    {
+    public:
+        /** Focal point or lookAt. The view direction can be obtained by (focal-pos).normalized () */
+        Vec3d focal;
+
+        /** \brief Position of the camera. */
+        Vec3d pos;
+
+        /** \brief Up vector of the camera. */
+        Vec3d view_up;
+
+        /** \brief Near/far clipping planes depths */
+        Vec2d clip;
+
+        /** \brief Field of view angle in y direction (radians). */
+        double fovy;
+
+        // the following variables are the actual position and size of the window on the screen and NOT the viewport!
+        // except for the size, which is the same the viewport is assumed to be centered and same size as the window.
+        Vec2i window_size;
+        Vec2i window_pos;
+
+        /** \brief Computes View matrix for Camera (Based on gluLookAt)
+          * \param[out] view_mat the resultant matrix
+          */
+        void computeViewMatrix(Affine3d& view_mat) const;
+
+        /** \brief Computes Projection Matrix for Camera
+          *  \param[out] proj the resultant matrix
+          */
+        void computeProjectionMatrix(Matx44d& proj) const;
+
+        /** \brief converts point to window coordiantes
+          * \param[in] pt xyz point to be converted
+          * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+          *
+          * This function computes the projection and view matrix every time.
+          * It is very inefficient to use this for every point in the point cloud!
+          */
+        void cvtWindowCoordinates (const cv::Point3f& pt, Vec4d& window_cord) const
+        {
+            Affine3d view;
+            computeViewMatrix (view);
+
+            Matx44d proj;
+            computeProjectionMatrix (proj);
+            cvtWindowCoordinates (pt, proj * view.matrix, window_cord);
+            return;
+        }
+
+        /** \brief converts point to window coordiantes
+          * \param[in] pt xyz point to be converted
+          * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+          * \param[in] composite_mat composite transformation matrix (proj*view)
+          *
+          * Use this function to compute window coordinates with a precomputed
+          * transformation function.  The typical composite matrix will be
+          * the projection matrix * the view matrix.  However, additional
+          * matrices like a camera disortion matrix can also be added.
+          */
+        void cvtWindowCoordinates (const Point3f& pt, Matx44d& composite_mat, Vec4d& window_cord) const
+        {
+            Vec4d pte (pt.x, pt.y, pt.z, 1);
+            window_cord = composite_mat * pte;
+            window_cord = window_cord/window_cord[3];
+
+            window_cord[0] = (window_cord[0]+1.0) / 2.0*window_size[0];
+            window_cord[1] = (window_cord[1]+1.0) / 2.0*window_size[1];
+            window_cord[2] = (window_cord[2]+1.0) / 2.0;
+        }
+    };
+
+}

modules/viz/src/q/interactor_style.h

+#pragma once
+
+#include <q/viz_types.h>
+#include <opencv2/viz/events.hpp>
+
+namespace temp_viz
+{
+        /** \brief PCLVisualizerInteractorStyle defines an unique, custom VTK
+          * based interactory style for PCL Visualizer applications. Besides
+          * defining the rendering style, we also create a list of custom actions
+          * that are triggered on different keys being pressed:
+          *
+          * -        p, P   : switch to a point-based representation
+          * -        w, W   : switch to a wireframe-based representation (where available)
+          * -        s, S   : switch to a surface-based representation (where available)
+          * -        j, J   : take a .PNG snapshot of the current window view
+          * -        c, C   : display current camera/window parameters
+          * -        f, F   : fly to point mode
+          * -        e, E   : exit the interactor\
+          * -        q, Q   : stop and call VTK's TerminateApp
+          * -       + / -   : increment/decrement overall point size
+          * -  r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]
+          * -  ALT + s, S   : turn stereo mode on/off
+          * -  ALT + f, F   : switch between maximized window mode and original size
+          * -
+          * -  SHIFT + left click   : select a point
+          *
+          * \author Radu B. Rusu
+          * \ingroup visualization
+          */
+        class CV_EXPORTS InteractorStyle : public vtkInteractorStyleTrackballCamera
+        {
+        public:
+
+            enum KeyboardModifier
+            {
+                KB_MOD_ALT,
+                KB_MOD_CTRL,
+                KB_MOD_SHIFT
+            };
+
+            static InteractorStyle *New ();
+
+
+            InteractorStyle () {}
+            virtual ~InteractorStyle () {}
+
+            // this macro defines Superclass, the isA functionality and the safe downcast method
+            vtkTypeMacro (InteractorStyle, vtkInteractorStyleTrackballCamera);
+
+            /** \brief Initialization routine. Must be called before anything else. */
+            virtual void Initialize ();
+
+            /** \brief Pass a pointer to the actor map
+                  * \param[in] actors the actor map that will be used with this style
+                  */
+            inline void setCloudActorMap (const cv::Ptr<CloudActorMap>& actors) { actors_ = actors; }
+
+
+            /** \brief Pass a set of renderers to the interactor style.
+                  * \param[in] rens the vtkRendererCollection to use
+                  */
+            void setRenderer (vtkSmartPointer<vtkRenderer>& ren) { renderer_ = ren; }
+
+            /** \brief Register a callback function for mouse events
+                  * \param[in] cb a boost function that will be registered as a callback for a mouse event
+                  * \return a connection object that allows to disconnect the callback function.
+                  */
+            boost::signals2::connection registerMouseCallback (boost::function<void (const cv::MouseEvent&)> cb);
+
+            /** \brief Register a callback boost::function for keyboard events
+                  * \param[in] cb a boost function that will be registered as a callback for a keyboard event
+                  * \return a connection object that allows to disconnect the callback function.
+                  */
+            boost::signals2::connection registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> cb);
+
+
+            /** \brief Save the current rendered image to disk, as a PNG screenshot.
+                  * \param[in] file the name of the PNG file
+                  */
+            void saveScreenshot (const std::string &file);
+
+            /** \brief Change the default keyboard modified from ALT to a different special key.
+                  * Allowed values are:
+                  * - KB_MOD_ALT
+                  * - KB_MOD_CTRL
+                  * - KB_MOD_SHIFT
+                  * \param[in] modifier the new keyboard modifier
+                  */
+            inline void setKeyboardModifier (const KeyboardModifier &modifier) { modifier_ = modifier; }
+        protected:
+            /** \brief Set to true after initialization is complete. */
+            bool init_;
+
+            /** \brief Collection of vtkRenderers stored internally. */
+            //vtkSmartPointer<vtkRendererCollection> rens_;
+            vtkSmartPointer<vtkRenderer> renderer_;
+
+            /** \brief Actor map stored internally. */
+            cv::Ptr<CloudActorMap> actors_;
+
+            /** \brief The current window width/height. */
+            Vec2i win_size_;
+
+            /** \brief The current window position x/y. */
+            Vec2i win_pos_;
+
+            /** \brief The maximum resizeable window width/height. */
+            Vec2i max_win_size_;
+
+            /** \brief A PNG writer for screenshot captures. */
+            vtkSmartPointer<vtkPNGWriter> snapshot_writer_;
+            /** \brief Internal window to image filter. Needed by \a snapshot_writer_. */
+            vtkSmartPointer<vtkWindowToImageFilter> wif_;
+
+            boost::signals2::signal<void (const cv::MouseEvent&)> mouse_signal_;
+            boost::signals2::signal<void (const cv::KeyboardEvent&)> keyboard_signal_;
+
+            /** \brief Interactor style internal method. Gets called whenever a key is pressed. */
+            virtual void OnChar ();
+
+            // Keyboard events
+            virtual void OnKeyDown ();
+            virtual void OnKeyUp ();
+
+            // mouse button events
+            virtual void OnMouseMove ();
+            virtual void OnLeftButtonDown ();
+            virtual void OnLeftButtonUp ();
+            virtual void OnMiddleButtonDown ();
+            virtual void OnMiddleButtonUp ();
+            virtual void OnRightButtonDown ();
+            virtual void OnRightButtonUp ();
+            virtual void OnMouseWheelForward ();
+            virtual void OnMouseWheelBackward ();
+
+            /** \brief Interactor style internal method. Gets called periodically if a timer is set. */
+            virtual void OnTimer ();
+
+
+            void zoomIn ();
+            void zoomOut ();
+
+            /** \brief True if we're using red-blue colors for anaglyphic stereo, false if magenta-green. */
+            bool stereo_anaglyph_mask_default_;
+
+            /** \brief The keyboard modifier to use. Default: Alt. */
+            KeyboardModifier modifier_;
+        };
+}

modules/viz/src/q/shapes.h

+#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);
+
+    /** \brief Create a cylinder shape from a set of model coefficients.
+      * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
+      * \param[in] numsides (optional) the number of sides used for rendering the cylinder
+      *
+      * \code
+      * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCylinder)
+      * // Eigen::Vector3f pt_on_axis, axis_direction;
+      * // float radius;
+      *
+      * temp_viz::ModelCoefficients cylinder_coeff;
+      * cylinder_coeff.values.resize (7);    // We need 7 values
+      * cylinder_coeff.values[0] = pt_on_axis.x ();
+      * cylinder_coeff.values[1] = pt_on_axis.y ();
+      * cylinder_coeff.values[2] = pt_on_axis.z ();
+      *
+      * cylinder_coeff.values[3] = axis_direction.x ();
+      * cylinder_coeff.values[4] = axis_direction.y ();
+      * cylinder_coeff.values[5] = axis_direction.z ();
+      *
+      * cylinder_coeff.values[6] = radius;
+      *
+      * vtkSmartPointer<vtkDataSet> data = temp_viz::createCylinder (cylinder_coeff, numsides);
+      * \endcode
+      *
+      * \ingroup visualization
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides = 30);
+
+
+    /** \brief Create a planar shape from a set of model coefficients.
+      * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+      *
+      * \code
+      * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelPlane)
+      * // Eigen::Vector4f plane_parameters;
+      *
+      * temp_viz::ModelCoefficients plane_coeff;
+      * plane_coeff.values.resize (4);    // We need 4 values
+      * plane_coeff.values[0] = plane_parameters.x ();
+      * plane_coeff.values[1] = plane_parameters.y ();
+      * plane_coeff.values[2] = plane_parameters.z ();
+      * plane_coeff.values[3] = plane_parameters.w ();
+      *
+      * vtkSmartPointer<vtkDataSet> data = temp_viz::createPlane (plane_coeff);
+      * \endcode
+      *
+      * \ingroup visualization
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients);
+
+    /** \brief Create a planar shape from a set of model coefficients.
+      * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+      * \param[in] x,y,z projection of this point on the plane is used to get the center of the plane.
+      * \ingroup visualization
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z);
+    
+    /** \brief Create a 2d circle shape from a set of model coefficients.
+      * \param[in] coefficients the model coefficients (x, y, radius)
+      * \param[in] z (optional) specify a z value (default: 0)
+      *
+      * \code
+      * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCircle2D)
+      * // float x, y, radius;
+      *
+      * temp_viz::ModelCoefficients circle_coeff;
+      * circle_coeff.values.resize (3);    // We need 3 values
+      * circle_coeff.values[0] = x;
+      * circle_coeff.values[1] = y;
+      * circle_coeff.values[2] = radius;
+      *
+      * vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
+      * \endcode
+      *
+      * \ingroup visualization
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z = 0.0);
+
+
+    /** \brief Creaet a cube shape from a set of model coefficients.
+      * \param[in] coefficients the cube coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
+      * \ingroup visualization 
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const temp_viz::ModelCoefficients &coefficients);
+
+    /** \brief Creaet a cube shape from a set of model coefficients.
+      *
+      * \param[in] translation a translation to apply to the cube from 0,0,0
+      * \param[in] rotation a quaternion-based rotation to apply to the cube 
+      * \param[in] width the cube's width
+      * \param[in] height the cube's height
+      * \param[in] depth the cube's depth
+      * \ingroup visualization 
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth);
+    
+    /** \brief Create a cube from a set of bounding points
+      * \param[in] x_min is the minimum x value of the box
+      * \param[in] x_max is the maximum x value of the box
+      * \param[in] y_min is the minimum y value of the box 
+      * \param[in] y_max is the maximum y value of the box
+      * \param[in] z_min is the minimum z value of the box
+      * \param[in] z_max is the maximum z value of the box
+      * \param[in] id the cube id/name (default: "cube")
+      * \param[in] viewport (optional) the id of the new viewport (default: 0)
+      */
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max);
+    
+    /** \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);
+}

modules/viz/src/q/viz3d_impl.hpp

+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/viz/events.hpp>
+#include <q/interactor_style.h>
+#include <q/viz_types.h>
+#include <q/common.h>
+#include <opencv2/viz/types.hpp>
+#include <opencv2/core/affine.hpp>
+#include <opencv2/viz/viz3d.hpp>
+
+namespace temp_viz
+{
+
+class CV_EXPORTS Viz3d::VizImpl
+{
+public:
+    typedef cv::Ptr<VizImpl> Ptr;
+
+    VizImpl (const std::string &name = std::string());
+
+    virtual ~VizImpl ();
+    void setFullScreen (bool mode);
+    void setWindowName (const std::string &name);
+
+    /** \brief Register a callback boost::function for keyboard events
+          * \param[in] cb a boost function that will be registered as a callback for a keyboard event
+          * \return a connection object that allows to disconnect the callback function.
+          */
+    boost::signals2::connection registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> cb);
+    inline boost::signals2::connection registerKeyboardCallback (void (*callback) (const cv::KeyboardEvent&, void*), void* cookie = NULL)
+    { return (registerKeyboardCallback (boost::bind (callback, _1, cookie))); }
+
+    /** \brief Register a callback function for keyboard events
+          * \param[in] callback  the member function that will be registered as a callback for a keyboard event
+          * \param[in] instance  instance to the class that implements the callback function
+          * \param[in] cookie    user data that is passed to the callback
+          * \return a connection object that allows to disconnect the callback function.
+          */
+    template<typename T> inline boost::signals2::connection registerKeyboardCallback (void (T::*callback) (const cv::KeyboardEvent&, void*), T& instance, void* cookie = NULL)
+    { return (registerKeyboardCallback (boost::bind (callback,  boost::ref (instance), _1, cookie))); }
+
+    /** \brief Register a callback function for mouse events
+          * \param[in] cb a boost function that will be registered as a callback for a mouse event
+          * \return a connection object that allows to disconnect the callback function.
+          */
+    boost::signals2::connection registerMouseCallback (boost::function<void (const cv::MouseEvent&)> cb);
+    inline boost::signals2::connection registerMouseCallback (void (*callback) (const cv::MouseEvent&, void*), void* cookie = NULL)
+    { return (registerMouseCallback (boost::bind (callback, _1, cookie))); }
+
+    /** \brief Register a callback function for mouse events
+          * \param[in] callback  the member function that will be registered as a callback for a mouse event
+          * \param[in] instance  instance to the class that implements the callback function
+          * \param[in] cookie    user data that is passed to the callback
+          * \return a connection object that allows to disconnect the callback function.
+          */
+    template<typename T> inline boost::signals2::connection registerMouseCallback (void (T::*callback) (const cv::MouseEvent&, void*), T& instance, void* cookie = NULL)
+    { return (registerMouseCallback (boost::bind (callback, boost::ref (instance), _1, cookie))); }
+
+    void spin ();
+    void spinOnce (int time = 1, bool force_redraw = false);
+
+    /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z, Roll,Pitch,Yaw
+           *
+           * \param[in] scale the scale of the axes (default: 1)
+           * \param[in] t transformation matrix
+           *
+           * RPY Angles
+           * Rotate the reference frame by the angle roll about axis x
+           * Rotate the reference frame by the angle pitch about axis y
+           * Rotate the reference frame by the angle yaw about axis z
+           *
+           * Description:
+           * Sets the orientation of the Prop3D.  Orientation is specified as
+           * X,Y and Z rotations in that order, but they are performed as
+           * RotateZ, RotateX, and finally RotateY.
+           *
+           * All axies use right hand rule. x=red axis, y=green axis, z=blue axis
+           * z direction is point into the screen.
+           *     z
+           *      \
+           *       \
+           *        \
+           *         -----------> x
+           *         |
+           *         |
+           *         |
+           *         |
+           *         |
+           *         |
+           *         y
+           */
+    void addCoordinateSystem (double scale, const cv::Affine3f& t, const std::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")
+    {
+        // 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 removeText3D (const std::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 = "");
+
+    /** \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 addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = "");
+
+    bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud");
+    void addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat());
+    bool updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat());
+
+    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 addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::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");
+
+    bool setShapeRenderingProperties (int property, double value, const std::string &id);
+    void setShapeColor (const Color& color, const std::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" );
+
+    /** \brief Returns true when the user tried to close the window */
+    bool wasStopped () const { if (interactor_ != NULL) return (stopped_); else return true; }
+
+    /** \brief Set the stopped flag back to false */
+    void resetStoppedFlag () { if (interactor_ != NULL) stopped_ = false; }
+
+    /** \brief Stop the interaction and close the visualizaton window. */
+    void close ()
+    {
+        stopped_ = true;
+        // This tends to close the window...
+        interactor_->TerminateApp ();
+    }
+
+    bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon");
+    bool addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id = "line");
+    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 addSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id = "sphere");
+    bool updateSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id = "sphere");
+
+    // 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");
+
+    /** \brief Add a cylinder from a set of given model coefficients
+          * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
+          * \param[in] id the cylinder id/name (default: "cylinder")
+          *
+          * \code
+          * // The following are given (or computed using sample consensus techniques)
+          * // See SampleConsensusModelCylinder for more information.
+          * // float radius;
+          *
+          * temp_viz::ModelCoefficients cylinder_coeff;
+          * cylinder_coeff.values.resize (7);    // We need 7 values
+          * cylinder_coeff.values[0] = pt_on_axis.x ();
+          * cylinder_coeff.values[1] = pt_on_axis.y ();
+          * cylinder_coeff.values[2] = pt_on_axis.z ();
+          *
+          * cylinder_coeff.values[3] = axis_direction.x ();
+          * cylinder_coeff.values[4] = axis_direction.y ();
+          * cylinder_coeff.values[5] = axis_direction.z ();
+          *
+          * cylinder_coeff.values[6] = radius;
+          *
+          * addCylinder (cylinder_coeff);
+          * \endcode
+          */
+    bool addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cylinder");
+
+    /** \brief Add a plane from a set of given model coefficients
+          * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+          * \param[in] id the plane id/name (default: "plane")
+          *
+          * \code
+          * // The following are given (or computed using sample consensus techniques)
+          * // See SampleConsensusModelPlane for more information
+
+          *
+          * temp_viz::ModelCoefficients plane_coeff;
+          * plane_coeff.values.resize (4);    // We need 4 values
+          * plane_coeff.values[0] = plane_parameters.x ();
+          * plane_coeff.values[1] = plane_parameters.y ();
+          * plane_coeff.values[2] = plane_parameters.z ();
+          * plane_coeff.values[3] = plane_parameters.w ();
+          *
+          * addPlane (plane_coeff);
+          * \endcode
+          */
+    bool addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "plane");
+    bool addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id = "plane");
+
+    /** \brief Add a circle from a set of given model coefficients
+          * \param[in] coefficients the model coefficients (x, y, radius)
+          * \param[in] id the circle id/name (default: "circle")
+          *
+          * \code
+          * // The following are given (or computed using sample consensus techniques)
+          * // See SampleConsensusModelCircle2D for more information
+          * // float x, y, radius;
+          *
+          * temp_viz::ModelCoefficients circle_coeff;
+          * circle_coeff.values.resize (3);    // We need 3 values
+          * circle_coeff.values[0] = x;
+          * circle_coeff.values[1] = y;
+          * circle_coeff.values[2] = radius;
+          *
+          * vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
+          * \endcode
+           */
+    bool addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "circle");
+
+    /** \brief Add a cube from a set of given model coefficients
+          * \param[in] coefficients the model coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
+          * \param[in] id the cube id/name (default: "cube")
+          */
+    bool addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cube");
+
+    /** \brief Add a cube from a set of given model coefficients
+          * \param[in] translation a translation to apply to the cube from 0,0,0
+          * \param[in] rotation a quaternion-based rotation to apply to the cube
+          * \param[in] width the cube's width
+          * \param[in] height the cube's height
+          * \param[in] depth the cube's depth
+          * \param[in] id the cube id/name (default: "cube")
+          */
+    bool addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id = "cube");
+
+    /** \brief Add a cube
+          * \param[in] x_min the min X coordinate
+          * \param[in] x_max the max X coordinate
+          * \param[in] y_min the min Y coordinate
+          * \param[in] y_max the max Y coordinate
+          * \param[in] z_min the min Z coordinate
+          * \param[in] z_max the max Z coordinate
+          * \param[in] r how much red (0.0 -> 1.0)
+          * \param[in] g how much green (0.0 -> 1.0)
+          * \param[in] b how much blue (0.0 -> 1.0)
+          * \param[in] id the cube id/name (default: "cube")
+          */
+    bool addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max, const Color& color, const std::string &id = "cube");
+
+    /** \brief Changes the visual representation for all actors to surface representation. */
+    void setRepresentationToSurfaceForAllActors ();
+
+    /** \brief Changes the visual representation for all actors to points representation. */
+    void setRepresentationToPointsForAllActors ();
+
+    /** \brief Changes the visual representation for all actors to wireframe representation. */
+    void setRepresentationToWireframeForAllActors ();
+
+    /** \brief Initialize camera parameters with some default values. */
+    void initCameraParameters ();
+
+    /** \brief Search for camera parameters at the command line and set them internally.
+        bool getCameraParameters (int argc, char **argv);
+
+        /** \brief Checks whether the camera parameters were manually loaded from file.*/
+    bool cameraParamsSet () const;
+
+    /** \brief Update camera parameters and render. */
+    void updateCamera ();
+
+    /** \brief Reset camera parameters and render. */
+    void resetCamera ();
+
+    /** \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");
+
+    /** \brief Set the camera pose given by position, viewpoint and up vector
+          * \param[in] pos_x the x coordinate of the camera location
+          * \param[in] pos_y the y coordinate of the camera location
+          * \param[in] pos_z the z coordinate of the camera location
+          * \param[in] view_x the x component of the view point of the camera
+          * \param[in] view_y the y component of the view point of the camera
+          * \param[in] view_z the z component of the view point of the camera
+          * \param[in] up_x the x component of the view up direction of the camera
+          * \param[in] up_y the y component of the view up direction of the camera
+          * \param[in] up_z the y component of the view up direction of the camera
+          */
+    void setCameraPosition (const cv::Vec3d& pos, const cv::Vec3d& view, const cv::Vec3d& up);
+
+    /** \brief Set the camera location and viewup according to the given arguments
+          * \param[in] pos_x the x coordinate of the camera location
+          * \param[in] pos_y the y coordinate of the camera location
+          * \param[in] pos_z the z coordinate of the camera location
+          * \param[in] up_x the x component of the view up direction of the camera
+          * \param[in] up_y the y component of the view up direction of the camera
+          * \param[in] up_z the z component of the view up direction of the camera
+          */
+    void setCameraPosition (double pos_x, double pos_y, double pos_z, double up_x, double up_y, double up_z);
+
+    /** \brief Set the camera parameters via an intrinsics and and extrinsics matrix
+          * \note This assumes that the pixels are square and that the center of the image is at the center of the sensor.
+          * \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);
+
+    /** \brief Set the camera parameters by given a full camera data structure.
+          * \param[in] camera camera structure containing all the camera parameters.
+          */
+    void setCameraParameters (const Camera &camera);
+
+    /** \brief Set the camera clipping distances.
+          * \param[in] near the near clipping distance (no objects closer than this to the camera will be drawn)
+          * \param[in] far the far clipping distance (no objects further away than this to the camera will be drawn)
+          */
+    void setCameraClipDistances (double near, double far);
+
+    /** \brief Set the camera vertical field of view in radians */
+    void setCameraFieldOfView (double fovy);
+
+    /** \brief Get the current camera parameters. */
+    void getCameras (Camera& camera);
+
+    /** \brief Get the current viewing pose. */
+    cv::Affine3f getViewerPose ();
+    void saveScreenshot (const std::string &file);
+
+    /** \brief Return a pointer to the underlying VTK Render Window used. */
+    //vtkSmartPointer<vtkRenderWindow> getRenderWindow () { return (window_); }
+
+    void setPosition (int x, int y);
+    void setSize (int xw, int yw);
+
+private:
+    vtkSmartPointer<vtkRenderWindowInteractor> interactor_;
+
+    struct ExitMainLoopTimerCallback : public vtkCommand
+    {
+        static ExitMainLoopTimerCallback* New()
+        {
+            return new ExitMainLoopTimerCallback;
+        }
+        virtual void Execute(vtkObject* vtkNotUsed(caller), unsigned long event_id, void* call_data)
+        {
+            if (event_id != vtkCommand::TimerEvent)
+                return;
+
+            int timer_id = *reinterpret_cast<int*> (call_data);
+            if (timer_id != right_timer_id)
+                return;
+
+            // Stop vtk loop and send notification to app to wake it up
+            viz_->interactor_->TerminateApp ();
+        }
+        int right_timer_id;
+        VizImpl* viz_;
+    };
+
+    struct ExitCallback : public vtkCommand
+    {
+        static ExitCallback* New ()
+        {
+            return new ExitCallback;
+        }
+        virtual void Execute (vtkObject*, unsigned long event_id, void*)
+        {
+            if (event_id == vtkCommand::ExitEvent)
+            {
+                viz_->stopped_ = true;
+                viz_->interactor_->TerminateApp ();
+            }
+        }
+        VizImpl* viz_;
+    };
+
+    /** \brief Set to false if the interaction loop is running. */
+    bool stopped_;
+
+    double s_lastDone_;
+
+    /** \brief Global timer ID. Used in destructor only. */
+    int timer_id_;
+
+    /** \brief Callback object enabling us to leave the main loop, when a timer fires. */
+    vtkSmartPointer<ExitMainLoopTimerCallback> exit_main_loop_timer_callback_;
+    vtkSmartPointer<ExitCallback> exit_callback_;
+
+    vtkSmartPointer<vtkRenderer> renderer_;
+    vtkSmartPointer<vtkRenderWindow> window_;
+
+    /** \brief The render window interactor style. */
+    vtkSmartPointer<InteractorStyle> style_;
+
+    /** \brief Internal list with actor pointers and name IDs for point clouds. */
+    cv::Ptr<CloudActorMap> cloud_actor_map_;
+
+    /** \brief Internal list with actor pointers and name IDs for shapes. */
+    cv::Ptr<ShapeActorMap> shape_actor_map_;
+
+    /** \brief Boolean that holds whether or not the camera parameters were manually initialized*/
+    bool camera_set_;
+
+    bool removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor);
+    bool removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor);
+    bool removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor);
+
+    //void addActorToRenderer (const vtkSmartPointer<vtkProp> &actor);
+
+
+    /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
+          * \param[in] data the vtk polydata object to create an actor for
+          * \param[out] actor the resultant vtk actor object
+          * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
+          */
+    void createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor, bool use_scalars = true);
+
+    /** \brief Updates a set of cells (vtkIdTypeArray) if the number of points in a cloud changes
+          * \param[out] cells the vtkIdTypeArray object (set of cells) to update
+          * \param[out] initcells a previously saved set of cells. If the number of points in the current cloud is
+          * higher than the number of cells in \a cells, and initcells contains enough data, then a copy from it
+          * will be made instead of regenerating the entire array.
+          * \param[in] nr_points the number of points in the new cloud. This dictates how many cells we need to
+          * generate
+          */
+    void updateCells (vtkSmartPointer<vtkIdTypeArray> &cells, vtkSmartPointer<vtkIdTypeArray> &initcells, vtkIdType nr_points);
+
+    void allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata);
+    void allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata);
+    void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
+
+};
+
+//void getTransformationMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternionf& orientation, Eigen::Matrix4f &transformation);
+
+//void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+
+void convertToVtkMatrix (const cv::Matx44f& m, 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
+      * \param[out] vtk_matrix the resultant VTK 4x4 matrix
+      */
+void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+void convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m);
+
+}
+
+
+

modules/viz/src/q/viz_types.h

+#pragma once
+
+#include "precomp.hpp"
+
+namespace temp_viz
+{
+    struct CV_EXPORTS CloudActor
+    {
+        /** \brief The actor holding the data to render. */
+        vtkSmartPointer<vtkLODActor> actor;
+
+        /** \brief The viewpoint transformation matrix. */
+        vtkSmartPointer<vtkMatrix4x4> viewpoint_transformation_;
+
+        /** \brief Internal cell array. Used for optimizing updatePointCloud. */
+        vtkSmartPointer<vtkIdTypeArray> cells;
+    };
+
+    typedef std::map<std::string, CloudActor> CloudActorMap;
+    typedef std::map<std::string, vtkSmartPointer<vtkProp> > ShapeActorMap;
+}
+

modules/viz/src/shapes.cpp

+#include <q/shapes.h>
+
+inline float rad2deg (float alpha)
+{ return (alpha * 57.29578f); }
+
+inline double rad2deg (double alpha){return (alpha * 57.29578);}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides)
+{
+  vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
+  line->SetPoint1 (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+  line->SetPoint2 (coefficients.values[3]+coefficients.values[0], coefficients.values[4]+coefficients.values[1], coefficients.values[5]+coefficients.values[2]);
+
+  vtkSmartPointer<vtkTubeFilter> tuber = vtkSmartPointer<vtkTubeFilter>::New ();
+  tuber->SetInputConnection (line->GetOutputPort ());
+  tuber->SetRadius (coefficients.values[6]);
+  tuber->SetNumberOfSides (numsides);
+
+  return (tuber->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (const temp_viz::ModelCoefficients &coefficients)
+{
+  // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
+  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  t->Identity ();
+  t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+  Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
+                                           coefficients.values[4], coefficients.values[5]));
+  t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
+
+  vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+  cube->SetXLength (coefficients.values[7]);
+  cube->SetYLength (coefficients.values[8]);
+  cube->SetZLength (coefficients.values[9]);
+
+  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+  tf->SetTransform (t);
+  tf->SetInputConnection (cube->GetOutputPort ());
+
+  return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth)
+{
+  // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
+  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  t->Identity ();
+  t->Translate (translation.x (), translation.y (), translation.z ());
+
+  Eigen::AngleAxisf a (rotation);
+  t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
+
+  vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+  cube->SetXLength (width);
+  cube->SetYLength (height);
+  cube->SetZLength (depth);
+
+  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+  tf->SetTransform (t);
+  tf->SetInputConnection (cube->GetOutputPort ());
+
+  return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max)
+{
+  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+  cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max);
+  return (cube->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients)
+{
+  vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
+  plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+  double norm_sqr = coefficients.values[0] * coefficients.values[0]
+                  + coefficients.values[1] * coefficients.values[1]
+                  + coefficients.values[2] * coefficients.values[2];
+
+  plane->Push (-coefficients.values[3] / sqrt(norm_sqr));
+  return (plane->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z)
+{
+  vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
+
+
+  double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
+                           coefficients.values[1] * coefficients.values[1] +
+                           coefficients.values[2] * coefficients.values[2] );
+
+//  double nx = coefficients.values [0] * norm;
+//  double ny = coefficients.values [1] * norm;
+//  double nz = coefficients.values [2] * norm;
+//  double d  = coefficients.values [3] * norm;
+
+//  plane->SetNormal (nx, ny, nz);
+  plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+  double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
+  x -= coefficients.values[0] * t * norm_sqr;
+  y -= coefficients.values[1] * t * norm_sqr;
+  z -= coefficients.values[2] * t * norm_sqr;
+  plane->SetCenter (x, y, z);
+
+  return (plane->GetOutput ());
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z)
+{
+  vtkSmartPointer<vtkDiskSource> disk = vtkSmartPointer<vtkDiskSource>::New ();
+  // Maybe the resolution should be lower e.g. 50 or 25
+  disk->SetCircumferentialResolution (100);
+  disk->SetInnerRadius (coefficients.values[2] - 0.001);
+  disk->SetOuterRadius (coefficients.values[2] + 0.001);
+  disk->SetCircumferentialResolution (20);
+
+  // An alternative to <vtkDiskSource> could be <vtkRegularPolygonSource> with <vtkTubeFilter>
+  /*
+  vtkSmartPointer<vtkRegularPolygonSource> circle = vtkSmartPointer<vtkRegularPolygonSource>::New();
+  circle->SetRadius (coefficients.values[2]);
+  circle->SetNumberOfSides (100);
+
+  vtkSmartPointer<vtkTubeFilter> tube = vtkSmartPointer<vtkTubeFilter>::New();
+  tube->SetInput (circle->GetOutput());
+  tube->SetNumberOfSides (25);
+  tube->SetRadius (0.001);
+  */
+
+  // Set the circle origin
+  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  t->Identity ();
+  t->Translate (coefficients.values[0], coefficients.values[1], z);
+
+  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+  tf->SetTransform (t);
+  tf->SetInputConnection (disk->GetOutputPort ());
+  /*
+  tf->SetInputConnection (tube->GetOutputPort ());
+  */
+
+  return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution)
+{
+  // Set the sphere origin
+  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+  t->Identity ();
+  t->Translate (center.x, center.y, center.z);
+
+  vtkSmartPointer<vtkSphereSource> s_sphere = vtkSmartPointer<vtkSphereSource>::New ();
+  s_sphere->SetRadius (radius);
+  s_sphere->SetPhiResolution (sphere_resolution);
+  s_sphere->SetThetaResolution (sphere_resolution);
+  s_sphere->LatLongTessellationOff ();
+
+  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+  tf->SetTransform (t);
+  tf->SetInputConnection (s_sphere->GetOutputPort ());
+  tf->Update ();
+
+  return (tf->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 ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
+{
+  polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+}
+
+

modules/viz/src/types.cpp

+#include <opencv2/viz/types.hpp>
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Color
+
+temp_viz::Color::Color() : Scalar(0, 0, 0) {}
+temp_viz::Color::Color(double gray) : Scalar(gray, gray, gray) {}
+temp_viz::Color::Color(double blue, double green, double red) : Scalar(blue, green, red) {}
+temp_viz::Color::Color(const Scalar& color) : Scalar(color) {}
+
+temp_viz::Color temp_viz::Color::black()   { return Color(  0,   0, 0); }
+temp_viz::Color temp_viz::Color::green()   { return Color(  0, 255, 0); }
+temp_viz::Color temp_viz::Color::blue()    { return Color(255,   0, 0); }
+temp_viz::Color temp_viz::Color::cyan()    { return Color(255, 255, 0); }
+
+temp_viz::Color temp_viz::Color::red()     { return Color(  0,   0, 255); }
+temp_viz::Color temp_viz::Color::magenta() { return Color(  0, 255, 255); }
+temp_viz::Color temp_viz::Color::yellow()  { return Color(255,   0, 255); }
+temp_viz::Color temp_viz::Color::white()   { return Color(255, 255, 255); }
+
+temp_viz::Color temp_viz::Color::gray()    { return Color(128, 128, 128); }
+

modules/viz/src/viz.cpp

+#include "precomp.hpp"
\ No newline at end of file

modules/viz/src/viz3d.cpp

+#include <opencv2/viz/viz3d.hpp>
+#include <q/viz3d_impl.hpp>
+
+
+temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_name))
+{
+
+}
+
+temp_viz::Viz3d::~Viz3d()
+{
+
+}
+
+
+void temp_viz::Viz3d::setBackgroundColor(const Color& color)
+{
+    impl_->setBackgroundColor(color);
+}
+
+void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String &id)
+{
+    impl_->addCoordinateSystem(scale, t, id);
+}
+
+void temp_viz::Viz3d::addPointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask)
+{
+    impl_->addPointCloud(cloud, colors, id, mask);
+}
+
+bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals, int level, float scale, const String& id)
+{
+    return impl_->addPointCloudNormals(cloud, normals, level, scale, id);
+}
+
+bool temp_viz::Viz3d::updatePointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask)
+{
+    return impl_->updatePointCloud(cloud, colors, id, mask);
+}
+
+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)
+{
+    return impl_->updatePolygonMesh(mesh, Mat(), 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)
+{
+    return impl_->addText(text, xpos, ypos, color, fontsize, id);
+}
+
+bool temp_viz::Viz3d::addPolygon(const Mat& cloud, const Color& color, const String& id)
+{
+    return impl_->addPolygon(cloud, color, id);
+}
+
+bool temp_viz::Viz3d::addSphere (const cv::Point3f &center, double radius, const Color& color, const std::string &id)
+{
+    return impl_->addSphere(center, radius, color, id);
+}
+
+void temp_viz::Viz3d::spin()
+{
+    impl_->spin();
+}
+
+void temp_viz::Viz3d::spinOnce (int time, bool force_redraw)
+{
+    impl_->spinOnce(time, force_redraw);
+}
+
+bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, const String &id)
+{
+    return impl_->addPlane(coefficients, id);
+}
+
+bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id)
+{
+    return impl_->addPlane(coefficients, x, y, z, id);
+}
+
+bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)
+{
+    return impl_->removeCoordinateSystem(id);
+}

modules/viz/src/viz3d_impl.cpp

+#include <opencv2/core.hpp>
+#include <q/shapes.h>
+
+#include <vtkCellData.h>
+#include <vtkSmartPointer.h>
+#include <vtkCellArray.h>
+#include <vtkProperty2D.h>
+#include <vtkMapper2D.h>
+#include <vtkLeaderActor2D.h>
+#include <q/shapes.h>
+#include <vtkAlgorithmOutput.h>
+
+#include <q/viz3d_impl.hpp>
+
+void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
+{
+    if (window_)
+        window_->SetFullScreen (mode);
+}
+
+void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
+{
+    if (window_)
+        window_->SetWindowName (name.c_str ());
+}
+
+void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
+void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
+
+void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask)
+{
+    CV_Assert(cloud.type() == CV_32FC3 && colors.type() == CV_8UC3 && colors.size() == cloud.size());
+    CV_Assert(mask.empty() || (mask.type() == CV_8U && mask.size() == cloud.size()));
+
+    vtkSmartPointer<vtkPolyData> polydata;
+
+    allocVtkPolyData(polydata);
+    //polydata = vtkSmartPointer<vtkPolyData>::New ();
+    vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
+    polydata->SetVerts (vertices);
+
+    vtkSmartPointer<vtkIdTypeArray> initcells;
+    vtkIdType nr_points = cloud.size().area();
+    vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
+
+    if (!points)
+    {
+        points = vtkSmartPointer<vtkPoints>::New ();
+        points->SetDataTypeToFloat ();
+        polydata->SetPoints (points);
+    }
+    points->SetNumberOfPoints (nr_points);
+
+    // Get a pointer to the beginning of the data array
+    float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+    if (mask.empty())
+    {
+        int j = 0;
+        for(int y = 0; y < cloud.rows; ++y)
+        {
+            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+            for(int x = 0; x < cloud.cols; ++x)
+                memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+        }
+    }
+    else
+    {
+        int j = 0;
+        for(int y = 0; y < cloud.rows; ++y)
+        {
+            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+            const unsigned char* mrow = mask.ptr<unsigned char>(y);
+            for(int x = 0; x < cloud.cols; ++x)
+                if (mrow[x])
+                    memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+        }
+        nr_points = j;
+        points->SetNumberOfPoints (nr_points);
+    }
+
+    vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
+    updateCells (cells, initcells, nr_points);
+
+    // Set the cells and the vertices
+    vertices->SetCells (nr_points, cells);
+
+    /////////////////////////////////////////////////////////////////////////////////
+
+    // use the given geometry handler
+    polydata->Update ();
+
+    // Get the colors from the handler
+    bool has_colors = false;
+    double minmax[2];
+    vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+    scalars->SetNumberOfComponents (3);
+    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+
+    // Get a random color
+    unsigned char* colors_data = new unsigned char[nr_points * 3];
+
+    if (mask.empty())
+    {
+        int j = 0;
+        for(int y = 0; y < colors.rows; ++y)
+        {
+            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+            for(int x = 0; x < colors.cols; ++x)
+                memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+        }
+    }
+    else
+    {
+        int j = 0;
+        for(int y = 0; y < colors.rows; ++y)
+        {
+            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+            const unsigned char* mrow = mask.ptr<unsigned char>(y);
+            for(int x = 0; x < colors.cols; ++x)
+                if (mrow[x])
+                    memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+        }
+    }
+
+    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
+
+    /////////////////////////////////////////
+    has_colors = true;
+
+    if (has_colors)
+    {
+        polydata->GetPointData ()->SetScalars (scalars);
+        scalars->GetRange (minmax);
+    }
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (polydata, actor);
+    if (has_colors)
+        actor->GetMapper ()->SetScalarRange (minmax);
+
+    // Add it to all renderers
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*cloud_actor_map_)[id].actor = actor;
+    (*cloud_actor_map_)[id].cells = initcells;
+
+    const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
+    const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+
+    // Save the viewpoint transformation matrix to the global actor map
+    vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
+    convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
+    (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
+}
+
+
+bool temp_viz::Viz3d::VizImpl::updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return (false);
+
+    // Get the current poly data
+    vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
+    if (!polydata)
+        return (false);
+    vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
+    vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
+    // Copy the new point array in
+    vtkIdType nr_points = cloud.size().area();
+    points->SetNumberOfPoints (nr_points);
+
+    // Get a pointer to the beginning of the data array
+    float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+    if (mask.empty())
+    {
+        int j = 0;
+        for(int y = 0; y < cloud.rows; ++y)
+        {
+            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+            for(int x = 0; x < cloud.cols; ++x)
+                memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+        }
+    }
+    else
+    {
+        int j = 0;
+        for(int y = 0; y < cloud.rows; ++y)
+        {
+            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+            const unsigned char* mrow = mask.ptr<unsigned char>(y);
+            for(int x = 0; x < cloud.cols; ++x)
+                if (mrow[x])
+                    memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+        }
+        nr_points = j;
+        points->SetNumberOfPoints (nr_points);
+    }
+
+    vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
+    updateCells (cells, am_it->second.cells, nr_points);
+
+
+    // Set the cells and the vertices
+    vertices->SetCells (nr_points, cells);
+
+#if 1
+    // Get the colors from the handler
+    //  vtkSmartPointer<vtkDataArray> scalars;
+    //  color_handler.getColor (scalars);
+    //  double minmax[2];
+    //  scalars->GetRange (minmax);
+
+    //  // Update the data
+    //  polydata->GetPointData ()->SetScalars (scalars);
+    //  polydata->Update ();
+
+    //  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
+    //  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
+
+
+    ////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    // Get the colors from the handler
+    bool has_colors = false;
+    double minmax[2];
+    vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+    scalars->SetNumberOfComponents (3);
+    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+
+    // Get a random color
+    unsigned char* colors_data = new unsigned char[nr_points * 3];
+
+    if (mask.empty())
+    {
+        int j = 0;
+        for(int y = 0; y < colors.rows; ++y)
+        {
+            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+            for(int x = 0; x < colors.cols; ++x)
+                memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+        }
+    }
+    else
+    {
+        int j = 0;
+        for(int y = 0; y < colors.rows; ++y)
+        {
+            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+            const unsigned char* mrow = mask.ptr<unsigned char>(y);
+            for(int x = 0; x < colors.cols; ++x)
+                if (mrow[x])
+                    memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+        }
+    }
+
+    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
+
+    /////////////////////////////////////////
+    has_colors = true;
+
+    if (has_colors)
+    {
+        polydata->GetPointData ()->SetScalars (scalars);
+        scalars->GetRange (minmax);
+    }
+
+#else
+    vtkSmartPointer<vtkDataArray> scalars;
+    polydata->GetPointData ()->SetScalars (scalars);
+    polydata->Update ();
+    double minmax[2];
+    minmax[0] = std::numeric_limits<double>::min ();
+    minmax[1] = std::numeric_limits<double>::max ();
+    am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
+    am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
+#endif
+
+
+    // Update the mapper
+    reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
+    return (true);
+}
+
+
+
+bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level, float scale, const std::string &id)
+{
+    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);
+
+    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
+    vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
+
+    points->SetDataTypeToFloat ();
+    vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
+    data->SetNumberOfComponents (3);
+
+    vtkIdType nr_normals = 0;
+    float* pts = 0;
+
+    // If the cloud is organized, then distribute the normal step in both directions
+    if (cloud.cols > 1 && cloud.rows > 1)
+    {
+        vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
+        nr_normals = (static_cast<vtkIdType> ((cloud.cols - 1)/ point_step) + 1) *
+                (static_cast<vtkIdType> ((cloud.rows - 1) / point_step) + 1);
+        pts = new float[2 * nr_normals * 3];
+
+        vtkIdType cell_count = 0;
+        for (vtkIdType y = 0; y < cloud.rows; y += point_step)
+            for (vtkIdType x = 0; x < cloud.cols; x += point_step)
+            {
+                cv::Point3f p = cloud.at<cv::Point3f>(y, x);
+                cv::Point3f n = normals.at<cv::Point3f>(y, x) * scale;
+
+                pts[2 * cell_count * 3 + 0] = p.x;
+                pts[2 * cell_count * 3 + 1] = p.y;
+                pts[2 * cell_count * 3 + 2] = p.z;
+                pts[2 * cell_count * 3 + 3] = p.x + n.x;
+                pts[2 * cell_count * 3 + 4] = p.y + n.y;
+                pts[2 * cell_count * 3 + 5] = p.z + n.z;
+
+                lines->InsertNextCell (2);
+                lines->InsertCellPoint (2 * cell_count);
+                lines->InsertCellPoint (2 * cell_count + 1);
+                cell_count++;
+            }
+    }
+    else
+    {
+        nr_normals = (cloud.size().area() - 1) / level + 1 ;
+        pts = new float[2 * nr_normals * 3];
+
+        for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
+        {
+            cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
+            cv::Point3f n = normals.ptr<cv::Point3f>()[i] * scale;
+
+            pts[2 * j * 3 + 0] = p.x;
+            pts[2 * j * 3 + 1] = p.y;
+            pts[2 * j * 3 + 2] = p.z;
+            pts[2 * j * 3 + 3] = p.x + n.x;
+            pts[2 * j * 3 + 4] = p.y + n.y;
+            pts[2 * j * 3 + 5] = p.z + n.z;
+
+            lines->InsertNextCell (2);
+            lines->InsertCellPoint (2 * j);
+            lines->InsertCellPoint (2 * j + 1);
+        }
+    }
+
+    data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
+    points->SetData (data);
+
+    vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
+    polyData->SetPoints (points);
+    polyData->SetLines (lines);
+
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetInput (polyData);
+    mapper->SetColorModeToMapScalars();
+    mapper->SetScalarModeToUsePointData();
+
+    // create actor
+    vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+    actor->SetMapper (mapper);
+
+    // Add it to all renderers
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*cloud_actor_map_)[id].actor = actor;
+    return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addLine] A shape with id <" << id <<  "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    vtkSmartPointer<vtkDataSet> data = createLine (pt1, pt2);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+
+    Color c = vtkcolor(color);
+    actor->GetProperty ()->SetColor (c.val);
+    actor->GetMapper ()->ScalarVisibilityOff ();
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+
+
+inline bool temp_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));
+    CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
+
+    if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
+        return std::cout << "[addPolygonMesh] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    //    int rgb_idx = -1;
+    //    std::vector<sensor_msgs::PointField> fields;
+
+
+    //    rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields);
+    //    if (rgb_idx == -1)
+    //      rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields);
+
+    vtkSmartPointer<vtkUnsignedCharArray> colors_array;
+#if 1
+    if (!mesh.colors.empty())
+    {
+        colors_array = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+        colors_array->SetNumberOfComponents (3);
+        colors_array->SetName ("Colors");
+
+        const unsigned char* data = mesh.colors.ptr<unsigned char>();
+
+        //TODO check mask
+        CV_Assert(mask.empty()); //because not implemented;
+
+        for(int i = 0; i < mesh.colors.cols; ++i)
+            colors_array->InsertNextTupleValue(&data[i*3]);
+
+        //      temp_viz::RGB rgb_data;
+        //      for (size_t i = 0; i < cloud->size (); ++i)
+        //      {
+        //        if (!isFinite (cloud->points[i]))
+        //          continue;
+        //        memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (temp_viz::RGB));
+        //        unsigned char color[3];
+        //        color[0] = rgb_data.r;
+        //        color[1] = rgb_data.g;
+        //        color[2] = rgb_data.b;
+        //        colors->InsertNextTupleValue (color);
+        //      }
+    }
+#endif
+
+    // Create points from polyMesh.cloud
+    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+    vtkIdType nr_points = mesh.cloud.size().area();
+
+    points->SetNumberOfPoints (nr_points);
+
+
+    // Get a pointer to the beginning of the data array
+    float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
+
+
+    std::vector<int> lookup;
+    // If the dataset is dense (no NaNs)
+    if (mask.empty())
+    {
+        cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
+        mesh.cloud.copyTo(hdr);
+    }
+    else
+    {
+        lookup.resize (nr_points);
+
+        const unsigned char *mdata = mask.ptr<unsigned char>();
+        const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+        cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
+
+        int j = 0;    // true point index
+        for (int i = 0; i < nr_points; ++i)
+            if(mdata[i])
+            {
+                lookup[i] = j;
+                out[j++] = cdata[i];
+            }
+        nr_points = j;
+        points->SetNumberOfPoints (nr_points);
+    }
+
+    // Get the maximum size of a polygon
+    int max_size_of_polygon = -1;
+    for (size_t i = 0; i < mesh.polygons.size (); ++i)
+        if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
+            max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
+
+    vtkSmartPointer<vtkLODActor> actor;
+
+    if (mesh.polygons.size () > 1)
+    {
+        // Create polys from polyMesh.polygons
+        vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
+        vtkIdType *cell = cell_array->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
+        int idx = 0;
+        if (lookup.size () > 0)
+        {
+            for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+            {
+                size_t n_points = mesh.polygons[i].vertices.size ();
+                *cell++ = n_points;
+                //cell_array->InsertNextCell (n_points);
+                for (size_t j = 0; j < n_points; j++, ++idx)
+                    *cell++ = lookup[mesh.polygons[i].vertices[j]];
+                //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
+            }
+        }
+        else
+        {
+            for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+            {
+                size_t n_points = mesh.polygons[i].vertices.size ();
+                *cell++ = n_points;
+                //cell_array->InsertNextCell (n_points);
+                for (size_t j = 0; j < n_points; j++, ++idx)
+                    *cell++ = mesh.polygons[i].vertices[j];
+                //cell_array->InsertCellPoint (vertices[i].vertices[j]);
+            }
+        }
+        vtkSmartPointer<vtkPolyData> polydata;
+        allocVtkPolyData (polydata);
+        cell_array->GetData ()->SetNumberOfValues (idx);
+        cell_array->Squeeze ();
+        polydata->SetStrips (cell_array);
+        polydata->SetPoints (points);
+
+        if (colors_array)
+            polydata->GetPointData ()->SetScalars (colors_array);
+
+        createActorFromVTKDataSet (polydata, actor, false);
+    }
+    else
+    {
+        vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
+        size_t n_points = mesh.polygons[0].vertices.size ();
+        polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
+
+        if (lookup.size () > 0)
+        {
+            for (size_t j = 0; j < n_points - 1; ++j)
+                polygon->GetPointIds ()->SetId (j, lookup[mesh.polygons[0].vertices[j]]);
+        }
+        else
+        {
+            for (size_t j = 0; j < n_points - 1; ++j)
+                polygon->GetPointIds ()->SetId (j, mesh.polygons[0].vertices[j]);
+        }
+        vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
+        allocVtkUnstructuredGrid (poly_grid);
+        poly_grid->Allocate (1, 1);
+        poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
+        poly_grid->SetPoints (points);
+        poly_grid->Update ();
+        if (colors_array)
+            poly_grid->GetPointData ()->SetScalars (colors_array);
+
+        createActorFromVTKDataSet (poly_grid, actor, false);
+    }
+    renderer_->AddActor (actor);
+    actor->GetProperty ()->SetRepresentationToSurface ();
+    // Backface culling renders the visualization slower, but guarantees that we see all triangles
+    actor->GetProperty ()->BackfaceCullingOff ();
+    actor->GetProperty ()->SetInterpolationToFlat ();
+    actor->GetProperty ()->EdgeVisibilityOff ();
+    actor->GetProperty ()->ShadingOff ();
+
+    // Save the pointer/ID pair to the global actor map
+    (*cloud_actor_map_)[id].actor = actor;
+    //if (vertices.size () > 1)
+    //  (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
+
+    const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
+    const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+
+    // Save the viewpoint transformation matrix to the global actor map
+    vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
+    convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
+    (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
+
+    return (true);
+}
+
+
+inline bool temp_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));
+    CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
+
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return (false);
+
+    // Get the current poly data
+    vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
+    if (!polydata)
+        return (false);
+    vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
+    if (!cells)
+        return (false);
+    vtkSmartPointer<vtkPoints> points   = polydata->GetPoints ();
+    // Copy the new point array in
+    vtkIdType nr_points = mesh.cloud.size().area();
+    points->SetNumberOfPoints (nr_points);
+
+    // Get a pointer to the beginning of the data array
+    float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+    int ptr = 0;
+    std::vector<int> lookup;
+    // If the dataset is dense (no NaNs)
+    if (mask.empty())
+    {
+        cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
+        mesh.cloud.copyTo(hdr);
+
+    }
+    else
+    {
+        lookup.resize (nr_points);
+
+        const unsigned char *mdata = mask.ptr<unsigned char>();
+        const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+        cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
+
+        int j = 0;    // true point index
+        for (int i = 0; i < nr_points; ++i)
+            if(mdata[i])
+            {
+                lookup[i] = j;
+                out[j++] = cdata[i];
+            }
+        nr_points = j;
+        points->SetNumberOfPoints (nr_points);;
+    }
+
+    // Update colors
+    vtkUnsignedCharArray* colors_array = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
+
+    if (!mesh.colors.empty() && colors_array)
+    {
+        if (mask.empty())
+        {
+            const unsigned char* data = mesh.colors.ptr<unsigned char>();
+            for(int i = 0; i < mesh.colors.cols; ++i)
+                colors_array->InsertNextTupleValue(&data[i*3]);
+        }
+        else
+        {
+            const unsigned char* color = mesh.colors.ptr<unsigned char>();
+            const unsigned char* mdata = mask.ptr<unsigned char>();
+
+            int j = 0;
+            for(int i = 0; i < mesh.colors.cols; ++i)
+                if (mdata[i])
+                    colors_array->SetTupleValue (j++, &color[i*3]);
+
+        }
+    }
+
+    // Get the maximum size of a polygon
+    int max_size_of_polygon = -1;
+    for (size_t i = 0; i < mesh.polygons.size (); ++i)
+        if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
+            max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
+
+    // Update the cells
+    cells = vtkSmartPointer<vtkCellArray>::New ();
+    vtkIdType *cell = cells->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
+    int idx = 0;
+    if (lookup.size () > 0)
+    {
+        for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+        {
+            size_t n_points = mesh.polygons[i].vertices.size ();
+            *cell++ = n_points;
+            for (size_t j = 0; j < n_points; j++, cell++, ++idx)
+                *cell = lookup[mesh.polygons[i].vertices[j]];
+        }
+    }
+    else
+    {
+        for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+        {
+            size_t n_points = mesh.polygons[i].vertices.size ();
+            *cell++ = n_points;
+            for (size_t j = 0; j < n_points; j++, cell++, ++idx)
+                *cell = mesh.polygons[i].vertices[j];
+        }
+    }
+    cells->GetData ()->SetNumberOfValues (idx);
+    cells->Squeeze ();
+    // Set the the vertices
+    polydata->SetStrips (cells);
+    polydata->Update ();
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color, bool display_length, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    // Create an Actor
+    vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
+    leader->GetPositionCoordinate()->SetCoordinateSystemToWorld ();
+    leader->GetPositionCoordinate()->SetValue (p1.x, p1.y, p1.z);
+    leader->GetPosition2Coordinate()->SetCoordinateSystemToWorld ();
+    leader->GetPosition2Coordinate()->SetValue (p2.x, p2.y, p2.z);
+    leader->SetArrowStyleToFilled();
+    leader->SetArrowPlacementToPoint2 ();
+
+    if (display_length)
+        leader->AutoLabelOn ();
+    else
+        leader->AutoLabelOff ();
+
+    Color c = vtkcolor(color);
+    leader->GetProperty ()->SetColor (c.val);
+    renderer_->AddActor (leader);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = leader;
+    return (true);
+}
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color_line, const Color& color_text, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    // Create an Actor
+    vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
+    leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
+    leader->GetPositionCoordinate ()->SetValue (p1.x, p1.y, p1.z);
+    leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
+    leader->GetPosition2Coordinate ()->SetValue (p2.x, p2.y, p2.z);
+    leader->SetArrowStyleToFilled ();
+    leader->AutoLabelOn ();
+
+    Color ct = vtkcolor(color_text);
+    leader->GetLabelTextProperty()->SetColor(ct.val);
+
+    Color cl = vtkcolor(color_line);
+    leader->GetProperty ()->SetColor (cl.val);
+    renderer_->AddActor (leader);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = leader;
+    return (true);
+}
+
+#include <vtkSphereSource.h>
+////////////////////////////////////////////////////////////////////////////////////////////
+inline bool temp_viz::Viz3d::VizImpl::addSphere (const cv::Point3f& center, float radius, const Color& color, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addSphere] A shape with id <"<<id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    //vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
+    vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New ();
+    data->SetRadius (radius);
+    data->SetCenter (center.x, center.y, center.z);
+    data->SetPhiResolution (10);
+    data->SetThetaResolution (10);
+    data->LatLongTessellationOff ();
+    data->Update ();
+
+    // Setup actor and mapper
+    vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+    mapper->SetInputConnection (data->GetOutputPort ());
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+    actor->SetMapper (mapper);
+    //createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToSurface ();
+    actor->GetProperty ()->SetInterpolationToFlat ();
+
+    Color c = vtkcolor(color);
+    actor->GetProperty ()->SetColor (c.val);
+    actor->GetMapper ()->ImmediateModeRenderingOn ();
+    actor->GetMapper ()->StaticOn ();
+    actor->GetMapper ()->ScalarVisibilityOff ();
+    actor->GetMapper ()->Update ();
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+inline bool temp_viz::Viz3d::VizImpl::updateSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it == shape_actor_map_->end ())
+        return (false);
+
+    //////////////////////////////////////////////////////////////////////////
+    // Get the actor pointer
+    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
+    vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
+    vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
+
+    src->SetCenter(center.x, center.y, center.z);
+    src->SetRadius(radius);
+    src->Update ();
+    Color c = vtkcolor(color);
+    actor->GetProperty ()->SetColor (c.val);
+    actor->Modified ();
+
+    return (true);
+}
+
+//////////////////////////////////////////////////
+inline bool temp_viz::Viz3d::VizImpl::addText3D (const std::string &text, const cv::Point3f& position, const Color& color, double textScale, const std::string &id)
+{
+    std::string tid;
+    if (id.empty ())
+        tid = text;
+    else
+        tid = id;
+
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addText3d] A text with id <" << tid << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
+    textSource->SetText (text.c_str());
+    textSource->Update ();
+
+    vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+    textMapper->SetInputConnection (textSource->GetOutputPort ());
+
+    // Since each follower may follow a different camera, we need different followers
+    vtkRenderer* renderer = renderer_;
+
+    vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
+    textActor->SetMapper (textMapper);
+    textActor->SetPosition (position.x, position.y, position.z);
+    textActor->SetScale (textScale);
+
+    Color c = vtkcolor(color);
+    textActor->GetProperty ()->SetColor (c.val);
+    textActor->SetCamera (renderer->GetActiveCamera ());
+
+    renderer->AddActor (textActor);
+    renderer->Render ();
+
+    // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
+    // for multiple viewport
+    (*shape_actor_map_)[tid] = textActor;
+
+
+    return (true);
+}
+
+inline bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
+{
+    CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);
+
+    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+    vtkSmartPointer<vtkPolygon> polygon    = vtkSmartPointer<vtkPolygon>::New ();
+
+    int total = cloud.size().area();
+    points->SetNumberOfPoints (total);
+    polygon->GetPointIds ()->SetNumberOfIds (total);
+
+    for (int i = 0; i < total; ++i)
+    {
+        cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
+        points->SetPoint (i, p.x, p.y, p.z);
+        polygon->GetPointIds ()->SetId (i, i);
+    }
+
+    vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
+    allocVtkUnstructuredGrid (poly_grid);
+    poly_grid->Allocate (1, 1);
+    poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
+    poly_grid->SetPoints (points);
+    poly_grid->Update ();
+
+
+    //////////////////////////////////////////////////////
+    vtkSmartPointer<vtkDataSet> data = poly_grid;
+
+    Color c = vtkcolor(color);
+
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
+
+        // Add old data
+        all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
+
+        // Add new data
+        vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
+        surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
+        vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
+        all_data->AddInput (poly_data);
+
+        // Create an Actor
+        vtkSmartPointer<vtkLODActor> actor;
+        createActorFromVTKDataSet (all_data->GetOutput (), actor);
+        actor->GetProperty ()->SetRepresentationToWireframe ();
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        actor->GetProperty ()->BackfaceCullingOff ();
+
+        removeActorFromRenderer (am_it->second);
+        renderer_->AddActor (actor);
+
+        // Save the pointer/ID pair to the global actor map
+        (*shape_actor_map_)[id] = actor;
+    }
+    else
+    {
+        // Create an Actor
+        vtkSmartPointer<vtkLODActor> actor;
+        createActorFromVTKDataSet (data, actor);
+        actor->GetProperty ()->SetRepresentationToWireframe ();
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        actor->GetProperty ()->BackfaceCullingOff ();
+        renderer_->AddActor (actor);
+
+        // Save the pointer/ID pair to the global actor map
+        (*shape_actor_map_)[id] = actor;
+    }
+
+    return (true);
+}

modules/viz/src/viz_main.cpp

+#include "precomp.hpp"
+
+#include <opencv2/calib3d.hpp>
+#include <q/shapes.h>
+#include <q/viz3d_impl.hpp>
+
+#include <vtkRenderWindowInteractor.h>
+#ifndef __APPLE__
+vtkRenderWindowInteractor* vtkRenderWindowInteractorFixNew ()
+{
+  return (vtkRenderWindowInteractor::New ());
+}
+#endif
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+temp_viz::Viz3d::VizImpl::VizImpl (const std::string &name)
+    :  style_ (vtkSmartPointer<temp_viz::InteractorStyle>::New ())
+    , cloud_actor_map_ (new CloudActorMap)
+    , shape_actor_map_ (new ShapeActorMap)
+    , s_lastDone_(0.0)
+{
+    renderer_ = vtkSmartPointer<vtkRenderer>::New ();
+
+    // Create a RendererWindow
+    window_ = vtkSmartPointer<vtkRenderWindow>::New ();
+
+    // Set the window size as 1/2 of the screen size
+    cv::Vec2i window_size = cv::Vec2i(window_->GetScreenSize()) / 2;
+    window_->SetSize (window_size.val);
+
+    window_->AddRenderer (renderer_);
+
+    // Create the interactor style
+    style_->Initialize ();
+    style_->setRenderer (renderer_);
+    style_->setCloudActorMap (cloud_actor_map_);
+    style_->UseTimersOn ();
+
+    /////////////////////////////////////////////////
+    interactor_ = vtkSmartPointer <vtkRenderWindowInteractor>::Take (vtkRenderWindowInteractorFixNew ());
+
+    //win_->PointSmoothingOn ();
+    //win_->LineSmoothingOn ();
+    //win_->PolygonSmoothingOn ();
+    window_->AlphaBitPlanesOff ();
+    window_->PointSmoothingOff ();
+    window_->LineSmoothingOff ();
+    window_->PolygonSmoothingOff ();
+    window_->SwapBuffersOn ();
+    window_->SetStereoTypeToAnaglyph ();
+
+    interactor_->SetRenderWindow (window_);
+    interactor_->SetInteractorStyle (style_);
+    //interactor_->SetStillUpdateRate (30.0);
+    interactor_->SetDesiredUpdateRate (30.0);
+
+    // Initialize and create timer, also create window
+    interactor_->Initialize ();
+    timer_id_ = interactor_->CreateRepeatingTimer (5000L);
+
+    // Set a simple PointPicker
+    vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
+    pp->SetTolerance (pp->GetTolerance () * 2);
+    interactor_->SetPicker (pp);
+
+    exit_main_loop_timer_callback_ = vtkSmartPointer<ExitMainLoopTimerCallback>::New ();
+    exit_main_loop_timer_callback_->viz_ = this;
+    exit_main_loop_timer_callback_->right_timer_id = -1;
+    interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
+
+    exit_callback_ = vtkSmartPointer<ExitCallback>::New ();
+    exit_callback_->viz_ = this;
+    interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_);
+
+    resetStoppedFlag ();
+
+
+    //////////////////////////////
+
+    String window_name("Viz");
+    window_name = name.empty() ? window_name : window_name + " - " + name;
+    window_->SetWindowName (window_name.c_str ());
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+temp_viz::Viz3d::VizImpl::~VizImpl ()
+{
+    if (interactor_ != NULL)
+        interactor_->DestroyTimer (timer_id_);
+
+    renderer_->Clear();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_->saveScreenshot (file); }
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::Viz3d::VizImpl::registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> callback)
+{
+    return (style_->registerKeyboardCallback (callback));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::Viz3d::VizImpl::registerMouseCallback (boost::function<void (const cv::MouseEvent&)> callback)
+{
+    return (style_->registerMouseCallback (callback));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::spin ()
+{
+    resetStoppedFlag ();
+    window_->Render ();
+    interactor_->Start ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::spinOnce (int time, bool force_redraw)
+{
+    resetStoppedFlag ();
+
+    if (time <= 0)
+        time = 1;
+
+    if (force_redraw)
+        interactor_->Render ();
+
+    double s_now_ = cv::getTickCount() / cv::getTickFrequency();
+    if (s_lastDone_ > s_now_)
+      s_lastDone_ = s_now_;
+
+    if ((s_now_ - s_lastDone_) > (1.0 / interactor_->GetDesiredUpdateRate ()))
+    {
+        exit_main_loop_timer_callback_->right_timer_id = interactor_->CreateRepeatingTimer (time);
+        interactor_->Start ();
+        interactor_->DestroyTimer (exit_main_loop_timer_callback_->right_timer_id);
+        s_lastDone_ = s_now_;
+    }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::addCoordinateSystem (double scale, const cv::Affine3f& affine, const std::string &id)
+{
+    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<vtkPolyDataMapper> axes_mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+    axes_mapper->SetScalarModeToUsePointData ();
+    axes_mapper->SetInput (axes_tubes->GetOutput ());
+
+    vtkSmartPointer<vtkLODActor> axes_actor = vtkSmartPointer<vtkLODActor>::New ();
+    axes_actor->SetMapper (axes_mapper);
+
+    cv::Vec3d t = affine.translation();
+    axes_actor->SetPosition (t[0], t[1], t[2]);
+
+    cv::Matx33f m = affine.rotation();
+
+    cv::Vec3f rvec;
+    cv::Rodrigues(m, rvec);
+
+    float r_angle = cv::norm(rvec);
+    rvec *= 1.f/r_angle;
+
+    axes_actor->SetOrientation(0,0,0);
+    axes_actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]);
+    renderer_->AddActor (axes_actor);
+
+    (*shape_actor_map_)[id] = axes_actor;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeCoordinateSystem (const std::string &id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it == shape_actor_map_->end ())
+        return false;
+
+    // Remove it from all renderers
+    if (!removeActorFromRenderer(am_it->second))
+        return false;
+
+    shape_actor_map_->erase(am_it);
+    return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removePointCloud (const std::string &id)
+{
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return false;
+
+    if (removeActorFromRenderer (am_it->second.actor))
+        return cloud_actor_map_->erase (am_it), true;
+
+    return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeShape (const std::string &id)
+{
+    // Check to see if the given ID entry exists
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    // Extra step: check if there is a cloud with the same ID
+    CloudActorMap::iterator ca_it = cloud_actor_map_->find (id);
+
+    bool shape = true;
+    // Try to find a shape first
+    if (am_it == shape_actor_map_->end ())
+    {
+        // There is no cloud or shape with this ID, so just exit
+        if (ca_it == cloud_actor_map_->end ())
+            return false;
+        // Cloud found, set shape to false
+        shape = false;
+    }
+
+    // Remove the pointer/ID pair to the global actor map
+    if (shape)
+    {
+        if (removeActorFromRenderer (am_it->second))
+        {
+            shape_actor_map_->erase (am_it);
+            return (true);
+        }
+    }
+    else
+    {
+        if (removeActorFromRenderer (ca_it->second.actor))
+        {
+            cloud_actor_map_->erase (ca_it);
+            return true;
+        }
+    }
+    return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeText3D (const std::string &id)
+{
+    // Check to see if the given ID entry exists
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it == shape_actor_map_->end ())
+        return false;
+
+    // Remove it from all renderers
+    if (removeActorFromRenderer (am_it->second))
+        return shape_actor_map_->erase (am_it), true;
+
+    return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeAllPointClouds ()
+{
+    // Check to see if the given ID entry exists
+    CloudActorMap::iterator am_it = cloud_actor_map_->begin ();
+    while (am_it != cloud_actor_map_->end () )
+    {
+        if (removePointCloud (am_it->first))
+            am_it = cloud_actor_map_->begin ();
+        else
+            ++am_it;
+    }
+    return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeAllShapes ()
+{
+    // Check to see if the given ID entry exists
+    ShapeActorMap::iterator am_it = shape_actor_map_->begin ();
+    while (am_it != shape_actor_map_->end ())
+    {
+        if (removeShape (am_it->first))
+            am_it = shape_actor_map_->begin ();
+        else
+            ++am_it;
+    }
+    return (true);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor)
+{
+    vtkLODActor* actor_to_remove = vtkLODActor::SafeDownCast (actor);
+
+
+
+    // Iterate over all actors in this renderer
+    vtkPropCollection* actors = renderer_->GetViewProps ();
+    actors->InitTraversal ();
+
+    vtkProp* current_actor = NULL;
+    while ((current_actor = actors->GetNextProp ()) != NULL)
+    {
+        if (current_actor != actor_to_remove)
+            continue;
+        renderer_->RemoveActor (actor);
+        //        renderer->Render ();
+        // Found the correct viewport and removed the actor
+        return (true);
+    }
+
+    return false;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor)
+{
+    vtkActor* actor_to_remove = vtkActor::SafeDownCast (actor);
+
+    // Add it to all renderers
+    //rens_->InitTraversal ();
+
+
+        // Iterate over all actors in this renderer
+    vtkPropCollection* actors = renderer_->GetViewProps ();
+    actors->InitTraversal ();
+    vtkProp* current_actor = NULL;
+    while ((current_actor = actors->GetNextProp ()) != NULL)
+    {
+        if (current_actor != actor_to_remove)
+            continue;
+        renderer_->RemoveActor (actor);
+        //        renderer->Render ();
+        // Found the correct viewport and removed the actor
+        return (true);
+    }
+    return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor)
+{
+    vtkProp* actor_to_remove = vtkProp::SafeDownCast(actor);
+
+    vtkPropCollection* actors = renderer_->GetViewProps ();
+    actors->InitTraversal ();
+    vtkProp* current_actor = NULL;
+    while ((current_actor = actors->GetNextProp ()) != NULL)
+    {
+        if (current_actor != actor_to_remove)
+            continue;
+        renderer_->RemoveActor (actor);
+        return true;
+    }
+    return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor, bool use_scalars)
+{
+    if (!actor)
+        actor = vtkSmartPointer<vtkLODActor>::New ();
+
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetInput (data);
+
+    if (use_scalars)
+    {
+        vtkSmartPointer<vtkDataArray> scalars = data->GetPointData ()->GetScalars ();
+        if (scalars)
+        {
+            cv::Vec3d minmax(scalars->GetRange());
+            mapper->SetScalarRange(minmax.val);
+            mapper->SetScalarModeToUsePointData ();
+
+            // interpolation OFF, if data is a vtkPolyData that contains only vertices, ON for anything else.
+            vtkPolyData* polyData = vtkPolyData::SafeDownCast (data);
+            bool interpolation = (polyData && polyData->GetNumberOfCells () != polyData->GetNumberOfVerts ());
+
+            mapper->SetInterpolateScalarsBeforeMapping (interpolation);
+            mapper->ScalarVisibilityOn ();
+        }
+    }
+    mapper->ImmediateModeRenderingOff ();
+
+    actor->SetNumberOfCloudPoints (int (std::max<vtkIdType> (1, data->GetNumberOfPoints () / 10)));
+    actor->GetProperty ()->SetInterpolationToFlat ();
+
+    /// FIXME disabling backface culling due to known VTK bug: vtkTextActors are not
+    /// shown when there is a vtkActor with backface culling on present in the scene
+    /// Please see VTK bug tracker for more details: http://www.vtk.org/Bug/view.php?id=12588
+    // actor->GetProperty ()->BackfaceCullingOn ();
+
+    actor->SetMapper (mapper);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setBackgroundColor (const Color& color)
+{
+    Color c = vtkcolor(color);
+    renderer_->SetBackground (c.val);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setPointCloudColor (const Color& color, const std::string &id)
+{
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it != cloud_actor_map_->end ())
+    {
+        vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+        Color c = vtkcolor(color);
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        actor->Modified ();
+    }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::getPointCloudRenderingProperties (int property, double &value, const std::string &id)
+{
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return false;
+
+    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+    switch (property)
+    {
+        case VIZ_POINT_SIZE:
+        {
+            value = actor->GetProperty ()->GetPointSize ();
+            actor->Modified ();
+            break;
+        }
+        case VIZ_OPACITY:
+        {
+            value = actor->GetProperty ()->GetOpacity ();
+            actor->Modified ();
+            break;
+        }
+        case VIZ_LINE_WIDTH:
+        {
+            value = actor->GetProperty ()->GetLineWidth ();
+            actor->Modified ();
+            break;
+        }
+        default:
+            CV_Assert("getPointCloudRenderingProperties: Unknown property");
+    }
+
+    return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setPointCloudRenderingProperties (int property, double value, const std::string &id)
+{
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;
+
+    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+    switch (property)
+    {
+        case VIZ_POINT_SIZE:
+        {
+            actor->GetProperty ()->SetPointSize (float (value));
+            actor->Modified ();
+            break;
+        }
+        case VIZ_OPACITY:
+        {
+            actor->GetProperty ()->SetOpacity (value);
+            actor->Modified ();
+            break;
+        }
+            // Turn on/off flag to control whether data is rendered using immediate
+            // mode or note. Immediate mode rendering tends to be slower but it can
+            // handle larger datasets. The default value is immediate mode off. If you
+            // are having problems rendering a large dataset you might want to consider
+            // using immediate more rendering.
+        case VIZ_IMMEDIATE_RENDERING:
+        {
+            actor->GetMapper ()->SetImmediateModeRendering (int (value));
+            actor->Modified ();
+            break;
+        }
+        case VIZ_LINE_WIDTH:
+        {
+            actor->GetProperty ()->SetLineWidth (float (value));
+            actor->Modified ();
+            break;
+        }
+        default:
+            CV_Assert("setPointCloudRenderingProperties: Unknown property");
+    }
+    return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setPointCloudSelected (const bool selected, const std::string &id)
+{
+    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+    if (am_it == cloud_actor_map_->end ())
+        return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;
+
+    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+    if (selected)
+    {
+        actor->GetProperty ()->EdgeVisibilityOn ();
+        actor->GetProperty ()->SetEdgeColor (1.0, 0.0, 0.0);
+        actor->Modified ();
+    }
+    else
+    {
+        actor->GetProperty ()->EdgeVisibilityOff ();
+        actor->Modified ();
+    }
+
+    return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setShapeColor (const Color& color, const std::string &id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+
+        Color c = vtkcolor(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 ();
+    }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setShapeRenderingProperties (int property, double value, const std::string &id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it == shape_actor_map_->end ())
+        return std::cout << "[setShapeRenderingProperties] Could not find any shape with id <" << id << ">!\n" << std::endl, false;
+
+    vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+
+    switch (property)
+    {
+    case VIZ_POINT_SIZE:
+    {
+        actor->GetProperty ()->SetPointSize (float (value));
+        actor->Modified ();
+        break;
+    }
+    case VIZ_OPACITY:
+    {
+        actor->GetProperty ()->SetOpacity (value);
+        actor->Modified ();
+        break;
+    }
+    case VIZ_LINE_WIDTH:
+    {
+        actor->GetProperty ()->SetLineWidth (float (value));
+        actor->Modified ();
+        break;
+    }
+    case VIZ_FONT_SIZE:
+    {
+        vtkTextActor* text_actor = vtkTextActor::SafeDownCast (am_it->second);
+        vtkSmartPointer<vtkTextProperty> tprop = text_actor->GetTextProperty ();
+        tprop->SetFontSize (int (value));
+        text_actor->Modified ();
+        break;
+    }
+    case VIZ_REPRESENTATION:
+    {
+        switch (int (value))
+        {
+            case REPRESENTATION_POINTS:    actor->GetProperty ()->SetRepresentationToPoints (); break;
+            case REPRESENTATION_WIREFRAME: actor->GetProperty ()->SetRepresentationToWireframe (); break;
+            case REPRESENTATION_SURFACE:   actor->GetProperty ()->SetRepresentationToSurface ();  break;
+        }
+        actor->Modified ();
+        break;
+    }
+    case VIZ_SHADING:
+    {
+        switch (int (value))
+        {
+        case SHADING_FLAT: actor->GetProperty ()->SetInterpolationToFlat (); break;
+        case SHADING_GOURAUD:
+        {
+            if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
+            {
+                std::cout << "[temp_viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Gouraud shading was requested. Estimating normals...\n" << std::endl;
+
+                vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
+                normals->SetInput (actor->GetMapper ()->GetInput ());
+                normals->Update ();
+                vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
+            }
+            actor->GetProperty ()->SetInterpolationToGouraud ();
+            break;
+        }
+        case SHADING_PHONG:
+        {
+            if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
+            {
+                std::cout << "[temp_viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Phong shading was requested. Estimating normals...\n" << std::endl;
+                vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
+                normals->SetInput (actor->GetMapper ()->GetInput ());
+                normals->Update ();
+                vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
+            }
+            actor->GetProperty ()->SetInterpolationToPhong ();
+            break;
+        }
+        }
+        actor->Modified ();
+        break;
+    }
+    default:
+        CV_Assert("setShapeRenderingProperties: Unknown property");
+
+    }
+    return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::initCameraParameters ()
+{
+    Camera camera_temp;
+    // Set default camera parameters to something meaningful
+    camera_temp.clip = Vec2d(0.01, 1000.01);
+
+    // Look straight along the z-axis
+    camera_temp.focal = Vec3d(0.0, 0.0, 1.0);
+
+    // Position the camera at the origin
+    camera_temp.pos = Vec3d(0.0, 0.0, 0.0);
+
+    // Set the up-vector of the camera to be the y-axis
+    camera_temp.view_up = Vec3d(0.0, 1.0, 0.0);
+
+    // Set the camera field of view to about
+    camera_temp.fovy = 0.8575;
+    camera_temp.window_size = Vec2i(window_->GetScreenSize()) / 2;
+    camera_temp.window_pos = Vec2i(0, 0);
+
+    setCameraParameters (camera_temp);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::cameraParamsSet () const { return (camera_set_); }
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::updateCamera ()
+{
+    std::cout << "[temp_viz::PCLVisualizer::updateCamera()] This method was deprecated, just re-rendering all scenes now." << std::endl;
+    //rens_->InitTraversal ();
+    // Update the camera parameters
+
+    renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::updateShapePose (const std::string &id, const cv::Affine3f& pose)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+
+    vtkLODActor* actor;
+
+    if (am_it == shape_actor_map_->end ())
+        return (false);
+    else
+        actor = vtkLODActor::SafeDownCast (am_it->second);
+
+    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+
+    convertToVtkMatrix (pose.matrix, matrix);
+
+    actor->SetUserMatrix (matrix);
+    actor->Modified ();
+
+    return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::getCameras (temp_viz::Camera& camera)
+{
+    vtkCamera* active_camera = renderer_->GetActiveCamera ();
+
+    camera.pos = cv::Vec3d(active_camera->GetPosition());
+    camera.focal = cv::Vec3d(active_camera->GetFocalPoint());
+    camera.clip = cv::Vec2d(active_camera->GetClippingRange());
+    camera.view_up = cv::Vec3d(active_camera->GetViewUp());
+
+    camera.fovy = active_camera->GetViewAngle()/ 180.0 * CV_PI;
+    camera.window_size = cv::Vec2i(renderer_->GetRenderWindow()->GetSize());
+    camera.window_pos = cv::Vec2d::all(0);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+cv::Affine3f temp_viz::Viz3d::VizImpl::getViewerPose ()
+{
+    vtkCamera& camera = *renderer_->GetActiveCamera ();
+
+    Vec3d pos(camera.GetPosition());
+    Vec3d view_up(camera.GetViewUp());
+    Vec3d focal(camera.GetFocalPoint());
+
+    Vec3d y_axis = normalized(view_up);
+    Vec3d z_axis = normalized(focal - pos);
+    Vec3d x_axis = normalized(y_axis.cross(z_axis));
+
+    cv::Matx33d R;
+    R(0, 0) = x_axis[0];
+    R(0, 1) = y_axis[0];
+    R(0, 2) = z_axis[0];
+
+    R(1, 0) = x_axis[1];
+    R(1, 1) = y_axis[1];
+    R(1, 2) = z_axis[1];
+
+    R(2, 0) = x_axis[2];
+    R(2, 1) = y_axis[2];
+    R(2, 2) = z_axis[2];
+
+    return cv::Affine3f(R, pos);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::resetCamera ()
+{
+    renderer_->ResetCamera ();
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraPosition (const cv::Vec3d& pos, const cv::Vec3d& view, const cv::Vec3d& up)
+{
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetPosition (pos[0], pos[1], pos[2]);
+    cam->SetFocalPoint (view[0], view[1], view[2]);
+    cam->SetViewUp (up[0], up[1], up[2]);
+    renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraPosition (double pos_x, double pos_y, double pos_z, double up_x, double up_y, double up_z)
+{
+    //rens_->InitTraversal ();
+
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetPosition (pos_x, pos_y, pos_z);
+    cam->SetViewUp (up_x, up_y, up_z);
+    renderer_->Render ();
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics)
+{
+    // Position = extrinsic translation
+    cv::Vec3f pos_vec = extrinsics.translation();
+
+
+    // Rotate the view vector
+    cv::Matx33f rotation = extrinsics.rotation();
+    cv::Vec3f y_axis (0.f, 1.f, 0.f);
+    cv::Vec3f up_vec (rotation * y_axis);
+
+    // Compute the new focal point
+    cv::Vec3f z_axis (0.f, 0.f, 1.f);
+    cv::Vec3f focal_vec = pos_vec + rotation * z_axis;
+
+    // Get the width and height of the image - assume the calibrated centers are at the center of the image
+    Eigen::Vector2i window_size;
+    window_size[0] = static_cast<int> (intrinsics(0, 2));
+    window_size[1] = static_cast<int> (intrinsics(1, 2));
+
+    // Compute the vertical field of view based on the focal length and image heigh
+    double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI;
+
+    //rens_->InitTraversal ();
+
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]);
+    cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]);
+    cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]);
+    cam->SetUseHorizontalViewAngle (0);
+    cam->SetViewAngle (fovy);
+    cam->SetClippingRange (0.01, 1000.01);
+    window_->SetSize (window_size[0], window_size[1]);
+
+    renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraParameters (const temp_viz::Camera &camera)
+{
+    //rens_->InitTraversal ();
+
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetPosition (camera.pos[0], camera.pos[1], camera.pos[2]);
+    cam->SetFocalPoint (camera.focal[0], camera.focal[1], camera.focal[2]);
+    cam->SetViewUp (camera.view_up[0], camera.view_up[1], camera.view_up[2]);
+    cam->SetClippingRange (camera.clip.val);
+    cam->SetUseHorizontalViewAngle (0);
+    cam->SetViewAngle (camera.fovy * 180.0 / M_PI);
+
+    window_->SetSize (static_cast<int> (camera.window_size[0]), static_cast<int> (camera.window_size[1]));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraClipDistances (double near, double far)
+{
+    //rens_->InitTraversal ();
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetClippingRange (near, far);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraFieldOfView (double fovy)
+{
+    //rens_->InitTraversal ();
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetUseHorizontalViewAngle (0);
+    cam->SetViewAngle (fovy * 180.0 / M_PI);
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::resetCameraViewpoint (const std::string &id)
+{
+    vtkSmartPointer<vtkMatrix4x4> camera_pose;
+    static CloudActorMap::iterator it = cloud_actor_map_->find (id);
+    if (it != cloud_actor_map_->end ())
+        camera_pose = it->second.viewpoint_transformation_;
+    else
+        return;
+
+    // Prevent a segfault
+    if (!camera_pose)
+        return;
+
+    // set all renderer to this viewpoint
+    //rens_->InitTraversal ();
+
+
+    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+    cam->SetPosition (camera_pose->GetElement (0, 3),
+                      camera_pose->GetElement (1, 3),
+                      camera_pose->GetElement (2, 3));
+
+    cam->SetFocalPoint (camera_pose->GetElement (0, 3) - camera_pose->GetElement (0, 2),
+                        camera_pose->GetElement (1, 3) - camera_pose->GetElement (1, 2),
+                        camera_pose->GetElement (2, 3) - camera_pose->GetElement (2, 2));
+
+    cam->SetViewUp (camera_pose->GetElement (0, 1),
+                    camera_pose->GetElement (1, 1),
+                    camera_pose->GetElement (2, 1));
+
+    renderer_->SetActiveCamera (cam);
+    renderer_->ResetCameraClippingRange ();
+    renderer_->Render ();
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addCylinder] A shape with id <"<<id <<"> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkDataSet> data = createCylinder (coefficients);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkDataSet> data = createCube (coefficients);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    Eigen::Vector3f t(translation[0], translation[1], translation[2]);
+    Eigen::Quaternionf q(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+
+    vtkSmartPointer<vtkDataSet> data = createCube (t, q, width, height, depth);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
+                                            const Color& color , const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+    vtkSmartPointer<vtkDataSet> data = createCube (x_min, x_max, y_min, y_max, z_min, z_max);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+
+    Color c = vtkcolor(color);
+    actor->GetProperty ()->SetColor (c.val);
+    renderer_->AddActor (actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addModelFromPolyData] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (polydata, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string & id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addModelFromPolyData] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
+    trans_filter->SetTransform (transform);
+    trans_filter->SetInput (polydata);
+    trans_filter->Update();
+
+    // Create an Actor
+    vtkSmartPointer <vtkLODActor> actor;
+    createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, const std::string &id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.." << std::endl, false;
+
+    vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New ();
+    reader->SetFileName (filename.c_str ());
+
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (reader->GetOutput (), actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    renderer_->AddActor(actor);
+
+    (*shape_actor_map_)[id] = actor;
+    return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id)
+{
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl, false;
+
+    vtkSmartPointer <vtkPLYReader > reader = vtkSmartPointer<vtkPLYReader>::New ();
+    reader->SetFileName (filename.c_str ());
+
+    vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
+    trans_filter->SetTransform (transform);
+    trans_filter->SetInputConnection (reader->GetOutputPort ());
+
+    vtkSmartPointer <vtkLODActor> actor;
+    createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    renderer_->AddActor(actor);
+
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Add a plane from a set of given model coefficients
+  * \param coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+  * \param id the plane id/name (default: "plane")
+  * \param viewport (optional) the id of the new viewport (default: 0)
+  */
+bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addPlane] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkDataSet> data = createPlane (coefficients);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    //  actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetRepresentationToSurface ();
+    actor->GetProperty ()->SetLighting (false);
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addPlane] A shape with id <" << id << "> already exists! Please choose a different id and retry.\n" << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkDataSet> data = createPlane (coefficients, x, y, z);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+    {
+        std::cout << "[addCircle] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.\n" << std::endl;
+        return (false);
+    }
+
+    vtkSmartPointer<vtkDataSet> data = create2DCircle (coefficients);
+
+    // Create an Actor
+    vtkSmartPointer<vtkLODActor> actor;
+    createActorFromVTKDataSet (data, actor);
+    actor->GetProperty ()->SetRepresentationToWireframe ();
+    actor->GetProperty ()->SetLighting (false);
+
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
+{
+   std::string tid = id.empty() ? text : id;
+
+    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addText] A text with id <"<<id <<"> already exists! Please choose a different id and retry.\n" << std::endl, false;
+
+    // Create an Actor
+    vtkSmartPointer<vtkTextActor> actor = vtkSmartPointer<vtkTextActor>::New ();
+    actor->SetPosition (xpos, ypos);
+    actor->SetInput (text.c_str ());
+
+    vtkSmartPointer<vtkTextProperty> tprop = actor->GetTextProperty ();
+    tprop->SetFontSize (fontsize);
+    tprop->SetFontFamilyToArial ();
+    tprop->SetJustificationToLeft ();
+    tprop->BoldOn ();
+
+    Color c = vtkcolor(color);
+    tprop->SetColor (c.val);
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[tid] = actor;
+    return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
+{
+    std::string tid = id.empty() ? text : id;
+
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
+    if (am_it == shape_actor_map_->end ())
+        return false;
+
+    // Retrieve the Actor
+    vtkTextActor *actor = vtkTextActor::SafeDownCast (am_it->second);
+
+    actor->SetPosition (xpos, ypos);
+    actor->SetInput (text.c_str ());
+
+    vtkTextProperty* tprop = actor->GetTextProperty ();
+    tprop->SetFontSize (fontsize);
+
+    Color c = vtkcolor(color);
+    tprop->SetColor (c.val);
+
+    actor->Modified ();
+
+    return (true);
+}
+
+bool temp_viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
+{
+    CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
+
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    if (am_it != shape_actor_map_->end ())
+        return std::cout << "[addPolylineFromPolygonMesh] A shape with id <"<< id << "> already exists! Please choose a different id and retry.\n" << std::endl, false;
+
+    vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
+    poly_points->SetNumberOfPoints (mesh.cloud.size().area());
+
+    const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+    for (int i = 0; i < mesh.cloud.cols; ++i)
+        poly_points->InsertPoint (i, cdata[i].x, cdata[i].y,cdata[i].z);
+
+
+    // Create a cell array to store the lines in and add the lines to it
+    vtkSmartPointer <vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
+    vtkSmartPointer <vtkPolyData> polyData;
+    allocVtkPolyData (polyData);
+
+    for (size_t i = 0; i < mesh.polygons.size (); i++)
+    {
+        vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
+        polyLine->GetPointIds()->SetNumberOfIds(mesh.polygons[i].vertices.size());
+        for(unsigned int k = 0; k < mesh.polygons[i].vertices.size(); k++)
+        {
+            polyLine->GetPointIds()->SetId(k,mesh. polygons[i].vertices[k]);
+        }
+
+        cells->InsertNextCell (polyLine);
+    }
+
+    // Add the points to the dataset
+    polyData->SetPoints (poly_points);
+
+    // Add the lines to the dataset
+    polyData->SetLines (cells);
+
+    // Setup actor and mapper
+    vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+    mapper->SetInput (polyData);
+
+    vtkSmartPointer <vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
+    actor->SetMapper (mapper);
+    renderer_->AddActor(actor);
+
+    // Save the pointer/ID pair to the global actor map
+    (*shape_actor_map_)[id] = actor;
+    return (true);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToSurfaceForAllActors ()
+{
+    vtkActorCollection * actors = renderer_->GetActors ();
+    actors->InitTraversal ();
+    vtkActor * actor;
+    while ((actor = actors->GetNextActor ()) != NULL)
+        actor->GetProperty ()->SetRepresentationToSurface ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToPointsForAllActors ()
+{
+    vtkActorCollection * actors = renderer_->GetActors ();
+    actors->InitTraversal ();
+    vtkActor * actor;
+    while ((actor = actors->GetNextActor ()) != NULL)
+        actor->GetProperty ()->SetRepresentationToPoints ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToWireframeForAllActors ()
+{
+    vtkActorCollection * actors = renderer_->GetActors ();
+    actors->InitTraversal ();
+    vtkActor *actor;
+    while ((actor = actors->GetNextActor ()) != NULL)
+        actor->GetProperty ()->SetRepresentationToWireframe ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::updateCells (vtkSmartPointer<vtkIdTypeArray> &cells, vtkSmartPointer<vtkIdTypeArray> &initcells, vtkIdType nr_points)
+{
+    // 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);
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata)
+{
+    polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata)
+{
+    polydata = vtkSmartPointer<vtkPolyData>::New ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
+{
+    polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix)
+{
+    // set rotation
+    Eigen::Matrix3f rot = orientation.toRotationMatrix ();
+    for (int i = 0; i < 3; i++)
+        for (int k = 0; k < 3; k++)
+            vtk_matrix->SetElement (i, k, rot (i, k));
+
+    // set translation
+    vtk_matrix->SetElement (0, 3, origin (0));
+    vtk_matrix->SetElement (1, 3, origin (1));
+    vtk_matrix->SetElement (2, 3, origin (2));
+    vtk_matrix->SetElement (3, 3, 1.0f);
+}
+
+void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix)
+{
+    for (int i = 0; i < 4; i++)
+        for (int k = 0; k < 4; k++)
+            vtk_matrix->SetElement (i, k, m (i, k));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
+{
+    for (int i = 0; i < 4; i++)
+        for (int k = 0; k < 4; k++)
+            m (i,k) = static_cast<float> (vtk_matrix->GetElement (i, k));
+}

modules/viz/test/test_main.cpp

+#include "test_precomp.hpp"
+
+CV_TEST_MAIN("cv")

modules/viz/test/test_precomp.cpp

+#include "test_precomp.hpp"

modules/viz/test/test_precomp.hpp

+#ifdef __GNUC__
+#  pragma GCC diagnostic ignored "-Wmissing-declarations"
+#  if defined __clang__ || defined __APPLE__
+#    pragma GCC diagnostic ignored "-Wmissing-prototypes"
+#    pragma GCC diagnostic ignored "-Wextra"
+#  endif
+#endif
+
+#ifndef __OPENCV_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include "opencv2/ts.hpp"
+#include "opencv2/core/core_c.h"
+#include <iostream>
+
+#endif

modules/viz/test/test_viz3d.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ //  By downloading, copying, installing or using the software you agree to this license.
+ //  If you do not agree to this license, do not download, install,
+ //  copy or use the software.
+ //
+ //
+ //                           License Agreement
+ //                For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+ // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ //   * Redistribution's of source code must retain the above copyright notice,
+ //     this list of conditions and the following disclaimer.
+ //
+ //   * Redistribution's in binary form must reproduce the above copyright notice,
+ //     this list of conditions and the following disclaimer in the documentation
+ //     and / or other materials provided with the distribution.
+ //
+ //   * The name of the copyright holders may not be used to endorse or promote products
+ //     derived from this software without specific prior written permission.
+ //
+ // This software is provided by the copyright holders and contributors "as is" and
+ // any express or implied warranties, including, but not limited to, the implied
+ // warranties of merchantability and fitness for a particular purpose are disclaimed.
+ // In no event shall the Intel Corporation or contributors be liable for any direct,
+ // indirect, incidental, special, exemplary, or consequential damages
+ // (including, but not limited to, procurement of substitute goods or services;
+ // loss of use, data, or profits; or business interruption) however caused
+ // and on any theory of liability, whether in contract, strict liability,
+ // or tort (including negligence or otherwise) arising in any way out of
+ // the use of this software, even if advised of the possibility of such damage.
+ //
+ //M*/
+#include "test_precomp.hpp"
+#include <opencv2/viz.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <fstream>
+#include <string>
+
+#include <opencv2/viz/types.hpp>
+#include <opencv2/viz/mesh_load.hpp>
+
+
+cv::Mat cvcloud_load()
+{
+    cv::Mat cloud(1, 20000, CV_32FC3);
+        std::ifstream ifs("d:/cloud_dragon.ply");
+
+    std::string str;
+    for(size_t i = 0; i < 11; ++i)
+        std::getline(ifs, str);
+
+    cv::Point3f* data = cloud.ptr<cv::Point3f>();
+    for(size_t i = 0; i < 20000; ++i)
+        ifs >> data[i].x >> data[i].y >> data[i].z;
+
+    return cloud;
+}
+
+TEST(Viz_viz3d, accuracy)
+{
+    temp_viz::Viz3d v("abc");
+    //v.spin();
+
+    v.setBackgroundColor();
+
+    v.addCoordinateSystem(1.0, cv::Affine3f::Identity());
+
+    cv::Mat cloud = cvcloud_load();
+
+
+    cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
+    v.addPointCloud(cloud, colors);
+    cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));
+
+    v.addPointCloudNormals(cloud, normals, 100, 0.02, "n");
+
+
+    temp_viz::ModelCoefficients mc;
+    mc.values.resize(4);
+    mc.values[0] = mc.values[1] = mc.values[2] = mc.values[3] = 1;
+    v.addPlane(mc);
+
+
+    temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply");
+    v.addPolygonMesh(*mesh, "pq");
+
+    v.spinOnce(1000, true);
+
+    v.removeCoordinateSystem();
+
+    for(int i = 0; i < mesh->cloud.cols; ++i)
+        mesh->cloud.ptr<cv::Point3f>()[i] += cv::Point3f(1, 1, 1);
+
+    v.updatePolygonMesh(*mesh, "pq");
+
+
+    for(int i = 0; i < mesh->cloud.cols; ++i)
+        mesh->cloud.ptr<cv::Point3f>()[i] -= cv::Point3f(2, 2, 2);
+    v.addPolylineFromPolygonMesh(*mesh);
+
+
+    v.addText("===Abd sadfljsadlk", 100, 100, cv::Scalar(255, 0, 0), 15);
+        for(int i = 0; i < cloud.cols; ++i)
+        cloud.ptr<cv::Point3f>()[i].x *=2;
+
+    colors.setTo(cv::Scalar(255, 0, 0));
+
+    v.addSphere(cv::Point3f(0, 0, 0), 0.3, temp_viz::Color::blue());
+
+    cv::Mat cvpoly(1, 5, CV_32FC3);
+    cv::Point3f* pdata = cvpoly.ptr<cv::Point3f>();
+    pdata[0] = cv::Point3f(0, 0, 0);
+    pdata[1] = cv::Point3f(0, 1, 1);
+    pdata[2] = cv::Point3f(3, 1, 2);
+    pdata[3] = cv::Point3f(0, 2, 4);
+    pdata[4] = cv::Point3f(7, 2, 3);
+    v.addPolygon(cvpoly, temp_viz::Color::white());
+
+    v.updatePointCloud(cloud, colors);
+    v.spin();
+}
+