From 0e7d4a5703f21f5db02a2299499d16a6b9bf6d4c Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Mon, 18 Mar 2013 19:52:46 +0400 Subject: [PATCH] First version of 'viz' module --- cmake/OpenCVModule.cmake | 4 +- modules/core/include/opencv2/core/affine.hpp | 6 +- modules/viz/CMakeLists.txt | 99 ++ modules/viz/include/opencv2/viz.hpp | 3 + modules/viz/include/opencv2/viz/events.hpp | 110 ++ modules/viz/include/opencv2/viz/mesh_load.hpp | 10 + modules/viz/include/opencv2/viz/types.hpp | 84 ++ modules/viz/include/opencv2/viz/viz3d.hpp | 71 ++ modules/viz/src/common.cpp | 321 ++++++ modules/viz/src/interactor_style.cpp | 678 ++++++++++++ modules/viz/src/mesh_load.cpp | 81 ++ modules/viz/src/precomp.cpp | 1 + modules/viz/src/precomp.hpp | 157 +++ modules/viz/src/q/common.h | 123 +++ modules/viz/src/q/interactor_style.h | 150 +++ modules/viz/src/q/shapes.h | 124 +++ modules/viz/src/q/viz3d_impl.hpp | 469 +++++++++ modules/viz/src/q/viz_types.h | 22 + modules/viz/src/shapes.cpp | 196 ++++ modules/viz/src/types.cpp | 24 + modules/viz/src/viz.cpp | 1 + modules/viz/src/viz3d.cpp | 94 ++ modules/viz/src/viz3d_impl.cpp | 963 +++++++++++++++++ modules/viz/src/viz_main.cpp | 1398 +++++++++++++++++++++++++ modules/viz/test/test_main.cpp | 3 + modules/viz/test/test_precomp.cpp | 1 + modules/viz/test/test_precomp.hpp | 16 + modules/viz/test/test_viz3d.cpp | 133 +++ 28 files changed, 5340 insertions(+), 2 deletions(-) create mode 100644 modules/viz/CMakeLists.txt create mode 100644 modules/viz/include/opencv2/viz.hpp create mode 100644 modules/viz/include/opencv2/viz/events.hpp create mode 100644 modules/viz/include/opencv2/viz/mesh_load.hpp create mode 100644 modules/viz/include/opencv2/viz/types.hpp create mode 100644 modules/viz/include/opencv2/viz/viz3d.hpp create mode 100644 modules/viz/src/common.cpp create mode 100644 modules/viz/src/interactor_style.cpp create mode 100644 modules/viz/src/mesh_load.cpp create mode 100644 modules/viz/src/precomp.cpp create mode 100644 modules/viz/src/precomp.hpp create mode 100644 modules/viz/src/q/common.h create mode 100644 modules/viz/src/q/interactor_style.h create mode 100644 modules/viz/src/q/shapes.h create mode 100644 modules/viz/src/q/viz3d_impl.hpp create mode 100644 modules/viz/src/q/viz_types.h create mode 100644 modules/viz/src/shapes.cpp create mode 100644 modules/viz/src/types.cpp create mode 100644 modules/viz/src/viz.cpp create mode 100644 modules/viz/src/viz3d.cpp create mode 100644 modules/viz/src/viz3d_impl.cpp create mode 100644 modules/viz/src/viz_main.cpp create mode 100644 modules/viz/test/test_main.cpp create mode 100644 modules/viz/test/test_precomp.cpp create mode 100644 modules/viz/test/test_precomp.hpp create mode 100644 modules/viz/test/test_viz3d.cpp diff --git a/cmake/OpenCVModule.cmake b/cmake/OpenCVModule.cmake index 48aa713..bd5720b 100644 --- a/cmake/OpenCVModule.cmake +++ b/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}) diff --git a/modules/core/include/opencv2/core/affine.hpp b/modules/core/include/opencv2/core/affine.hpp index ccbc5c8..c9329c9 100644 --- a/modules/core/include/opencv2/core/affine.hpp +++ b/modules/core/include/opencv2/core/affine.hpp @@ -109,6 +109,8 @@ namespace cv template operator Affine3() const; + operator cv::Mat(); + Mat4 matrix; #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H @@ -314,6 +316,8 @@ template template inline cv::Affine3::operator Affin return Affine3(matrix); } +template inline cv::Affine3::operator cv::Mat() { return cv::Mat(matrix, false); } + template inline cv::Affine3 cv::operator*(const cv::Affine3& affine1, const cv::Affine3& 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 inline cv::Affine3::Affine3(const Eigen::Transform& affine) { diff --git a/modules/viz/CMakeLists.txt b/modules/viz/CMakeLists.txt new file mode 100644 index 0000000..0509478 --- /dev/null +++ b/modules/viz/CMakeLists.txt @@ -0,0 +1,99 @@ +############################################################################### +# 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() + diff --git a/modules/viz/include/opencv2/viz.hpp b/modules/viz/include/opencv2/viz.hpp new file mode 100644 index 0000000..3e1663d --- /dev/null +++ b/modules/viz/include/opencv2/viz.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include diff --git a/modules/viz/include/opencv2/viz/events.hpp b/modules/viz/include/opencv2/viz/events.hpp new file mode 100644 index 0000000..c39580a --- /dev/null +++ b/modules/viz/include/opencv2/viz/events.hpp @@ -0,0 +1,110 @@ +#pragma once + +#include +#include + +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; +} diff --git a/modules/viz/include/opencv2/viz/mesh_load.hpp b/modules/viz/include/opencv2/viz/mesh_load.hpp new file mode 100644 index 0000000..737679c --- /dev/null +++ b/modules/viz/include/opencv2/viz/mesh_load.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include +#include +#include + +namespace temp_viz +{ + CV_EXPORTS Mesh3d::Ptr mesh_load(const String& file); +} diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp new file mode 100644 index 0000000..6085da2 --- /dev/null +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include +#include + +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 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 vertices; + }; + + class CV_EXPORTS Mesh3d + { + public: + typedef cv::Ptr Ptr; + + Mat cloud, colors; + std::vector 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 inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } +} diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp new file mode 100644 index 0000000..e48f520 --- /dev/null +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -0,0 +1,71 @@ +#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 +#include + + +#include +#include + +namespace temp_viz +{ + class CV_EXPORTS Viz3d + { + public: + + typedef cv::Ptr 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 ¢er, 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_; + }; +} + + + diff --git a/modules/viz/src/common.cpp b/modules/viz/src/common.cpp new file mode 100644 index 0000000..78678a0 --- /dev/null +++ b/modules/viz/src/common.cpp @@ -0,0 +1,321 @@ +#include +#include +#include + + +///////////////////////////////////////////////////////////////////////////////////////////// + +//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 (max_bb.x () - center.x ())), +// fabs (static_cast (max_bb.y () - center.y ())), +// fabs (static_cast (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; +} diff --git a/modules/viz/src/interactor_style.cpp b/modules/viz/src/interactor_style.cpp new file mode 100644 index 0000000..d13ad3c --- /dev/null +++ b/modules/viz/src/interactor_style.cpp @@ -0,0 +1,678 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#include + +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::New (); + snapshot_writer_ = vtkSmartPointer::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 callback) +{ + return (mouse_signal_.connect (callback)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +boost::signals2::connection temp_viz::InteractorStyle::registerKeyboardCallback (boost::function 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 ac = CurrentRenderer->GetActors (); + vtkCollectionSimpleIterator ait; + for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) + { + for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) + { + vtkSmartPointer apart = reinterpret_cast (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 (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 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 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 ac = CurrentRenderer->GetActors (); + vtkCollectionSimpleIterator ait; + for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) + { + for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) + { + vtkSmartPointer apart = reinterpret_cast (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 ac = CurrentRenderer->GetActors (); + vtkCollectionSimpleIterator ait; + for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); ) + { + for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); ) + { + vtkSmartPointer apart = static_cast (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 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 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 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 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); + +} + diff --git a/modules/viz/src/mesh_load.cpp b/modules/viz/src/mesh_load.cpp new file mode 100644 index 0000000..8b0a678 --- /dev/null +++ b/modules/viz/src/mesh_load.cpp @@ -0,0 +1,81 @@ +#include + +#include "precomp.hpp" + +#include +#include +#include +#include +#include + +temp_viz::Mesh3d::Ptr temp_viz::mesh_load(const String& file) +{ + Mesh3d::Ptr mesh = new Mesh3d(); + + vtkSmartPointer reader = vtkSmartPointer::New(); + reader->SetFileName(file.c_str()); + reader->Update(); + vtkSmartPointer poly_data = reader->GetOutput (); + + typedef unsigned int uint32_t; + mesh->polygons.clear(); + + vtkSmartPointer 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()[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()[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 (cell_points[i]); + ++id_poly; + } + + return mesh; +} diff --git a/modules/viz/src/precomp.cpp b/modules/viz/src/precomp.cpp new file mode 100644 index 0000000..89d1450 --- /dev/null +++ b/modules/viz/src/precomp.cpp @@ -0,0 +1 @@ +#include "precomp.hpp" \ No newline at end of file diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp new file mode 100644 index 0000000..ec60d50 --- /dev/null +++ b/modules/viz/src/precomp.hpp @@ -0,0 +1,157 @@ +#pragma once + +#include +#include +#include + + +#include +#include +#include +#include + +#include + +#if defined __GNUC__ +#pragma GCC system_header +#ifdef __DEPRECATED +#undef __DEPRECATED +#define __DEPRECATED_DISABLED__ +#endif +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +#include +#include +#include + + + +#include +#include +#include +#include +#include + + + +#if defined __GNUC__ && defined __DEPRECATED_DISABLED__ +#define __DEPRECATED +#undef __DEPRECATED_DISABLED__ +#endif diff --git a/modules/viz/src/q/common.h b/modules/viz/src/q/common.h new file mode 100644 index 0000000..5562bf4 --- /dev/null +++ b/modules/viz/src/q/common.h @@ -0,0 +1,123 @@ +#pragma once + +#include +#include +#include +//#include + +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; + } + }; + +} diff --git a/modules/viz/src/q/interactor_style.h b/modules/viz/src/q/interactor_style.h new file mode 100644 index 0000000..91493ed --- /dev/null +++ b/modules/viz/src/q/interactor_style.h @@ -0,0 +1,150 @@ +#pragma once + +#include +#include + +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& actors) { actors_ = actors; } + + + /** \brief Pass a set of renderers to the interactor style. + * \param[in] rens the vtkRendererCollection to use + */ + void setRenderer (vtkSmartPointer& 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 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 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 rens_; + vtkSmartPointer renderer_; + + /** \brief Actor map stored internally. */ + cv::Ptr 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 snapshot_writer_; + /** \brief Internal window to image filter. Needed by \a snapshot_writer_. */ + vtkSmartPointer wif_; + + boost::signals2::signal mouse_signal_; + boost::signals2::signal 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_; + }; +} diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h new file mode 100644 index 0000000..3ff6375 --- /dev/null +++ b/modules/viz/src/q/shapes.h @@ -0,0 +1,124 @@ +#pragma once + +#include +#include +#include "precomp.hpp" + +namespace temp_viz +{ + CV_EXPORTS vtkSmartPointer createLine (const cv::Point3f& pt1, const cv::Point3f& pt2); + CV_EXPORTS vtkSmartPointer createSphere (const cv::Point3f ¢er, 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 data = temp_viz::createCylinder (cylinder_coeff, numsides); + * \endcode + * + * \ingroup visualization + */ + CV_EXPORTS vtkSmartPointer 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 data = temp_viz::createPlane (plane_coeff); + * \endcode + * + * \ingroup visualization + */ + CV_EXPORTS vtkSmartPointer 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 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 data = temp_viz::create2DCircle (circle_coeff, z); + * \endcode + * + * \ingroup visualization + */ + CV_EXPORTS vtkSmartPointer 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 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 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 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 &polydata); +} diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp new file mode 100644 index 0000000..1d946f1 --- /dev/null +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -0,0 +1,469 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace temp_viz +{ + +class CV_EXPORTS Viz3d::VizImpl +{ +public: + typedef cv::Ptr 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 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 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 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 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 ¢er, float radius, const Color& color, const std::string &id = "sphere"); + bool updateSphere (const cv::Point3f ¢er, float radius, const Color& color, const std::string &id = "sphere"); + + // Add a vtkPolydata as a mesh + bool addModelFromPolyData (vtkSmartPointer polydata, const std::string & id = "PolyData"); + bool addModelFromPolyData (vtkSmartPointer polydata, vtkSmartPointer transform, const std::string &id = "PolyData"); + bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel"); + bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer 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 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 getRenderWindow () { return (window_); } + + void setPosition (int x, int y); + void setSize (int xw, int yw); + +private: + vtkSmartPointer 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 (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 exit_main_loop_timer_callback_; + vtkSmartPointer exit_callback_; + + vtkSmartPointer renderer_; + vtkSmartPointer window_; + + /** \brief The render window interactor style. */ + vtkSmartPointer style_; + + /** \brief Internal list with actor pointers and name IDs for point clouds. */ + cv::Ptr cloud_actor_map_; + + /** \brief Internal list with actor pointers and name IDs for shapes. */ + cv::Ptr shape_actor_map_; + + /** \brief Boolean that holds whether or not the camera parameters were manually initialized*/ + bool camera_set_; + + bool removeActorFromRenderer (const vtkSmartPointer &actor); + bool removeActorFromRenderer (const vtkSmartPointer &actor); + bool removeActorFromRenderer (const vtkSmartPointer &actor); + + //void addActorToRenderer (const vtkSmartPointer &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 &data, vtkSmartPointer &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 &cells, vtkSmartPointer &initcells, vtkIdType nr_points); + + void allocVtkPolyData (vtkSmartPointer &polydata); + void allocVtkPolyData (vtkSmartPointer &polydata); + void allocVtkUnstructuredGrid (vtkSmartPointer &polydata); + +}; + +//void getTransformationMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternionf& orientation, Eigen::Matrix4f &transformation); + +//void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer &vtk_matrix); + +void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer &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 &orientation, vtkSmartPointer &vtk_matrix); +void convertToEigenMatrix (const vtkSmartPointer &vtk_matrix, Eigen::Matrix4f &m); + +} + + + diff --git a/modules/viz/src/q/viz_types.h b/modules/viz/src/q/viz_types.h new file mode 100644 index 0000000..1acfa4b --- /dev/null +++ b/modules/viz/src/q/viz_types.h @@ -0,0 +1,22 @@ +#pragma once + +#include "precomp.hpp" + +namespace temp_viz +{ + struct CV_EXPORTS CloudActor + { + /** \brief The actor holding the data to render. */ + vtkSmartPointer actor; + + /** \brief The viewpoint transformation matrix. */ + vtkSmartPointer viewpoint_transformation_; + + /** \brief Internal cell array. Used for optimizing updatePointCloud. */ + vtkSmartPointer cells; + }; + + typedef std::map CloudActorMap; + typedef std::map > ShapeActorMap; +} + diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp new file mode 100644 index 0000000..955cf38 --- /dev/null +++ b/modules/viz/src/shapes.cpp @@ -0,0 +1,196 @@ +#include + +inline float rad2deg (float alpha) +{ return (alpha * 57.29578f); } + +inline double rad2deg (double alpha){return (alpha * 57.29578);} + + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides) +{ + vtkSmartPointer line = vtkSmartPointer::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 tuber = vtkSmartPointer::New (); + tuber->SetInputConnection (line->GetOutputPort ()); + tuber->SetRadius (coefficients.values[6]); + tuber->SetNumberOfSides (numsides); + + return (tuber->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createCube (const temp_viz::ModelCoefficients &coefficients) +{ + // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth] + vtkSmartPointer t = vtkSmartPointer::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 cube = vtkSmartPointer::New (); + cube->SetXLength (coefficients.values[7]); + cube->SetYLength (coefficients.values[8]); + cube->SetZLength (coefficients.values[9]); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (cube->GetOutputPort ()); + + return (tf->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer 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 t = vtkSmartPointer::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 cube = vtkSmartPointer::New (); + cube->SetXLength (width); + cube->SetYLength (height); + cube->SetZLength (depth); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (cube->GetOutputPort ()); + + return (tf->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max) +{ + vtkSmartPointer t = vtkSmartPointer::New (); + vtkSmartPointer cube = vtkSmartPointer::New (); + cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max); + return (cube->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients) +{ + vtkSmartPointer plane = vtkSmartPointer::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 temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z) +{ + vtkSmartPointer plane = vtkSmartPointer::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 temp_viz::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z) +{ + vtkSmartPointer disk = vtkSmartPointer::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 could be with + /* + vtkSmartPointer circle = vtkSmartPointer::New(); + circle->SetRadius (coefficients.values[2]); + circle->SetNumberOfSides (100); + + vtkSmartPointer tube = vtkSmartPointer::New(); + tube->SetInput (circle->GetOutput()); + tube->SetNumberOfSides (25); + tube->SetRadius (0.001); + */ + + // Set the circle origin + vtkSmartPointer t = vtkSmartPointer::New (); + t->Identity (); + t->Translate (coefficients.values[0], coefficients.values[1], z); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (disk->GetOutputPort ()); + /* + tf->SetInputConnection (tube->GetOutputPort ()); + */ + + return (tf->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution) +{ + // Set the sphere origin + vtkSmartPointer t = vtkSmartPointer::New (); + t->Identity (); + t->Translate (center.x, center.y, center.z); + + vtkSmartPointer s_sphere = vtkSmartPointer::New (); + s_sphere->SetRadius (radius); + s_sphere->SetPhiResolution (sphere_resolution); + s_sphere->SetThetaResolution (sphere_resolution); + s_sphere->LatLongTessellationOff (); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (s_sphere->GetOutputPort ()); + tf->Update (); + + return (tf->GetOutput ()); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +vtkSmartPointer temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2) +{ + vtkSmartPointer line = vtkSmartPointer::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 &polydata) +{ + polydata = vtkSmartPointer::New (); +} + + diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp new file mode 100644 index 0000000..0baed3a --- /dev/null +++ b/modules/viz/src/types.cpp @@ -0,0 +1,24 @@ +#include + + + +////////////////////////////////////////////////////////////////////////////////////////////////////// +/// 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); } + diff --git a/modules/viz/src/viz.cpp b/modules/viz/src/viz.cpp new file mode 100644 index 0000000..89d1450 --- /dev/null +++ b/modules/viz/src/viz.cpp @@ -0,0 +1 @@ +#include "precomp.hpp" \ No newline at end of file diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp new file mode 100644 index 0000000..9e35e42 --- /dev/null +++ b/modules/viz/src/viz3d.cpp @@ -0,0 +1,94 @@ +#include +#include + + +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 ¢er, 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); +} diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp new file mode 100644 index 0000000..20d8a06 --- /dev/null +++ b/modules/viz/src/viz3d_impl.cpp @@ -0,0 +1,963 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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 polydata; + + allocVtkPolyData(polydata); + //polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + polydata->SetVerts (vertices); + + vtkSmartPointer initcells; + vtkIdType nr_points = cloud.size().area(); + vtkSmartPointer points = polydata->GetPoints (); + + if (!points) + { + points = vtkSmartPointer::New (); + points->SetDataTypeToFloat (); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); + + // Get a pointer to the beginning of the data array + float *data = (static_cast (points->GetData ()))->GetPointer (0); + + if (mask.empty()) + { + int j = 0; + for(int y = 0; y < cloud.rows; ++y) + { + const cv::Point3f* crow = cloud.ptr(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(y); + const unsigned char* mrow = mask.ptr(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 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 scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + reinterpret_cast(&(*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(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(y); + const unsigned char* mrow = mask.ptr(y); + for(int x = 0; x < colors.cols; ++x) + if (mrow[x]) + memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); + } + } + + reinterpret_cast(&(*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 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& sensor_orientation = Eigen::Quaternionf::Identity (); + + // Save the viewpoint transformation matrix to the global actor map + vtkSmartPointer transformation = vtkSmartPointer::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 polydata = reinterpret_cast(am_it->second.actor->GetMapper ())->GetInput (); + if (!polydata) + return (false); + vtkSmartPointer vertices = polydata->GetVerts (); + vtkSmartPointer 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 (points->GetData ()))->GetPointer (0); + + if (mask.empty()) + { + int j = 0; + for(int y = 0; y < cloud.rows; ++y) + { + const cv::Point3f* crow = cloud.ptr(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(y); + const unsigned char* mrow = mask.ptr(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 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 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 scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + reinterpret_cast(&(*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(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(y); + const unsigned char* mrow = mask.ptr(y); + for(int x = 0; x < colors.cols; ++x) + if (mrow[x]) + memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); + } + } + + reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); + + ///////////////////////////////////////// + has_colors = true; + + if (has_colors) + { + polydata->GetPointData ()->SetScalars (scalars); + scalars->GetRange (minmax); + } + +#else + vtkSmartPointer scalars; + polydata->GetPointData ()->SetScalars (scalars); + polydata->Update (); + double minmax[2]; + minmax[0] = std::numeric_limits::min (); + minmax[1] = std::numeric_limits::max (); + am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); + am_it->second.actor->GetMapper ()->SetScalarRange (minmax); +#endif + + + // Update the mapper + reinterpret_cast(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 points = vtkSmartPointer::New(); + vtkSmartPointer lines = vtkSmartPointer::New(); + + points->SetDataTypeToFloat (); + vtkSmartPointer data = vtkSmartPointer::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 (sqrt (double (level))); + nr_normals = (static_cast ((cloud.cols - 1)/ point_step) + 1) * + (static_cast ((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(y, x); + cv::Point3f n = normals.at(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()[i]; + cv::Point3f n = normals.ptr()[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 polyData = vtkSmartPointer::New(); + polyData->SetPoints (points); + polyData->SetLines (lines); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput (polyData); + mapper->SetColorModeToMapScalars(); + mapper->SetScalarModeToUsePointData(); + + // create actor + vtkSmartPointer actor = vtkSmartPointer::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 data = createLine (pt1, pt2); + + // Create an Actor + vtkSmartPointer 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 fields; + + + // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields); + // if (rgb_idx == -1) + // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields); + + vtkSmartPointer colors_array; +#if 1 + if (!mesh.colors.empty()) + { + colors_array = vtkSmartPointer::New (); + colors_array->SetNumberOfComponents (3); + colors_array->SetName ("Colors"); + + const unsigned char* data = mesh.colors.ptr(); + + //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 (&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 points = vtkSmartPointer::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 (points->GetData ())->GetPointer (0); + + + std::vector 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(); + const cv::Point3f *cdata = mesh.cloud.ptr(); + cv::Point3f* out = reinterpret_cast(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 (mesh.polygons[i].vertices.size ())) + max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); + + vtkSmartPointer actor; + + if (mesh.polygons.size () > 1) + { + // Create polys from polyMesh.polygons + vtkSmartPointer cell_array = vtkSmartPointer::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 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 polygon = vtkSmartPointer::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 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(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData (); + + const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero (); + const Eigen::Quaternion& sensor_orientation = Eigen::Quaternionf::Identity (); + + // Save the viewpoint transformation matrix to the global actor map + vtkSmartPointer transformation = vtkSmartPointer::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 polydata = static_cast(am_it->second.actor->GetMapper ())->GetInput (); + if (!polydata) + return (false); + vtkSmartPointer cells = polydata->GetStrips (); + if (!cells) + return (false); + vtkSmartPointer 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 (points->GetData ()))->GetPointer (0); + + int ptr = 0; + std::vector 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(); + const cv::Point3f *cdata = mesh.cloud.ptr(); + cv::Point3f* out = reinterpret_cast(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(); + for(int i = 0; i < mesh.colors.cols; ++i) + colors_array->InsertNextTupleValue(&data[i*3]); + } + else + { + const unsigned char* color = mesh.colors.ptr(); + const unsigned char* mdata = mask.ptr(); + + 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 (mesh.polygons[i].vertices.size ())) + max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); + + // Update the cells + cells = vtkSmartPointer::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 leader = vtkSmartPointer::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 leader = vtkSmartPointer::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 +//////////////////////////////////////////////////////////////////////////////////////////// +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 <"< already exists! Please choose a different id and retry." << std::endl, false; + + //vtkSmartPointer data = createSphere (center.getVector4fMap (), radius); + vtkSmartPointer data = vtkSmartPointer::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 mapper = vtkSmartPointer::New (); + mapper->SetInputConnection (data->GetOutputPort ()); + + // Create an Actor + vtkSmartPointer actor = vtkSmartPointer::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 ¢er, 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 textSource = vtkSmartPointer::New (); + textSource->SetText (text.c_str()); + textSource->Update (); + + vtkSmartPointer textMapper = vtkSmartPointer::New (); + textMapper->SetInputConnection (textSource->GetOutputPort ()); + + // Since each follower may follow a different camera, we need different followers + vtkRenderer* renderer = renderer_; + + vtkSmartPointer textActor = vtkSmartPointer::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 points = vtkSmartPointer::New (); + vtkSmartPointer polygon = vtkSmartPointer::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()[i]; + points->SetPoint (i, p.x, p.y, p.z); + polygon->GetPointIds ()->SetId (i, i); + } + + vtkSmartPointer 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 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 all_data = vtkSmartPointer::New (); + + // Add old data + all_data->AddInput (reinterpret_cast ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ()); + + // Add new data + vtkSmartPointer surface_filter = vtkSmartPointer::New (); + surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data)); + vtkSmartPointer poly_data = surface_filter->GetOutput (); + all_data->AddInput (poly_data); + + // Create an Actor + vtkSmartPointer 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 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); +} diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp new file mode 100644 index 0000000..2c2fe52 --- /dev/null +++ b/modules/viz/src/viz_main.cpp @@ -0,0 +1,1398 @@ +#include "precomp.hpp" + +#include +#include +#include + +#include +#ifndef __APPLE__ +vtkRenderWindowInteractor* vtkRenderWindowInteractorFixNew () +{ + return (vtkRenderWindowInteractor::New ()); +} +#endif + +///////////////////////////////////////////////////////////////////////////////////////////// +temp_viz::Viz3d::VizImpl::VizImpl (const std::string &name) + : style_ (vtkSmartPointer::New ()) + , cloud_actor_map_ (new CloudActorMap) + , shape_actor_map_ (new ShapeActorMap) + , s_lastDone_(0.0) +{ + renderer_ = vtkSmartPointer::New (); + + // Create a RendererWindow + window_ = vtkSmartPointer::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 ::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 pp = vtkSmartPointer::New (); + pp->SetTolerance (pp->GetTolerance () * 2); + interactor_->SetPicker (pp); + + exit_main_loop_timer_callback_ = vtkSmartPointer::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::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 callback) +{ + return (style_->registerKeyboardCallback (callback)); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +boost::signals2::connection temp_viz::Viz3d::VizImpl::registerMouseCallback (boost::function 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 axes = vtkSmartPointer::New (); + axes->SetOrigin (0, 0, 0); + axes->SetScaleFactor (scale); + + vtkSmartPointer axes_colors = vtkSmartPointer::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 axes_data = axes->GetOutput (); + axes_data->Update (); + axes_data->GetPointData ()->SetScalars (axes_colors); + + vtkSmartPointer axes_tubes = vtkSmartPointer::New (); + axes_tubes->SetInput (axes_data); + axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0); + axes_tubes->SetNumberOfSides (6); + + vtkSmartPointer axes_mapper = vtkSmartPointer::New (); + axes_mapper->SetScalarModeToUsePointData (); + axes_mapper->SetInput (axes_tubes->GetOutput ()); + + vtkSmartPointer axes_actor = vtkSmartPointer::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 &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 &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 &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 &data, vtkSmartPointer &actor, bool use_scalars) +{ + if (!actor) + actor = vtkSmartPointer::New (); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput (data); + + if (use_scalars) + { + vtkSmartPointer 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 (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 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 normals = vtkSmartPointer::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 normals = vtkSmartPointer::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 matrix = vtkSmartPointer::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 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 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 (intrinsics(0, 2)); + window_size[1] = static_cast (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 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 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 (camera.window_size[0]), static_cast (camera.window_size[1])); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::Viz3d::VizImpl::setCameraClipDistances (double near, double far) +{ + //rens_->InitTraversal (); + + vtkSmartPointer cam = renderer_->GetActiveCamera (); + cam->SetClippingRange (near, far); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::Viz3d::VizImpl::setCameraFieldOfView (double fovy) +{ + //rens_->InitTraversal (); + + vtkSmartPointer cam = renderer_->GetActiveCamera (); + cam->SetUseHorizontalViewAngle (0); + cam->SetViewAngle (fovy * 180.0 / M_PI); + +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::Viz3d::VizImpl::resetCameraViewpoint (const std::string &id) +{ + vtkSmartPointer 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 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 <"< already exists! Please choose a different id and retry." << std::endl; + return (false); + } + + vtkSmartPointer data = createCylinder (coefficients); + + // Create an Actor + vtkSmartPointer 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 data = createCube (coefficients); + + // Create an Actor + vtkSmartPointer 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 data = createCube (t, q, width, height, depth); + + // Create an Actor + vtkSmartPointer 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 data = createCube (x_min, x_max, y_min, y_max, z_min, z_max); + + // Create an Actor + vtkSmartPointer 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 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 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 polydata, vtkSmartPointer 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 <"< already exists! Please choose a different id and retry." << std::endl; + return (false); + } + + vtkSmartPointer trans_filter = vtkSmartPointer::New (); + trans_filter->SetTransform (transform); + trans_filter->SetInput (polydata); + trans_filter->Update(); + + // Create an Actor + vtkSmartPointer 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 <"< already exists! Please choose a different id and retry.." << std::endl, false; + + vtkSmartPointer reader = vtkSmartPointer::New (); + reader->SetFileName (filename.c_str ()); + + vtkSmartPointer 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 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 <"< already exists! Please choose a different id and retry." << std::endl, false; + + vtkSmartPointer reader = vtkSmartPointer::New (); + reader->SetFileName (filename.c_str ()); + + vtkSmartPointer trans_filter = vtkSmartPointer::New (); + trans_filter->SetTransform (transform); + trans_filter->SetInputConnection (reader->GetOutputPort ()); + + vtkSmartPointer 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 <"< already exists! Please choose a different id and retry." << std::endl; + return (false); + } + + vtkSmartPointer data = createPlane (coefficients); + + // Create an Actor + vtkSmartPointer 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 data = createPlane (coefficients, x, y, z); + + // Create an Actor + vtkSmartPointer 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 <"< already exists! Please choose a different id and retry.\n" << std::endl; + return (false); + } + + vtkSmartPointer data = create2DCircle (coefficients); + + // Create an Actor + vtkSmartPointer 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 <"< already exists! Please choose a different id and retry.\n" << std::endl, false; + + // Create an Actor + vtkSmartPointer actor = vtkSmartPointer::New (); + actor->SetPosition (xpos, ypos); + actor->SetInput (text.c_str ()); + + vtkSmartPointer 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 poly_points = vtkSmartPointer::New (); + poly_points->SetNumberOfPoints (mesh.cloud.size().area()); + + const cv::Point3f *cdata = mesh.cloud.ptr(); + 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 cells = vtkSmartPointer::New (); + vtkSmartPointer polyData; + allocVtkPolyData (polyData); + + for (size_t i = 0; i < mesh.polygons.size (); i++) + { + vtkSmartPointer polyLine = vtkSmartPointer::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::New (); + mapper->SetInput (polyData); + + vtkSmartPointer actor = vtkSmartPointer::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 &cells, vtkSmartPointer &initcells, vtkIdType nr_points) +{ + // If no init cells and cells has not been initialized... + if (!cells) + cells = vtkSmartPointer::New (); + + // If we have less values then we need to recreate the array + if (cells->GetNumberOfTuples () < nr_points) + { + cells = vtkSmartPointer::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::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 &polydata) +{ + polydata = vtkSmartPointer::New (); +} +////////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer &polydata) +{ + polydata = vtkSmartPointer::New (); +} +////////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::Viz3d::VizImpl::allocVtkUnstructuredGrid (vtkSmartPointer &polydata) +{ + polydata = vtkSmartPointer::New (); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +void temp_viz::convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion &orientation, vtkSmartPointer &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 &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 &vtk_matrix, Eigen::Matrix4f &m) +{ + for (int i = 0; i < 4; i++) + for (int k = 0; k < 4; k++) + m (i,k) = static_cast (vtk_matrix->GetElement (i, k)); +} diff --git a/modules/viz/test/test_main.cpp b/modules/viz/test/test_main.cpp new file mode 100644 index 0000000..6b24993 --- /dev/null +++ b/modules/viz/test/test_main.cpp @@ -0,0 +1,3 @@ +#include "test_precomp.hpp" + +CV_TEST_MAIN("cv") diff --git a/modules/viz/test/test_precomp.cpp b/modules/viz/test/test_precomp.cpp new file mode 100644 index 0000000..5956e13 --- /dev/null +++ b/modules/viz/test/test_precomp.cpp @@ -0,0 +1 @@ +#include "test_precomp.hpp" diff --git a/modules/viz/test/test_precomp.hpp b/modules/viz/test/test_precomp.hpp new file mode 100644 index 0000000..6c0f493 --- /dev/null +++ b/modules/viz/test/test_precomp.hpp @@ -0,0 +1,16 @@ +#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 + +#endif diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp new file mode 100644 index 0000000..922090c --- /dev/null +++ b/modules/viz/test/test_viz3d.cpp @@ -0,0 +1,133 @@ +/*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 +#include +#include + +#include +#include + +#include +#include + + +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(); + 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()[i] += cv::Point3f(1, 1, 1); + + v.updatePolygonMesh(*mesh, "pq"); + + + for(int i = 0; i < mesh->cloud.cols; ++i) + mesh->cloud.ptr()[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()[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(); + 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(); +} + -- 2.7.4