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 "")
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})
template <typename Y> operator Affine3<Y>() const;
+ operator cv::Mat();
+
Mat4 matrix;
#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
return Affine3<Y>(matrix);
}
+template<typename T> inline cv::Affine3<T>::operator cv::Mat() { return cv::Mat(matrix, false); }
+
template<typename T> inline cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
{
return affine2.concatenate(affine1);
return r;
}
-#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
+#if (defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H) || defined CV_AFFINE_FORCE_EIGEN_PLUGIN
template<typename T> inline cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
{
--- /dev/null
+###############################################################################
+# 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()
+
--- /dev/null
+#pragma once
+
+#include <opencv2/viz/viz3d.hpp>
--- /dev/null
+#pragma once
+
+#include <string>
+#include <opencv2/viz/types.hpp>
+
+namespace cv
+{
+ class KeyboardEvent
+ {
+ public:
+ static const unsigned int Alt = 1;
+ static const unsigned int Ctrl = 2;
+ static const unsigned int Shift = 4;
+
+ /** \brief Constructor
+ * \param[in] action true for key was pressed, false for released
+ * \param[in] key_sym the key-name that caused the action
+ * \param[in] key the key code that caused the action
+ * \param[in] alt whether the alt key was pressed at the time where this event was triggered
+ * \param[in] ctrl whether the ctrl was pressed at the time where this event was triggered
+ * \param[in] shift whether the shift was pressed at the time where this event was triggered
+ */
+ KeyboardEvent (bool action, const std::string& key_sym, unsigned char key, bool alt, bool ctrl, bool shift);
+
+ bool isAltPressed () const;
+ bool isCtrlPressed () const;
+ bool isShiftPressed () const;
+
+ unsigned char getKeyCode () const;
+
+ const std::string& getKeySym () const;
+ bool keyDown () const;
+ bool keyUp () const;
+
+ protected:
+
+ bool action_;
+ unsigned int modifiers_;
+ unsigned char key_code_;
+ std::string key_sym_;
+ };
+
+ class MouseEvent
+ {
+ public:
+ enum Type
+ {
+ MouseMove = 1,
+ MouseButtonPress,
+ MouseButtonRelease,
+ MouseScrollDown,
+ MouseScrollUp,
+ MouseDblClick
+ } ;
+
+ enum MouseButton
+ {
+ NoButton = 0,
+ LeftButton,
+ MiddleButton,
+ RightButton,
+ VScroll /*other buttons, scroll wheels etc. may follow*/
+ } ;
+
+ MouseEvent (const Type& type, const MouseButton& button, const Point& p, bool alt, bool ctrl, bool shift);
+
+
+ Type type;
+ MouseButton button;
+ Point pointer;
+ unsigned int key_state;
+ };
+}
+
+////////////////////////////////////////////////////////////////////
+/// Implementation
+
+inline cv::KeyboardEvent::KeyboardEvent (bool _action, const std::string& _key_sym, unsigned char key, bool alt, bool ctrl, bool shift)
+ : action_ (_action), modifiers_ (0), key_code_(key), key_sym_ (_key_sym)
+{
+ if (alt)
+ modifiers_ = Alt;
+
+ if (ctrl)
+ modifiers_ |= Ctrl;
+
+ if (shift)
+ modifiers_ |= Shift;
+}
+
+inline bool cv::KeyboardEvent::isAltPressed () const { return (modifiers_ & Alt) != 0; }
+inline bool cv::KeyboardEvent::isCtrlPressed () const { return (modifiers_ & Ctrl) != 0; }
+inline bool cv::KeyboardEvent::isShiftPressed () const { return (modifiers_ & Shift) != 0; }
+inline unsigned char cv::KeyboardEvent::getKeyCode () const { return key_code_; }
+inline const std::string& cv::KeyboardEvent::getKeySym () const { return (key_sym_); }
+inline bool cv::KeyboardEvent::keyDown () const { return action_; }
+inline bool cv::KeyboardEvent::keyUp () const { return !action_; }
+
+inline cv::MouseEvent::MouseEvent (const Type& _type, const MouseButton& _button, const Point& _p, bool alt, bool ctrl, bool shift)
+ : type(_type), button(_button), pointer(_p), key_state(0)
+{
+ if (alt)
+ key_state = KeyboardEvent::Alt;
+
+ if (ctrl)
+ key_state |= KeyboardEvent::Ctrl;
+
+ if (shift)
+ key_state |= KeyboardEvent::Shift;
+}
--- /dev/null
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/viz/types.hpp>
+#include <vector>
+
+namespace temp_viz
+{
+ CV_EXPORTS Mesh3d::Ptr mesh_load(const String& file);
+}
--- /dev/null
+#pragma once
+
+#include <vector>
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+#include <opencv2/core/affine.hpp>
+
+namespace temp_viz
+{
+ //qt creator hack
+ typedef cv::Scalar Scalar;
+ typedef cv::Mat Mat;
+ typedef std::string String;
+
+ typedef cv::Vec3d Vec3d;
+ typedef cv::Vec4d Vec4d;
+ typedef cv::Vec2d Vec2d;
+ typedef cv::Vec2i Vec2i;
+ typedef cv::Matx33d Matx33d;
+ typedef cv::Affine3f Affine3f;
+ typedef cv::Affine3d Affine3d;
+ typedef cv::Point3f Point3f;
+ typedef cv::Matx44d Matx44d;
+ typedef cv::Matx44f Matx44f;
+ typedef cv::Size Size;
+ typedef cv::Point Point;
+
+
+
+ struct CV_EXPORTS ModelCoefficients
+ {
+ std::vector<float> values;
+ };
+
+
+ class CV_EXPORTS Color : public Scalar
+ {
+ public:
+ Color();
+ Color(double gray);
+ Color(double blue, double green, double red);
+
+ Color(const Scalar& color);
+
+ static Color black();
+ static Color blue();
+ static Color green();
+ static Color cyan();
+
+ static Color red();
+ static Color magenta();
+ static Color yellow();
+ static Color white();
+
+ static Color gray();
+ };
+
+
+ struct CV_EXPORTS Vertices
+ {
+ std::vector<unsigned int> vertices;
+ };
+
+ class CV_EXPORTS Mesh3d
+ {
+ public:
+ typedef cv::Ptr<Mesh3d> Ptr;
+
+ Mat cloud, colors;
+ std::vector<Vertices> polygons;
+ };
+
+
+ inline Color vtkcolor(const Color& color)
+ {
+ Color scaled_color = color * (1.0/255.0);
+ std::swap(scaled_color[0], scaled_color[2]);
+ return scaled_color;
+ }
+
+ inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); }
+
+ template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
+}
--- /dev/null
+#pragma once
+
+#if !defined YES_I_AGREE_THAT_VIZ_API_IS_NOT_STABLE_NOW_AND_BINARY_COMPARTIBILITY_WONT_BE_SUPPORTED
+ //#error "Viz is in beta state now. Please define macro above to use it"
+#endif
+
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+
+
+#include <string>
+#include <opencv2/viz/types.hpp>
+
+namespace temp_viz
+{
+ class CV_EXPORTS Viz3d
+ {
+ public:
+
+ typedef cv::Ptr<Viz3d> Ptr;
+
+ Viz3d(const String& window_name = String());
+ ~Viz3d();
+
+ void setBackgroundColor(const Color& color = Color::black());
+
+ void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate");
+
+ void addPointCloud(const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat());
+
+ bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud");
+
+
+
+
+ bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane");
+ bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane");
+ bool removeCoordinateSystem (const String &id = "coordinate");
+
+
+ bool updatePointCloud (const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat());
+
+
+ bool addPolygonMesh (const Mesh3d& mesh, const String &id = "polygon");
+ bool updatePolygonMesh (const Mesh3d& mesh, const String &id = "polygon");
+
+ bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String &id = "polyline");
+
+
+ bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String &id = "");
+
+
+ bool addPolygon(const Mat& cloud, const Color& color, const String &id = "polygon");
+
+ bool addSphere (const Point3f ¢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_;
+ };
+}
+
+
+
--- /dev/null
+#include <q/common.h>
+#include <cstdlib>
+#include <opencv2/viz/types.hpp>
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+//Eigen::Matrix4d temp_viz::vtkToEigen (vtkMatrix4x4* vtk_matrix)
+//{
+// Eigen::Matrix4d eigen_matrix = Eigen::Matrix4d::Identity ();
+// for (int i=0; i < 4; i++)
+// for (int j=0; j < 4; j++)
+// eigen_matrix (i, j) = vtk_matrix->GetElement (i, j);
+
+// return eigen_matrix;
+//}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+//Eigen::Vector2i temp_viz::worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
+//{
+// // Transform world to clipping coordinates
+// Eigen::Vector4d world (view_projection_matrix * world_pt);
+// // Normalize w-component
+// world /= world.w ();
+
+// // X/Y screen space coordinate
+// int screen_x = int (floor (double (((world.x () + 1) / 2.0) * width) + 0.5));
+// int screen_y = int (floor (double (((world.y () + 1) / 2.0) * height) + 0.5));
+
+// // Calculate -world_pt.y () because the screen Y axis is oriented top->down, ie 0 is top-left
+// //int winY = (int) floor ( (double) (((1 - world_pt.y ()) / 2.0) * height) + 0.5); // top left
+
+// return (Eigen::Vector2i (screen_x, screen_y));
+//}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+//void temp_viz::getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24])
+//{
+// // Set up the normals
+// Eigen::Vector4d normals[6];
+// for (int i=0; i < 6; i++)
+// {
+// normals[i] = Eigen::Vector4d (0.0, 0.0, 0.0, 1.0);
+
+// // if i is even set to -1, if odd set to +1
+// normals[i] (i/2) = 1 - (i%2)*2;
+// }
+
+// // Transpose the matrix for use with normals
+// Eigen::Matrix4d view_matrix = view_projection_matrix.transpose ();
+
+// // Transform the normals to world coordinates
+// for (int i=0; i < 6; i++)
+// {
+// normals[i] = view_matrix * normals[i];
+
+// double f = 1.0/sqrt (normals[i].x () * normals[i].x () +
+// normals[i].y () * normals[i].y () +
+// normals[i].z () * normals[i].z ());
+
+// planes[4*i + 0] = normals[i].x ()*f;
+// planes[4*i + 1] = normals[i].y ()*f;
+// planes[4*i + 2] = normals[i].z ()*f;
+// planes[4*i + 3] = normals[i].w ()*f;
+// }
+//}
+
+//int temp_viz::cullFrustum (double frustum[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb)
+//{
+// int result = PCL_INSIDE_FRUSTUM;
+
+// for(int i =0; i < 6; i++){
+// double a = frustum[(i*4)];
+// double b = frustum[(i*4)+1];
+// double c = frustum[(i*4)+2];
+// double d = frustum[(i*4)+3];
+
+// //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;
+
+// // Basic VFC algorithm
+// Eigen::Vector3d center ((max_bb.x () - min_bb.x ()) / 2 + min_bb.x (),
+// (max_bb.y () - min_bb.y ()) / 2 + min_bb.y (),
+// (max_bb.z () - min_bb.z ()) / 2 + min_bb.z ());
+
+// Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
+// fabs (static_cast<double> (max_bb.y () - center.y ())),
+// fabs (static_cast<double> (max_bb.z () - center.z ())));
+
+// double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
+// double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
+
+// if (m + n < 0){
+// result = PCL_OUTSIDE_FRUSTUM;
+// break;
+// }
+
+// if (m - n < 0)
+// {
+// result = PCL_INTERSECT_FRUSTUM;
+// }
+// }
+
+// return result;
+//}
+
+//void
+//temp_viz::getModelViewPosition (Eigen::Matrix4d model_view_matrix, Eigen::Vector3d &position)
+//{
+// //Compute eye or position from model view matrix
+// Eigen::Matrix4d inverse_model_view_matrix = model_view_matrix.inverse();
+// for (int i=0; i < 3; i++)
+// {
+// position(i) = inverse_model_view_matrix(i, 3);
+// }
+//}
+
+// Lookup table of max 6 bounding box vertices, followed by number of vertices, ie {v0, v1, v2, v3, v4, v5, nv}
+//
+// 3--------2
+// /| /| Y 0 = xmin, ymin, zmin
+// / | / | | 6 = xmax, ymax. zmax
+// 7--------6 | |
+// | | | | |
+// | 0-----|--1 +------X
+// | / | / /
+// |/ |/ /
+// 4--------5 Z
+
+int hull_vertex_table[43][7] = {
+ { 0, 0, 0, 0, 0, 0, 0 }, // inside
+ { 0, 4, 7, 3, 0, 0, 4 }, // left
+ { 1, 2, 6, 5, 0, 0, 4 }, // right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 1, 5, 4, 0, 0, 4 }, // bottom
+ { 0, 1, 5, 4, 7, 3, 6 }, // bottom, left
+ { 0, 1, 2, 6, 5, 4, 6 }, // bottom, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 2, 3, 7, 6, 0, 0, 4 }, // top
+ { 4, 7, 6, 2, 3, 0, 6 }, // top, left
+ { 2, 3, 7, 6, 5, 1, 6 }, // top, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 3, 2, 1, 0, 0, 4 }, // front
+ { 0, 4, 7, 3, 2, 1, 6 }, // front, left
+ { 0, 3, 2, 6, 5, 1, 6 }, // front, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 3, 2, 1, 5, 4, 6 }, // front, bottom
+ { 2, 1, 5, 4, 7, 3, 6 }, // front, bottom, left
+ { 0, 3, 2, 6, 5, 4, 6 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 3, 7, 6, 2, 1, 6 }, // front, top
+ { 0, 4, 7, 6, 2, 1, 6 }, // front, top, left
+ { 0, 3, 7, 6, 5, 1, 6 }, // front, top, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 4, 5, 6, 7, 0, 0, 4 }, // back
+ { 4, 5, 6, 7, 3, 0, 6 }, // back, left
+ { 1, 2, 6, 7, 4, 5, 6 }, // back, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 0, 1, 5, 6, 7, 4, 6 }, // back, bottom
+ { 0, 1, 5, 6, 7, 3, 6 }, // back, bottom, left
+ { 0, 1, 2, 6, 7, 4, 6 }, // back, bottom, right
+ { 0, 0, 0, 0, 0, 0, 0 },
+ { 2, 3, 7, 4, 5, 6, 6 }, // back, top
+ { 0, 4, 5, 6, 2, 3, 6 }, // back, top, left
+ { 1, 2, 3, 7, 4, 5, 6 } // back, top, right
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+//float
+//temp_viz::viewScreenArea (
+// const Eigen::Vector3d &eye,
+// const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb,
+// const Eigen::Matrix4d &view_projection_matrix, int width, int height)
+//{
+// Eigen::Vector4d bounding_box[8];
+// bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
+// bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
+// bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
+// bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
+// bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
+// bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
+// bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
+// bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
+
+// // Compute 6-bit code to classify eye with respect to the 6 defining planes
+// int pos = ((eye.x () < bounding_box[0].x ()) ) // 1 = left
+// + ((eye.x () > bounding_box[6].x ()) << 1) // 2 = right
+// + ((eye.y () < bounding_box[0].y ()) << 2) // 4 = bottom
+// + ((eye.y () > bounding_box[6].y ()) << 3) // 8 = top
+// + ((eye.z () < bounding_box[0].z ()) << 4) // 16 = front
+// + ((eye.z () > bounding_box[6].z ()) << 5); // 32 = back
+
+// // Look up number of vertices
+// int num = hull_vertex_table[pos][6];
+// if (num == 0)
+// {
+// return (float (width * height));
+// }
+// //return 0.0;
+
+
+// // cout << "eye: " << eye.x() << " " << eye.y() << " " << eye.z() << endl;
+// // cout << "min: " << bounding_box[0].x() << " " << bounding_box[0].y() << " " << bounding_box[0].z() << endl;
+// //
+// // cout << "pos: " << pos << " ";
+// // switch(pos){
+// // case 0: cout << "inside" << endl; break;
+// // case 1: cout << "left" << endl; break;
+// // case 2: cout << "right" << endl; break;
+// // case 3:
+// // case 4: cout << "bottom" << endl; break;
+// // case 5: cout << "bottom, left" << endl; break;
+// // case 6: cout << "bottom, right" << endl; break;
+// // case 7:
+// // case 8: cout << "top" << endl; break;
+// // case 9: cout << "top, left" << endl; break;
+// // case 10: cout << "top, right" << endl; break;
+// // case 11:
+// // case 12:
+// // case 13:
+// // case 14:
+// // case 15:
+// // case 16: cout << "front" << endl; break;
+// // case 17: cout << "front, left" << endl; break;
+// // case 18: cout << "front, right" << endl; break;
+// // case 19:
+// // case 20: cout << "front, bottom" << endl; break;
+// // case 21: cout << "front, bottom, left" << endl; break;
+// // case 22:
+// // case 23:
+// // case 24: cout << "front, top" << endl; break;
+// // case 25: cout << "front, top, left" << endl; break;
+// // case 26: cout << "front, top, right" << endl; break;
+// // case 27:
+// // case 28:
+// // case 29:
+// // case 30:
+// // case 31:
+// // case 32: cout << "back" << endl; break;
+// // case 33: cout << "back, left" << endl; break;
+// // case 34: cout << "back, right" << endl; break;
+// // case 35:
+// // case 36: cout << "back, bottom" << endl; break;
+// // case 37: cout << "back, bottom, left" << endl; break;
+// // case 38: cout << "back, bottom, right" << endl; break;
+// // case 39:
+// // case 40: cout << "back, top" << endl; break;
+// // case 41: cout << "back, top, left" << endl; break;
+// // case 42: cout << "back, top, right" << endl; break;
+// // }
+
+// //return -1 if inside
+// Eigen::Vector2d dst[8];
+// for (int i = 0; i < num; i++)
+// {
+// Eigen::Vector4d world_pt = bounding_box[hull_vertex_table[pos][i]];
+// Eigen::Vector2i screen_pt = temp_viz::worldToView(world_pt, view_projection_matrix, width, height);
+// // cout << "point[" << i << "]: " << screen_pt.x() << " " << screen_pt.y() << endl;
+// dst[i] = Eigen::Vector2d(screen_pt.x (), screen_pt.y ());
+// }
+
+// double sum = 0.0;
+// for (int i = 0; i < num; ++i)
+// {
+// sum += (dst[i].x () - dst[(i+1) % num].x ()) * (dst[i].y () + dst[(i+1) % num].y ());
+// }
+
+// return (fabsf (float (sum * 0.5f)));
+//}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Camera::computeViewMatrix (Affine3d& view_mat) const
+{
+ //constructs view matrix from camera pos, view up, and the point it is looking at
+ //this code is based off of gluLookAt http://www.opengl.org/wiki/GluLookAt_code
+
+ Vec3d zAxis = normalized(focal - pos);
+ Vec3d xAxis = normalized(zAxis.cross(view_up));
+ Vec3d yAxis = xAxis.cross (zAxis);
+
+ Matx33d R;
+
+ R(0, 0) = xAxis[0]; R(0, 1) = xAxis[1]; R(0, 2) = xAxis[2];
+ R(1, 0) = yAxis[0]; R(1, 1) = yAxis[1]; R(1, 2) = yAxis[2];
+ R(1, 0) = -zAxis[0]; R(2, 1) = -zAxis[1]; R(2, 2) = -zAxis[2];
+
+ Vec3d t = R * (-pos);
+
+ view_mat = Affine3d(R, t);
+}
+
+///////////////////////////////////////////////////////////////////////
+void temp_viz::Camera::computeProjectionMatrix (Matx44d& proj) const
+{
+ double top = clip[0] * tan (0.5 * fovy);
+ double left = -(top * window_size[0]) / window_size[1];
+ double right = -left;
+ double bottom = -top;
+
+ double temp1 = 2.0 * clip[0];
+ double temp2 = 1.0 / (right - left);
+ double temp3 = 1.0 / (top - bottom);
+ double temp4 = 1.0 / clip[1] - clip[0];
+
+ proj = Matx44d::zeros();
+
+ proj(0,0) = temp1 * temp2;
+ proj(1,1) = temp1 * temp3;
+ proj(0,2) = (right + left) * temp2;
+ proj(1,2) = (top + bottom) * temp3;
+ proj(2,2) = (-clip[1] - clip[0]) * temp4;
+ proj(3,2) = -1.0;
+ proj(2,3) = (-temp1 * clip[1]) * temp4;
+}
--- /dev/null
+#include <list>
+#include <q/interactor_style.h>
+#include <vtkPolyData.h>
+#include <vtkMapper.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkPointData.h>
+#include <vtkCellArray.h>
+#include <vtkAppendPolyData.h>
+#include <vtkTextProperty.h>
+#include <vtkAbstractPicker.h>
+#include <vtkAbstractPropPicker.h>
+#include <vtkPlanes.h>
+#include <vtkPointPicker.h>
+#include <vtkMatrix4x4.h>
+#include <vtkInteractorObserver.h>
+#include <vtkCamera.h>
+
+//#include <q/visualization/vtk/vtkVertexBufferObjectMapper.h>
+
+using namespace cv;
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::Initialize ()
+{
+ modifier_ = temp_viz::InteractorStyle::KB_MOD_ALT;
+ // Set windows size (width, height) to unknown (-1)
+ win_size_ = Vec2i(-1, -1);
+ win_pos_ = Vec2i(0, 0);
+ max_win_size_ = Vec2i(-1, -1);
+
+ // Create the image filter and PNG writer objects
+ wif_ = vtkSmartPointer<vtkWindowToImageFilter>::New ();
+ snapshot_writer_ = vtkSmartPointer<vtkPNGWriter>::New ();
+ snapshot_writer_->SetInputConnection (wif_->GetOutputPort ());
+
+ init_ = true;
+ stereo_anaglyph_mask_default_ = true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::saveScreenshot (const std::string &file)
+{
+ FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+ wif_->SetInput (Interactor->GetRenderWindow ());
+ wif_->Modified (); // Update the WindowToImageFilter
+ snapshot_writer_->Modified ();
+ snapshot_writer_->SetFileName (file.c_str ());
+ snapshot_writer_->Write ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::zoomIn ()
+{
+ FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+ // Zoom in
+ StartDolly ();
+ double factor = 10.0 * 0.2 * .5;
+ Dolly (pow (1.1, factor));
+ EndDolly ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::zoomOut ()
+{
+ FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+ // Zoom out
+ StartDolly ();
+ double factor = 10.0 * -0.2 * .5;
+ Dolly (pow (1.1, factor));
+ EndDolly ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnChar ()
+{
+ // Make sure we ignore the same events we handle in OnKeyDown to avoid calling things twice
+ FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+ if (Interactor->GetKeyCode () >= '0' && Interactor->GetKeyCode () <= '9')
+ return;
+ std::string key (Interactor->GetKeySym ());
+ if (key.find ("XF86ZoomIn") != std::string::npos)
+ zoomIn ();
+ else if (key.find ("XF86ZoomOut") != std::string::npos)
+ zoomOut ();
+
+ bool keymod = false;
+ switch (modifier_)
+ {
+ case KB_MOD_ALT:
+ {
+ keymod = Interactor->GetAltKey ();
+ break;
+ }
+ case KB_MOD_CTRL:
+ {
+ keymod = Interactor->GetControlKey ();
+ break;
+ }
+ case KB_MOD_SHIFT:
+ {
+ keymod = Interactor->GetShiftKey ();
+ break;
+ }
+ }
+
+ switch (Interactor->GetKeyCode ())
+ {
+ // All of the options below simply exit
+ case 'h': case 'H':
+ case 'l': case 'L':
+ case 'p': case 'P':
+ case 'j': case 'J':
+ case 'c': case 'C':
+ case 43: // KEY_PLUS
+ case 45: // KEY_MINUS
+ case 'f': case 'F':
+ case 'g': case 'G':
+ case 'o': case 'O':
+ case 'u': case 'U':
+ case 'q': case 'Q':
+ {
+ break;
+ }
+ // S and R have a special !ALT case
+ case 'r': case 'R':
+ case 's': case 'S':
+ {
+ if (!keymod)
+ Superclass::OnChar ();
+ break;
+ }
+ default:
+ {
+ Superclass::OnChar ();
+ break;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::InteractorStyle::registerMouseCallback (boost::function<void (const cv::MouseEvent&)> callback)
+{
+ return (mouse_signal_.connect (callback));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::InteractorStyle::registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> callback)
+{
+ return (keyboard_signal_.connect (callback));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+temp_viz::InteractorStyle::OnKeyDown ()
+{
+ if (!init_)
+ {
+ std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
+ return;
+ }
+
+ if (!renderer_)
+ {
+ std::cout << "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl;
+ return;
+ }
+
+ FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]);
+
+ if (wif_->GetInput () == NULL)
+ {
+ wif_->SetInput (Interactor->GetRenderWindow ());
+ wif_->Modified ();
+ snapshot_writer_->Modified ();
+ }
+
+ // Save the initial windows width/height
+ if (win_size_[0] == -1 || win_size_[1] == -1)
+ win_size_ = Vec2i(Interactor->GetRenderWindow ()->GetSize ());
+
+
+ // Get the status of special keys (Cltr+Alt+Shift)
+ bool shift = Interactor->GetShiftKey ();
+ bool ctrl = Interactor->GetControlKey ();
+ bool alt = Interactor->GetAltKey ();
+
+ bool keymod = false;
+ switch (modifier_)
+ {
+ case KB_MOD_ALT:
+ {
+ keymod = alt;
+ break;
+ }
+ case KB_MOD_CTRL:
+ {
+ keymod = ctrl;
+ break;
+ }
+ case KB_MOD_SHIFT:
+ {
+ keymod = shift;
+ break;
+ }
+ }
+
+ std::string key (Interactor->GetKeySym ());
+ if (key.find ("XF86ZoomIn") != std::string::npos)
+ zoomIn ();
+ else if (key.find ("XF86ZoomOut") != std::string::npos)
+ zoomOut ();
+
+ switch (Interactor->GetKeyCode ())
+ {
+ case 'h': case 'H':
+ {
+ std::cout << "| Help:\n"
+ "-------\n"
+ " p, P : switch to a point-based representation\n"
+ " w, W : switch to a wireframe-based representation (where available)\n"
+ " s, S : switch to a surface-based representation (where available)\n"
+ "\n"
+ " j, J : take a .PNG snapshot of the current window view\n"
+ " c, C : display current camera/window parameters\n"
+ " f, F : fly to point mode\n"
+ "\n"
+ " e, E : exit the interactor\n"
+ " q, Q : stop and call VTK's TerminateApp\n"
+ "\n"
+ " +/- : increment/decrement overall point size\n"
+ " +/- [+ ALT] : zoom in/out \n"
+ "\n"
+ " r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]\n"
+ "\n"
+ " ALT + s, S : turn stereo mode on/off\n"
+ " ALT + f, F : switch between maximized window mode and original size\n"
+ "\n"
+ " SHIFT + left click : select a point\n"
+ << std::endl;
+ break;
+ }
+
+ // Switch representation to points
+ case 'p': case 'P':
+ {
+ vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+ vtkCollectionSimpleIterator ait;
+ for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+ {
+ for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+ {
+ vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ());
+ apart->GetProperty ()->SetRepresentationToPoints ();
+ }
+ }
+ break;
+ }
+ // Save a PNG snapshot with the current screen
+ case 'j': case 'J':
+ {
+ char cam_fn[80], snapshot_fn[80];
+ unsigned t = static_cast<unsigned> (time (0));
+ sprintf (snapshot_fn, "screenshot-%d.png" , t);
+ saveScreenshot (snapshot_fn);
+
+ sprintf (cam_fn, "screenshot-%d.cam", t);
+ ofstream ofs_cam;
+ ofs_cam.open (cam_fn);
+ vtkSmartPointer<vtkCamera> cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
+ double clip[2], focal[3], pos[3], view[3];
+ cam->GetClippingRange (clip);
+ cam->GetFocalPoint (focal);
+ cam->GetPosition (pos);
+ cam->GetViewUp (view);
+#ifndef M_PI
+ # define M_PI 3.14159265358979323846 // pi
+#endif
+
+ int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
+ int *win_size = Interactor->GetRenderWindow ()->GetSize ();
+ ofs_cam << clip[0] << "," << clip[1] << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
+ pos[0] << "," << pos[1] << "," << pos[2] << "/" << view[0] << "," << view[1] << "," << view[2] << "/" <<
+ cam->GetViewAngle () / 180.0 * M_PI << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
+ << endl;
+ ofs_cam.close ();
+
+ std::cout << "Screenshot (" << snapshot_fn << ") and camera information (" << cam_fn << ") successfully captured." << std::endl;
+ break;
+ }
+ // display current camera settings/parameters
+ case 'c': case 'C':
+ {
+ vtkSmartPointer<vtkCamera> cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
+ double clip[2], focal[3], pos[3], view[3];
+ cam->GetClippingRange (clip);
+ cam->GetFocalPoint (focal);
+ cam->GetPosition (pos);
+ cam->GetViewUp (view);
+ int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
+ int *win_size = Interactor->GetRenderWindow ()->GetSize ();
+ std::cerr << clip[0] << "," << clip[1] << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
+ pos[0] << "," << pos[1] << "," << pos[2] << "/" << view[0] << "," << view[1] << "," << view[2] << "/" <<
+ cam->GetViewAngle () / 180.0 * M_PI << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
+ << endl;
+ break;
+ }
+ case '=':
+ {
+ zoomIn();
+ break;
+ }
+ case 43: // KEY_PLUS
+ {
+ if(alt)
+ zoomIn ();
+ else
+ {
+ vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+ vtkCollectionSimpleIterator ait;
+ for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+ {
+ for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+ {
+ vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ());
+ float psize = apart->GetProperty ()->GetPointSize ();
+ if (psize < 63.0f)
+ apart->GetProperty ()->SetPointSize (psize + 1.0f);
+ }
+ }
+ }
+ break;
+ }
+ case 45: // KEY_MINUS
+ {
+ if(alt)
+ zoomOut ();
+ else
+ {
+ vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+ vtkCollectionSimpleIterator ait;
+ for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+ {
+ for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+ {
+ vtkSmartPointer<vtkActor> apart = static_cast<vtkActor*> (path->GetLastNode ()->GetViewProp ());
+ float psize = apart->GetProperty ()->GetPointSize ();
+ if (psize > 1.0f)
+ apart->GetProperty ()->SetPointSize (psize - 1.0f);
+ }
+ }
+ }
+ break;
+ }
+ // Switch between maximize and original window size
+ case 'f': case 'F':
+ {
+ if (keymod)
+ {
+ Vec2i screen_size(Interactor->GetRenderWindow ()->GetScreenSize ());
+ Vec2i win_size(Interactor->GetRenderWindow ()->GetSize ());
+
+ // Is window size = max?
+ if (win_size == max_win_size_)
+ {
+ Interactor->GetRenderWindow ()->SetSize (win_size_.val);
+ Interactor->GetRenderWindow ()->SetPosition (win_pos_.val);
+ Interactor->GetRenderWindow ()->Render ();
+ Interactor->Render ();
+ }
+ // Set to max
+ else
+ {
+ win_pos_ = Vec2i(Interactor->GetRenderWindow ()->GetPosition ());
+ win_size_ = win_size;
+
+ Interactor->GetRenderWindow ()->SetSize (screen_size.val);
+ Interactor->GetRenderWindow ()->Render ();
+ Interactor->Render ();
+ max_win_size_ = Vec2i(Interactor->GetRenderWindow ()->GetSize ());
+ }
+ }
+ else
+ {
+ AnimState = VTKIS_ANIM_ON;
+ vtkAssemblyPath *path = NULL;
+ Interactor->GetPicker ()->Pick (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1], 0.0, CurrentRenderer);
+ vtkAbstractPropPicker *picker;
+ if ((picker = vtkAbstractPropPicker::SafeDownCast (Interactor->GetPicker ())))
+ path = picker->GetPath ();
+ if (path != NULL)
+ Interactor->FlyTo (CurrentRenderer, picker->GetPickPosition ());
+ AnimState = VTKIS_ANIM_OFF;
+ }
+ break;
+ }
+ // 's'/'S' w/out ALT
+ case 's': case 'S':
+ {
+ if (keymod)
+ {
+ int stereo_render = Interactor->GetRenderWindow ()->GetStereoRender ();
+ if (!stereo_render)
+ {
+ if (stereo_anaglyph_mask_default_)
+ {
+ Interactor->GetRenderWindow ()->SetAnaglyphColorMask (4, 3);
+ stereo_anaglyph_mask_default_ = false;
+ }
+ else
+ {
+ Interactor->GetRenderWindow ()->SetAnaglyphColorMask (2, 5);
+ stereo_anaglyph_mask_default_ = true;
+ }
+ }
+ Interactor->GetRenderWindow ()->SetStereoRender (!stereo_render);
+ Interactor->GetRenderWindow ()->Render ();
+ Interactor->Render ();
+ }
+ else
+ Superclass::OnKeyDown ();
+ break;
+ }
+
+ case 'o': case 'O':
+ {
+ vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+ int flag = cam->GetParallelProjection ();
+ cam->SetParallelProjection (!flag);
+
+ CurrentRenderer->SetActiveCamera (cam);
+ CurrentRenderer->Render ();
+ break;
+ }
+
+ // Overwrite the camera reset
+ case 'r': case 'R':
+ {
+ if (!keymod)
+ {
+ Superclass::OnKeyDown ();
+ break;
+ }
+
+ vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+
+ static CloudActorMap::iterator it = actors_->begin ();
+ // it might be that some actors don't have a valid transformation set -> we skip them to avoid a seg fault.
+ bool found_transformation = false;
+
+ for (size_t idx = 0; idx < actors_->size (); ++idx, ++it)
+ {
+ if (it == actors_->end ())
+ it = actors_->begin ();
+
+ const CloudActor& actor = it->second;
+ if (actor.viewpoint_transformation_.GetPointer ())
+ {
+ found_transformation = true;
+ break;
+ }
+ }
+
+ // if a valid transformation was found, use it otherwise fall back to default view point.
+ if (found_transformation)
+ {
+ const CloudActor& actor = it->second;
+ cam->SetPosition (actor.viewpoint_transformation_->GetElement (0, 3),
+ actor.viewpoint_transformation_->GetElement (1, 3),
+ actor.viewpoint_transformation_->GetElement (2, 3));
+
+ cam->SetFocalPoint (actor.viewpoint_transformation_->GetElement (0, 3) - actor.viewpoint_transformation_->GetElement (0, 2),
+ actor.viewpoint_transformation_->GetElement (1, 3) - actor.viewpoint_transformation_->GetElement (1, 2),
+ actor.viewpoint_transformation_->GetElement (2, 3) - actor.viewpoint_transformation_->GetElement (2, 2));
+
+ cam->SetViewUp (actor.viewpoint_transformation_->GetElement (0, 1),
+ actor.viewpoint_transformation_->GetElement (1, 1),
+ actor.viewpoint_transformation_->GetElement (2, 1));
+ }
+ else
+ {
+ cam->SetPosition (0, 0, 0);
+ cam->SetFocalPoint (0, 0, 1);
+ cam->SetViewUp (0, -1, 0);
+ }
+
+ // go to the next actor for the next key-press event.
+ if (it != actors_->end ())
+ ++it;
+ else
+ it = actors_->begin ();
+
+ CurrentRenderer->SetActiveCamera (cam);
+ CurrentRenderer->ResetCameraClippingRange ();
+ CurrentRenderer->Render ();
+ break;
+ }
+
+ case 'q': case 'Q':
+ {
+ Interactor->ExitCallback ();
+ return;
+ }
+ default:
+ {
+ Superclass::OnKeyDown ();
+ break;
+ }
+ }
+
+ KeyboardEvent event (true, Interactor->GetKeySym (), Interactor->GetKeyCode (), Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ keyboard_signal_ (event);
+
+ renderer_->Render ();
+ Interactor->Render ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnKeyUp ()
+{
+ KeyboardEvent event (false, Interactor->GetKeySym (), Interactor->GetKeyCode (), Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ keyboard_signal_ (event);
+ Superclass::OnKeyUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseMove ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseMove, MouseEvent::NoButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnMouseMove ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnLeftButtonDown ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+ MouseEvent event (type, MouseEvent::LeftButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnLeftButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnLeftButtonUp ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::LeftButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnLeftButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMiddleButtonDown ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+
+ MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+ MouseEvent event (type, MouseEvent::MiddleButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnMiddleButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMiddleButtonUp ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::MiddleButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnMiddleButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnRightButtonDown ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+
+ MouseEvent::Type type = (Interactor->GetRepeatCount() == 0) ? MouseEvent::MouseButtonPress : MouseEvent::MouseDblClick;
+ MouseEvent event (type, MouseEvent::RightButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnRightButtonDown ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnRightButtonUp ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseButtonRelease, MouseEvent::RightButton, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ Superclass::OnRightButtonUp ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseWheelForward ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseScrollUp, MouseEvent::VScroll, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ if (Interactor->GetRepeatCount ())
+ mouse_signal_ (event);
+
+ if (Interactor->GetAltKey ())
+ {
+ // zoom
+ vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+ double opening_angle = cam->GetViewAngle ();
+ if (opening_angle > 15.0)
+ opening_angle -= 1.0;
+
+ cam->SetViewAngle (opening_angle);
+ cam->Modified ();
+ CurrentRenderer->SetActiveCamera (cam);
+ CurrentRenderer->ResetCameraClippingRange ();
+ CurrentRenderer->Modified ();
+ CurrentRenderer->Render ();
+ renderer_->Render ();
+ Interactor->Render ();
+ }
+ else
+ Superclass::OnMouseWheelForward ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnMouseWheelBackward ()
+{
+ Vec2i p(Interactor->GetEventPosition());
+ MouseEvent event (MouseEvent::MouseScrollDown, MouseEvent::VScroll, p, Interactor->GetAltKey (), Interactor->GetControlKey (), Interactor->GetShiftKey ());
+ mouse_signal_ (event);
+ if (Interactor->GetRepeatCount ())
+ mouse_signal_ (event);
+
+ if (Interactor->GetAltKey ())
+ {
+ // zoom
+ vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+ double opening_angle = cam->GetViewAngle ();
+ if (opening_angle < 170.0)
+ opening_angle += 1.0;
+
+ cam->SetViewAngle (opening_angle);
+ cam->Modified ();
+ CurrentRenderer->SetActiveCamera (cam);
+ CurrentRenderer->ResetCameraClippingRange ();
+ CurrentRenderer->Modified ();
+ CurrentRenderer->Render ();
+ renderer_->Render ();
+ Interactor->Render ();
+ }
+ else
+ Superclass::OnMouseWheelBackward ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::InteractorStyle::OnTimer ()
+{
+ if (!init_)
+ {
+ std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing.\n" << std::endl;
+ return;
+ }
+
+ if (!renderer_)
+ {
+ std::cout << "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl;
+ return;
+ }
+ renderer_->Render ();
+ Interactor->Render ();
+}
+
+
+namespace temp_viz
+{
+ // Standard VTK macro for *New ()
+ vtkStandardNewMacro (InteractorStyle);
+
+}
+
--- /dev/null
+#include <opencv2/viz/mesh_load.hpp>
+
+#include "precomp.hpp"
+
+#include <vtkPLYReader.h>
+#include <vtkSmartPointer.h>
+#include <vtkPolyData.h>
+#include <vtkPointData.h>
+#include <vtkCellArray.h>
+
+temp_viz::Mesh3d::Ptr temp_viz::mesh_load(const String& file)
+{
+ Mesh3d::Ptr mesh = new Mesh3d();
+
+ vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New();
+ reader->SetFileName(file.c_str());
+ reader->Update();
+ vtkSmartPointer<vtkPolyData> poly_data = reader->GetOutput ();
+
+ typedef unsigned int uint32_t;
+ mesh->polygons.clear();
+
+ vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
+ vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
+ vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
+
+ mesh->cloud.create(1, nr_points, CV_32FC3);
+
+ double point_xyz[3];
+ for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
+ {
+ mesh_points->GetPoint (i, &point_xyz[0]);
+ mesh->cloud.ptr<cv::Point3f>()[i] = cv::Point3d(point_xyz[0], point_xyz[1], point_xyz[2]);;
+ }
+
+ // Then the color information, if any
+ vtkUnsignedCharArray* poly_colors = NULL;
+ if (poly_data->GetPointData() != NULL)
+ poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
+
+ // some applications do not save the name of scalars (including PCL's native vtk_io)
+ if (!poly_colors && poly_data->GetPointData () != NULL)
+ poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
+
+ if (!poly_colors && poly_data->GetPointData () != NULL)
+ poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
+
+ // TODO: currently only handles rgb values with 3 components
+ if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
+ {
+ mesh->colors.create(1, nr_points, CV_8UC3);
+ unsigned char point_color[3];
+
+ for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
+ {
+ poly_colors->GetTupleValue (i, &point_color[0]);
+
+ //RGB or BGR?????
+ mesh->colors.ptr<cv::Vec3b>()[i] = cv::Vec3b(point_color[0], point_color[1], point_color[2]);
+ }
+ }
+ else
+ mesh->colors.release();
+
+ // Now handle the polygons
+ mesh->polygons.resize (nr_polygons);
+ vtkIdType* cell_points;
+ vtkIdType nr_cell_points;
+ vtkCellArray * mesh_polygons = poly_data->GetPolys ();
+ mesh_polygons->InitTraversal ();
+ int id_poly = 0;
+ while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
+ {
+ mesh->polygons[id_poly].vertices.resize (nr_cell_points);
+ for (int i = 0; i < nr_cell_points; ++i)
+ mesh->polygons[id_poly].vertices[i] = static_cast<int> (cell_points[i]);
+ ++id_poly;
+ }
+
+ return mesh;
+}
--- /dev/null
+#include "precomp.hpp"
\ No newline at end of file
--- /dev/null
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <map>
+#include <vector>
+
+
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/signals2.hpp>
+#include <boost/thread.hpp>
+
+#include <Eigen/Geometry>
+
+#if defined __GNUC__
+#pragma GCC system_header
+#ifdef __DEPRECATED
+#undef __DEPRECATED
+#define __DEPRECATED_DISABLED__
+#endif
+#endif
+
+#include <vtkAppendPolyData.h>
+#include <vtkAssemblyPath.h>
+#include <vtkAxesActor.h>
+#include <vtkActor.h>
+#include <vtkBoxRepresentation.h>
+#include <vtkBoxWidget.h>
+#include <vtkBoxWidget2.h>
+#include <vtkCellData.h>
+#include <vtkMath.h>
+#include <vtkLoopSubdivisionFilter.h>
+#include <vtkLineSource.h>
+#include <vtkLegendScaleActor.h>
+#include <vtkLightKit.h>
+#include <vtkPlatonicSolidSource.h>
+#include <vtkPropPicker.h>
+#include <vtkGeneralTransform.h>
+#include <vtkSmartPointer.h>
+#include <vtkDataSet.h>
+#include <vtkDataSetSurfaceFilter.h>
+#include <vtkExecutive.h>
+#include <vtkPolygon.h>
+#include <vtkPointPicker.h>
+#include <vtkUnstructuredGrid.h>
+#include <vtkConeSource.h>
+#include <vtkDiskSource.h>
+#include <vtkPlaneSource.h>
+#include <vtkSphereSource.h>
+#include <vtkIdentityTransform.h>
+#include <vtkTransform.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkTubeFilter.h>
+#include <vtkCubeSource.h>
+#include <vtkAxes.h>
+#include <vtkFloatArray.h>
+#include <vtkPointData.h>
+#include <vtkPolyData.h>
+#include <vtkPolyDataReader.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkDataSetMapper.h>
+#include <vtkCellArray.h>
+#include <vtkCommand.h>
+#include <vtkCellLocator.h>
+#include <vtkPLYReader.h>
+#include <vtkTransformFilter.h>
+#include <vtkPolyLine.h>
+#include <vtkVectorText.h>
+#include <vtkFollower.h>
+#include <vtkCallbackCommand.h>
+#include <vtkInteractorStyle.h>
+#include <vtkInformationVector.h>
+#include <vtkDataArray.h>
+#include <vtkUnsignedCharArray.h>
+#include <vtkPoints.h>
+#include <vtkRendererCollection.h>
+#include <vtkPNGWriter.h>
+#include <vtkWindowToImageFilter.h>
+#include <vtkInteractorStyleTrackballCamera.h>
+#include <vtkProperty.h>
+#include <vtkCamera.h>
+#include <vtkObjectFactory.h>
+#include <vtkScalarBarActor.h>
+#include <vtkScalarsToColors.h>
+#include <vtkClipPolyData.h>
+#include <vtkPlanes.h>
+#include <vtkImageImport.h>
+#include <vtkImageViewer.h>
+#include <vtkInteractorStyleImage.h>
+#include <vtkImageFlip.h>
+#include <vtkTIFFWriter.h>
+#include <vtkBMPWriter.h>
+#include <vtkJPEGWriter.h>
+#include <vtkImageViewer2.h>
+#include <vtkRenderWindow.h>
+#include <vtkXYPlotActor.h>
+#include <vtkTextProperty.h>
+#include <vtkProperty2D.h>
+#include <vtkFieldData.h>
+#include <vtkDoubleArray.h>
+#include <vtkLODActor.h>
+#include <vtkPolyDataWriter.h>
+#include <vtkTextActor.h>
+#include <vtkCleanPolyData.h>
+#include <vtkRenderer.h>
+#include <vtkObject.h>
+#include <vtkOrientationMarkerWidget.h>
+#include <vtkImageReslice.h>
+#include <vtkImageChangeInformation.h>
+#include <vtkImageCanvasSource2D.h>
+#include <vtkImageBlend.h>
+#include <vtkImageStencilData.h>
+
+#include <vtkRenderWindowInteractor.h>
+#include <vtkChartXY.h>
+#include <vtkPlot.h>
+#include <vtkTable.h>
+#include <vtkContextView.h>
+#include <vtkContextScene.h>
+#include <vtkColorSeries.h>
+#include <vtkAxis.h>
+#include <vtkSelection.h>
+#include <vtkHardwareSelector.h>
+#include <vtkTriangle.h>
+#include <vtkWorldPointPicker.h>
+#include <vtkInteractorStyleRubberBandPick.h>
+#include <vtkInteractorStyleTrackballActor.h>
+#include <vtkAreaPicker.h>
+#include <vtkExtractGeometry.h>
+#include <vtkExtractPolyDataGeometry.h>
+#include <vtkVertexGlyphFilter.h>
+#include <vtkIdFilter.h>
+#include <vtkIdTypeArray.h>
+#include <vtkImageReader2Factory.h>
+#include <vtkImageReader2.h>
+#include <vtkImageData.h>
+
+
+
+#include <vtkPolyDataNormals.h>
+#include <vtkMapper.h>
+#include <vtkSelectionNode.h>
+
+
+
+#include <vtkAbstractPicker.h>
+#include <vtkAbstractPropPicker.h>
+#include <vtkPointPicker.h>
+#include <vtkMatrix4x4.h>
+#include <vtkInteractorObserver.h>
+
+
+
+#if defined __GNUC__ && defined __DEPRECATED_DISABLED__
+#define __DEPRECATED
+#undef __DEPRECATED_DISABLED__
+#endif
--- /dev/null
+#pragma once
+
+#include <opencv2/core/cvdef.h>
+#include <opencv2/core.hpp>
+#include <opencv2/viz/types.hpp>
+//#include <vtkMatrix4x4.h>
+
+namespace temp_viz
+{
+ //CV_EXPORTS Eigen::Matrix4d vtkToEigen (vtkMatrix4x4* vtk_matrix);
+ //CV_EXPORTS Eigen::Vector2i worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height);
+ //CV_EXPORTS void getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24]);
+
+// enum FrustumCull
+// {
+// PCL_INSIDE_FRUSTUM,
+// PCL_INTERSECT_FRUSTUM,
+// PCL_OUTSIDE_FRUSTUM
+// };
+
+ //CV_EXPORTS int cullFrustum (double planes[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb);
+ //CV_EXPORTS float viewScreenArea (const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height);
+
+ enum RenderingProperties
+ {
+ VIZ_POINT_SIZE,
+ VIZ_OPACITY,
+ VIZ_LINE_WIDTH,
+ VIZ_FONT_SIZE,
+ VIZ_COLOR,
+ VIZ_REPRESENTATION,
+ VIZ_IMMEDIATE_RENDERING,
+ VIZ_SHADING
+ };
+
+ enum RenderingRepresentationProperties
+ {
+ REPRESENTATION_POINTS,
+ REPRESENTATION_WIREFRAME,
+ REPRESENTATION_SURFACE
+ };
+
+ enum ShadingRepresentationProperties
+ {
+ SHADING_FLAT,
+ SHADING_GOURAUD,
+ SHADING_PHONG
+ };
+
+ class CV_EXPORTS Camera
+ {
+ public:
+ /** Focal point or lookAt. The view direction can be obtained by (focal-pos).normalized () */
+ Vec3d focal;
+
+ /** \brief Position of the camera. */
+ Vec3d pos;
+
+ /** \brief Up vector of the camera. */
+ Vec3d view_up;
+
+ /** \brief Near/far clipping planes depths */
+ Vec2d clip;
+
+ /** \brief Field of view angle in y direction (radians). */
+ double fovy;
+
+ // the following variables are the actual position and size of the window on the screen and NOT the viewport!
+ // except for the size, which is the same the viewport is assumed to be centered and same size as the window.
+ Vec2i window_size;
+ Vec2i window_pos;
+
+ /** \brief Computes View matrix for Camera (Based on gluLookAt)
+ * \param[out] view_mat the resultant matrix
+ */
+ void computeViewMatrix(Affine3d& view_mat) const;
+
+ /** \brief Computes Projection Matrix for Camera
+ * \param[out] proj the resultant matrix
+ */
+ void computeProjectionMatrix(Matx44d& proj) const;
+
+ /** \brief converts point to window coordiantes
+ * \param[in] pt xyz point to be converted
+ * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+ *
+ * This function computes the projection and view matrix every time.
+ * It is very inefficient to use this for every point in the point cloud!
+ */
+ void cvtWindowCoordinates (const cv::Point3f& pt, Vec4d& window_cord) const
+ {
+ Affine3d view;
+ computeViewMatrix (view);
+
+ Matx44d proj;
+ computeProjectionMatrix (proj);
+ cvtWindowCoordinates (pt, proj * view.matrix, window_cord);
+ return;
+ }
+
+ /** \brief converts point to window coordiantes
+ * \param[in] pt xyz point to be converted
+ * \param[out] window_cord vector containing the pts' window X,Y, Z and 1
+ * \param[in] composite_mat composite transformation matrix (proj*view)
+ *
+ * Use this function to compute window coordinates with a precomputed
+ * transformation function. The typical composite matrix will be
+ * the projection matrix * the view matrix. However, additional
+ * matrices like a camera disortion matrix can also be added.
+ */
+ void cvtWindowCoordinates (const Point3f& pt, Matx44d& composite_mat, Vec4d& window_cord) const
+ {
+ Vec4d pte (pt.x, pt.y, pt.z, 1);
+ window_cord = composite_mat * pte;
+ window_cord = window_cord/window_cord[3];
+
+ window_cord[0] = (window_cord[0]+1.0) / 2.0*window_size[0];
+ window_cord[1] = (window_cord[1]+1.0) / 2.0*window_size[1];
+ window_cord[2] = (window_cord[2]+1.0) / 2.0;
+ }
+ };
+
+}
--- /dev/null
+#pragma once
+
+#include <q/viz_types.h>
+#include <opencv2/viz/events.hpp>
+
+namespace temp_viz
+{
+ /** \brief PCLVisualizerInteractorStyle defines an unique, custom VTK
+ * based interactory style for PCL Visualizer applications. Besides
+ * defining the rendering style, we also create a list of custom actions
+ * that are triggered on different keys being pressed:
+ *
+ * - p, P : switch to a point-based representation
+ * - w, W : switch to a wireframe-based representation (where available)
+ * - s, S : switch to a surface-based representation (where available)
+ * - j, J : take a .PNG snapshot of the current window view
+ * - c, C : display current camera/window parameters
+ * - f, F : fly to point mode
+ * - e, E : exit the interactor\
+ * - q, Q : stop and call VTK's TerminateApp
+ * - + / - : increment/decrement overall point size
+ * - r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]
+ * - ALT + s, S : turn stereo mode on/off
+ * - ALT + f, F : switch between maximized window mode and original size
+ * -
+ * - SHIFT + left click : select a point
+ *
+ * \author Radu B. Rusu
+ * \ingroup visualization
+ */
+ class CV_EXPORTS InteractorStyle : public vtkInteractorStyleTrackballCamera
+ {
+ public:
+
+ enum KeyboardModifier
+ {
+ KB_MOD_ALT,
+ KB_MOD_CTRL,
+ KB_MOD_SHIFT
+ };
+
+ static InteractorStyle *New ();
+
+
+ InteractorStyle () {}
+ virtual ~InteractorStyle () {}
+
+ // this macro defines Superclass, the isA functionality and the safe downcast method
+ vtkTypeMacro (InteractorStyle, vtkInteractorStyleTrackballCamera);
+
+ /** \brief Initialization routine. Must be called before anything else. */
+ virtual void Initialize ();
+
+ /** \brief Pass a pointer to the actor map
+ * \param[in] actors the actor map that will be used with this style
+ */
+ inline void setCloudActorMap (const cv::Ptr<CloudActorMap>& actors) { actors_ = actors; }
+
+
+ /** \brief Pass a set of renderers to the interactor style.
+ * \param[in] rens the vtkRendererCollection to use
+ */
+ void setRenderer (vtkSmartPointer<vtkRenderer>& ren) { renderer_ = ren; }
+
+ /** \brief Register a callback function for mouse events
+ * \param[in] cb a boost function that will be registered as a callback for a mouse event
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ boost::signals2::connection registerMouseCallback (boost::function<void (const cv::MouseEvent&)> cb);
+
+ /** \brief Register a callback boost::function for keyboard events
+ * \param[in] cb a boost function that will be registered as a callback for a keyboard event
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ boost::signals2::connection registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> cb);
+
+
+ /** \brief Save the current rendered image to disk, as a PNG screenshot.
+ * \param[in] file the name of the PNG file
+ */
+ void saveScreenshot (const std::string &file);
+
+ /** \brief Change the default keyboard modified from ALT to a different special key.
+ * Allowed values are:
+ * - KB_MOD_ALT
+ * - KB_MOD_CTRL
+ * - KB_MOD_SHIFT
+ * \param[in] modifier the new keyboard modifier
+ */
+ inline void setKeyboardModifier (const KeyboardModifier &modifier) { modifier_ = modifier; }
+ protected:
+ /** \brief Set to true after initialization is complete. */
+ bool init_;
+
+ /** \brief Collection of vtkRenderers stored internally. */
+ //vtkSmartPointer<vtkRendererCollection> rens_;
+ vtkSmartPointer<vtkRenderer> renderer_;
+
+ /** \brief Actor map stored internally. */
+ cv::Ptr<CloudActorMap> actors_;
+
+ /** \brief The current window width/height. */
+ Vec2i win_size_;
+
+ /** \brief The current window position x/y. */
+ Vec2i win_pos_;
+
+ /** \brief The maximum resizeable window width/height. */
+ Vec2i max_win_size_;
+
+ /** \brief A PNG writer for screenshot captures. */
+ vtkSmartPointer<vtkPNGWriter> snapshot_writer_;
+ /** \brief Internal window to image filter. Needed by \a snapshot_writer_. */
+ vtkSmartPointer<vtkWindowToImageFilter> wif_;
+
+ boost::signals2::signal<void (const cv::MouseEvent&)> mouse_signal_;
+ boost::signals2::signal<void (const cv::KeyboardEvent&)> keyboard_signal_;
+
+ /** \brief Interactor style internal method. Gets called whenever a key is pressed. */
+ virtual void OnChar ();
+
+ // Keyboard events
+ virtual void OnKeyDown ();
+ virtual void OnKeyUp ();
+
+ // mouse button events
+ virtual void OnMouseMove ();
+ virtual void OnLeftButtonDown ();
+ virtual void OnLeftButtonUp ();
+ virtual void OnMiddleButtonDown ();
+ virtual void OnMiddleButtonUp ();
+ virtual void OnRightButtonDown ();
+ virtual void OnRightButtonUp ();
+ virtual void OnMouseWheelForward ();
+ virtual void OnMouseWheelBackward ();
+
+ /** \brief Interactor style internal method. Gets called periodically if a timer is set. */
+ virtual void OnTimer ();
+
+
+ void zoomIn ();
+ void zoomOut ();
+
+ /** \brief True if we're using red-blue colors for anaglyphic stereo, false if magenta-green. */
+ bool stereo_anaglyph_mask_default_;
+
+ /** \brief The keyboard modifier to use. Default: Alt. */
+ KeyboardModifier modifier_;
+ };
+}
--- /dev/null
+#pragma once
+
+#include <Eigen/Core>
+#include <opencv2/viz/types.hpp>
+#include "precomp.hpp"
+
+namespace temp_viz
+{
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f ¢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<vtkDataSet> data = temp_viz::createCylinder (cylinder_coeff, numsides);
+ * \endcode
+ *
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides = 30);
+
+
+ /** \brief Create a planar shape from a set of model coefficients.
+ * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+ *
+ * \code
+ * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelPlane)
+ * // Eigen::Vector4f plane_parameters;
+ *
+ * temp_viz::ModelCoefficients plane_coeff;
+ * plane_coeff.values.resize (4); // We need 4 values
+ * plane_coeff.values[0] = plane_parameters.x ();
+ * plane_coeff.values[1] = plane_parameters.y ();
+ * plane_coeff.values[2] = plane_parameters.z ();
+ * plane_coeff.values[3] = plane_parameters.w ();
+ *
+ * vtkSmartPointer<vtkDataSet> data = temp_viz::createPlane (plane_coeff);
+ * \endcode
+ *
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients);
+
+ /** \brief Create a planar shape from a set of model coefficients.
+ * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+ * \param[in] x,y,z projection of this point on the plane is used to get the center of the plane.
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z);
+
+ /** \brief Create a 2d circle shape from a set of model coefficients.
+ * \param[in] coefficients the model coefficients (x, y, radius)
+ * \param[in] z (optional) specify a z value (default: 0)
+ *
+ * \code
+ * // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCircle2D)
+ * // float x, y, radius;
+ *
+ * temp_viz::ModelCoefficients circle_coeff;
+ * circle_coeff.values.resize (3); // We need 3 values
+ * circle_coeff.values[0] = x;
+ * circle_coeff.values[1] = y;
+ * circle_coeff.values[2] = radius;
+ *
+ * vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
+ * \endcode
+ *
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z = 0.0);
+
+
+ /** \brief Creaet a cube shape from a set of model coefficients.
+ * \param[in] coefficients the cube coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const temp_viz::ModelCoefficients &coefficients);
+
+ /** \brief Creaet a cube shape from a set of model coefficients.
+ *
+ * \param[in] translation a translation to apply to the cube from 0,0,0
+ * \param[in] rotation a quaternion-based rotation to apply to the cube
+ * \param[in] width the cube's width
+ * \param[in] height the cube's height
+ * \param[in] depth the cube's depth
+ * \ingroup visualization
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth);
+
+ /** \brief Create a cube from a set of bounding points
+ * \param[in] x_min is the minimum x value of the box
+ * \param[in] x_max is the maximum x value of the box
+ * \param[in] y_min is the minimum y value of the box
+ * \param[in] y_max is the maximum y value of the box
+ * \param[in] z_min is the minimum z value of the box
+ * \param[in] z_max is the maximum z value of the box
+ * \param[in] id the cube id/name (default: "cube")
+ * \param[in] viewport (optional) the id of the new viewport (default: 0)
+ */
+ CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max);
+
+ /** \brief Allocate a new unstructured grid smartpointer. For internal use only.
+ * \param[out] polydata the resultant unstructured grid.
+ */
+ CV_EXPORTS void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
+}
--- /dev/null
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/viz/events.hpp>
+#include <q/interactor_style.h>
+#include <q/viz_types.h>
+#include <q/common.h>
+#include <opencv2/viz/types.hpp>
+#include <opencv2/core/affine.hpp>
+#include <opencv2/viz/viz3d.hpp>
+
+namespace temp_viz
+{
+
+class CV_EXPORTS Viz3d::VizImpl
+{
+public:
+ typedef cv::Ptr<VizImpl> Ptr;
+
+ VizImpl (const std::string &name = std::string());
+
+ virtual ~VizImpl ();
+ void setFullScreen (bool mode);
+ void setWindowName (const std::string &name);
+
+ /** \brief Register a callback boost::function for keyboard events
+ * \param[in] cb a boost function that will be registered as a callback for a keyboard event
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ boost::signals2::connection registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> cb);
+ inline boost::signals2::connection registerKeyboardCallback (void (*callback) (const cv::KeyboardEvent&, void*), void* cookie = NULL)
+ { return (registerKeyboardCallback (boost::bind (callback, _1, cookie))); }
+
+ /** \brief Register a callback function for keyboard events
+ * \param[in] callback the member function that will be registered as a callback for a keyboard event
+ * \param[in] instance instance to the class that implements the callback function
+ * \param[in] cookie user data that is passed to the callback
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ template<typename T> inline boost::signals2::connection registerKeyboardCallback (void (T::*callback) (const cv::KeyboardEvent&, void*), T& instance, void* cookie = NULL)
+ { return (registerKeyboardCallback (boost::bind (callback, boost::ref (instance), _1, cookie))); }
+
+ /** \brief Register a callback function for mouse events
+ * \param[in] cb a boost function that will be registered as a callback for a mouse event
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ boost::signals2::connection registerMouseCallback (boost::function<void (const cv::MouseEvent&)> cb);
+ inline boost::signals2::connection registerMouseCallback (void (*callback) (const cv::MouseEvent&, void*), void* cookie = NULL)
+ { return (registerMouseCallback (boost::bind (callback, _1, cookie))); }
+
+ /** \brief Register a callback function for mouse events
+ * \param[in] callback the member function that will be registered as a callback for a mouse event
+ * \param[in] instance instance to the class that implements the callback function
+ * \param[in] cookie user data that is passed to the callback
+ * \return a connection object that allows to disconnect the callback function.
+ */
+ template<typename T> inline boost::signals2::connection registerMouseCallback (void (T::*callback) (const cv::MouseEvent&, void*), T& instance, void* cookie = NULL)
+ { return (registerMouseCallback (boost::bind (callback, boost::ref (instance), _1, cookie))); }
+
+ void spin ();
+ void spinOnce (int time = 1, bool force_redraw = false);
+
+ /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z, Roll,Pitch,Yaw
+ *
+ * \param[in] scale the scale of the axes (default: 1)
+ * \param[in] t transformation matrix
+ *
+ * RPY Angles
+ * Rotate the reference frame by the angle roll about axis x
+ * Rotate the reference frame by the angle pitch about axis y
+ * Rotate the reference frame by the angle yaw about axis z
+ *
+ * Description:
+ * Sets the orientation of the Prop3D. Orientation is specified as
+ * X,Y and Z rotations in that order, but they are performed as
+ * RotateZ, RotateX, and finally RotateY.
+ *
+ * All axies use right hand rule. x=red axis, y=green axis, z=blue axis
+ * z direction is point into the screen.
+ * z
+ * \
+ * \
+ * \
+ * -----------> x
+ * |
+ * |
+ * |
+ * |
+ * |
+ * |
+ * y
+ */
+ void addCoordinateSystem (double scale, const cv::Affine3f& t, const std::string &id = "coordinate");
+
+ /** \brief Removes a previously added 3D axes (coordinate system)
+ */
+ bool removeCoordinateSystem (const std::string &id = "coordinate");
+ bool removePointCloud (const std::string &id = "cloud");
+ inline bool removePolygonMesh (const std::string &id = "polygon")
+ {
+ // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
+ return removePointCloud (id);
+ }
+ bool removeShape (const std::string &id = "cloud");
+
+ bool removeText3D (const std::string &id = "cloud");
+ bool removeAllPointClouds ();
+ bool removeAllShapes ();
+
+ void setBackgroundColor (const Color& color);
+
+ bool addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = "");
+ bool updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = "");
+
+ /** \brief Set the pose of an existing shape. Returns false if the shape doesn't exist, true if the pose was succesfully updated. */
+ bool updateShapePose (const std::string &id, const cv::Affine3f& pose);
+
+ bool addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = "");
+
+ bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud");
+ void addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat());
+ bool updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat());
+
+ bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon");
+ bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon");
+
+ bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id = "polyline");
+
+ void setPointCloudColor (const Color& color, const std::string &id = "cloud");
+ bool setPointCloudRenderingProperties (int property, double value, const std::string &id = "cloud");
+ bool getPointCloudRenderingProperties (int property, double &value, const std::string &id = "cloud");
+
+ bool setShapeRenderingProperties (int property, double value, const std::string &id);
+ void setShapeColor (const Color& color, const std::string &id);
+
+ /** \brief Set whether the point cloud is selected or not
+ * \param[in] selected whether the cloud is selected or not (true = selected)
+ * \param[in] id the point cloud object id (default: cloud)
+ */
+ bool setPointCloudSelected (const bool selected, const std::string &id = "cloud" );
+
+ /** \brief Returns true when the user tried to close the window */
+ bool wasStopped () const { if (interactor_ != NULL) return (stopped_); else return true; }
+
+ /** \brief Set the stopped flag back to false */
+ void resetStoppedFlag () { if (interactor_ != NULL) stopped_ = false; }
+
+ /** \brief Stop the interaction and close the visualizaton window. */
+ void close ()
+ {
+ stopped_ = true;
+ // This tends to close the window...
+ interactor_->TerminateApp ();
+ }
+
+ bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon");
+ bool addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id = "line");
+ bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow");
+ bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color_line, const Color& color_text, const std::string &id = "arrow");
+ bool addSphere (const cv::Point3f ¢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<vtkPolyData> polydata, const std::string & id = "PolyData");
+ bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PolyData");
+ bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel");
+ bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PLYModel");
+
+ /** \brief Add a cylinder from a set of given model coefficients
+ * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
+ * \param[in] id the cylinder id/name (default: "cylinder")
+ *
+ * \code
+ * // The following are given (or computed using sample consensus techniques)
+ * // See SampleConsensusModelCylinder for more information.
+ * // float radius;
+ *
+ * temp_viz::ModelCoefficients cylinder_coeff;
+ * cylinder_coeff.values.resize (7); // We need 7 values
+ * cylinder_coeff.values[0] = pt_on_axis.x ();
+ * cylinder_coeff.values[1] = pt_on_axis.y ();
+ * cylinder_coeff.values[2] = pt_on_axis.z ();
+ *
+ * cylinder_coeff.values[3] = axis_direction.x ();
+ * cylinder_coeff.values[4] = axis_direction.y ();
+ * cylinder_coeff.values[5] = axis_direction.z ();
+ *
+ * cylinder_coeff.values[6] = radius;
+ *
+ * addCylinder (cylinder_coeff);
+ * \endcode
+ */
+ bool addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cylinder");
+
+ /** \brief Add a plane from a set of given model coefficients
+ * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+ * \param[in] id the plane id/name (default: "plane")
+ *
+ * \code
+ * // The following are given (or computed using sample consensus techniques)
+ * // See SampleConsensusModelPlane for more information
+
+ *
+ * temp_viz::ModelCoefficients plane_coeff;
+ * plane_coeff.values.resize (4); // We need 4 values
+ * plane_coeff.values[0] = plane_parameters.x ();
+ * plane_coeff.values[1] = plane_parameters.y ();
+ * plane_coeff.values[2] = plane_parameters.z ();
+ * plane_coeff.values[3] = plane_parameters.w ();
+ *
+ * addPlane (plane_coeff);
+ * \endcode
+ */
+ bool addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "plane");
+ bool addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id = "plane");
+
+ /** \brief Add a circle from a set of given model coefficients
+ * \param[in] coefficients the model coefficients (x, y, radius)
+ * \param[in] id the circle id/name (default: "circle")
+ *
+ * \code
+ * // The following are given (or computed using sample consensus techniques)
+ * // See SampleConsensusModelCircle2D for more information
+ * // float x, y, radius;
+ *
+ * temp_viz::ModelCoefficients circle_coeff;
+ * circle_coeff.values.resize (3); // We need 3 values
+ * circle_coeff.values[0] = x;
+ * circle_coeff.values[1] = y;
+ * circle_coeff.values[2] = radius;
+ *
+ * vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
+ * \endcode
+ */
+ bool addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "circle");
+
+ /** \brief Add a cube from a set of given model coefficients
+ * \param[in] coefficients the model coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
+ * \param[in] id the cube id/name (default: "cube")
+ */
+ bool addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cube");
+
+ /** \brief Add a cube from a set of given model coefficients
+ * \param[in] translation a translation to apply to the cube from 0,0,0
+ * \param[in] rotation a quaternion-based rotation to apply to the cube
+ * \param[in] width the cube's width
+ * \param[in] height the cube's height
+ * \param[in] depth the cube's depth
+ * \param[in] id the cube id/name (default: "cube")
+ */
+ bool addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id = "cube");
+
+ /** \brief Add a cube
+ * \param[in] x_min the min X coordinate
+ * \param[in] x_max the max X coordinate
+ * \param[in] y_min the min Y coordinate
+ * \param[in] y_max the max Y coordinate
+ * \param[in] z_min the min Z coordinate
+ * \param[in] z_max the max Z coordinate
+ * \param[in] r how much red (0.0 -> 1.0)
+ * \param[in] g how much green (0.0 -> 1.0)
+ * \param[in] b how much blue (0.0 -> 1.0)
+ * \param[in] id the cube id/name (default: "cube")
+ */
+ bool addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max, const Color& color, const std::string &id = "cube");
+
+ /** \brief Changes the visual representation for all actors to surface representation. */
+ void setRepresentationToSurfaceForAllActors ();
+
+ /** \brief Changes the visual representation for all actors to points representation. */
+ void setRepresentationToPointsForAllActors ();
+
+ /** \brief Changes the visual representation for all actors to wireframe representation. */
+ void setRepresentationToWireframeForAllActors ();
+
+ /** \brief Initialize camera parameters with some default values. */
+ void initCameraParameters ();
+
+ /** \brief Search for camera parameters at the command line and set them internally.
+ bool getCameraParameters (int argc, char **argv);
+
+ /** \brief Checks whether the camera parameters were manually loaded from file.*/
+ bool cameraParamsSet () const;
+
+ /** \brief Update camera parameters and render. */
+ void updateCamera ();
+
+ /** \brief Reset camera parameters and render. */
+ void resetCamera ();
+
+ /** \brief Reset the camera direction from {0, 0, 0} to the center_{x, y, z} of a given dataset.
+ * \param[in] id the point cloud object id (default: cloud)
+ */
+ void resetCameraViewpoint (const std::string &id = "cloud");
+
+ /** \brief Set the camera pose given by position, viewpoint and up vector
+ * \param[in] pos_x the x coordinate of the camera location
+ * \param[in] pos_y the y coordinate of the camera location
+ * \param[in] pos_z the z coordinate of the camera location
+ * \param[in] view_x the x component of the view point of the camera
+ * \param[in] view_y the y component of the view point of the camera
+ * \param[in] view_z the z component of the view point of the camera
+ * \param[in] up_x the x component of the view up direction of the camera
+ * \param[in] up_y the y component of the view up direction of the camera
+ * \param[in] up_z the y component of the view up direction of the camera
+ */
+ void setCameraPosition (const cv::Vec3d& pos, const cv::Vec3d& view, const cv::Vec3d& up);
+
+ /** \brief Set the camera location and viewup according to the given arguments
+ * \param[in] pos_x the x coordinate of the camera location
+ * \param[in] pos_y the y coordinate of the camera location
+ * \param[in] pos_z the z coordinate of the camera location
+ * \param[in] up_x the x component of the view up direction of the camera
+ * \param[in] up_y the y component of the view up direction of the camera
+ * \param[in] up_z the z component of the view up direction of the camera
+ */
+ void setCameraPosition (double pos_x, double pos_y, double pos_z, double up_x, double up_y, double up_z);
+
+ /** \brief Set the camera parameters via an intrinsics and and extrinsics matrix
+ * \note This assumes that the pixels are square and that the center of the image is at the center of the sensor.
+ * \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters
+ * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters
+ */
+ void setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics);
+
+ /** \brief Set the camera parameters by given a full camera data structure.
+ * \param[in] camera camera structure containing all the camera parameters.
+ */
+ void setCameraParameters (const Camera &camera);
+
+ /** \brief Set the camera clipping distances.
+ * \param[in] near the near clipping distance (no objects closer than this to the camera will be drawn)
+ * \param[in] far the far clipping distance (no objects further away than this to the camera will be drawn)
+ */
+ void setCameraClipDistances (double near, double far);
+
+ /** \brief Set the camera vertical field of view in radians */
+ void setCameraFieldOfView (double fovy);
+
+ /** \brief Get the current camera parameters. */
+ void getCameras (Camera& camera);
+
+ /** \brief Get the current viewing pose. */
+ cv::Affine3f getViewerPose ();
+ void saveScreenshot (const std::string &file);
+
+ /** \brief Return a pointer to the underlying VTK Render Window used. */
+ //vtkSmartPointer<vtkRenderWindow> getRenderWindow () { return (window_); }
+
+ void setPosition (int x, int y);
+ void setSize (int xw, int yw);
+
+private:
+ vtkSmartPointer<vtkRenderWindowInteractor> interactor_;
+
+ struct ExitMainLoopTimerCallback : public vtkCommand
+ {
+ static ExitMainLoopTimerCallback* New()
+ {
+ return new ExitMainLoopTimerCallback;
+ }
+ virtual void Execute(vtkObject* vtkNotUsed(caller), unsigned long event_id, void* call_data)
+ {
+ if (event_id != vtkCommand::TimerEvent)
+ return;
+
+ int timer_id = *reinterpret_cast<int*> (call_data);
+ if (timer_id != right_timer_id)
+ return;
+
+ // Stop vtk loop and send notification to app to wake it up
+ viz_->interactor_->TerminateApp ();
+ }
+ int right_timer_id;
+ VizImpl* viz_;
+ };
+
+ struct ExitCallback : public vtkCommand
+ {
+ static ExitCallback* New ()
+ {
+ return new ExitCallback;
+ }
+ virtual void Execute (vtkObject*, unsigned long event_id, void*)
+ {
+ if (event_id == vtkCommand::ExitEvent)
+ {
+ viz_->stopped_ = true;
+ viz_->interactor_->TerminateApp ();
+ }
+ }
+ VizImpl* viz_;
+ };
+
+ /** \brief Set to false if the interaction loop is running. */
+ bool stopped_;
+
+ double s_lastDone_;
+
+ /** \brief Global timer ID. Used in destructor only. */
+ int timer_id_;
+
+ /** \brief Callback object enabling us to leave the main loop, when a timer fires. */
+ vtkSmartPointer<ExitMainLoopTimerCallback> exit_main_loop_timer_callback_;
+ vtkSmartPointer<ExitCallback> exit_callback_;
+
+ vtkSmartPointer<vtkRenderer> renderer_;
+ vtkSmartPointer<vtkRenderWindow> window_;
+
+ /** \brief The render window interactor style. */
+ vtkSmartPointer<InteractorStyle> style_;
+
+ /** \brief Internal list with actor pointers and name IDs for point clouds. */
+ cv::Ptr<CloudActorMap> cloud_actor_map_;
+
+ /** \brief Internal list with actor pointers and name IDs for shapes. */
+ cv::Ptr<ShapeActorMap> shape_actor_map_;
+
+ /** \brief Boolean that holds whether or not the camera parameters were manually initialized*/
+ bool camera_set_;
+
+ bool removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor);
+ bool removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor);
+ bool removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor);
+
+ //void addActorToRenderer (const vtkSmartPointer<vtkProp> &actor);
+
+
+ /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
+ * \param[in] data the vtk polydata object to create an actor for
+ * \param[out] actor the resultant vtk actor object
+ * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
+ */
+ void createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor, bool use_scalars = true);
+
+ /** \brief Updates a set of cells (vtkIdTypeArray) if the number of points in a cloud changes
+ * \param[out] cells the vtkIdTypeArray object (set of cells) to update
+ * \param[out] initcells a previously saved set of cells. If the number of points in the current cloud is
+ * higher than the number of cells in \a cells, and initcells contains enough data, then a copy from it
+ * will be made instead of regenerating the entire array.
+ * \param[in] nr_points the number of points in the new cloud. This dictates how many cells we need to
+ * generate
+ */
+ void updateCells (vtkSmartPointer<vtkIdTypeArray> &cells, vtkSmartPointer<vtkIdTypeArray> &initcells, vtkIdType nr_points);
+
+ void allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata);
+ void allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata);
+ void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
+
+};
+
+//void getTransformationMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternionf& orientation, Eigen::Matrix4f &transformation);
+
+//void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+
+void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+
+/** \brief Convert origin and orientation to vtkMatrix4x4
+ * \param[in] origin the point cloud origin
+ * \param[in] orientation the point cloud orientation
+ * \param[out] vtk_matrix the resultant VTK 4x4 matrix
+ */
+void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+void convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m);
+
+}
+
+
+
--- /dev/null
+#pragma once
+
+#include "precomp.hpp"
+
+namespace temp_viz
+{
+ struct CV_EXPORTS CloudActor
+ {
+ /** \brief The actor holding the data to render. */
+ vtkSmartPointer<vtkLODActor> actor;
+
+ /** \brief The viewpoint transformation matrix. */
+ vtkSmartPointer<vtkMatrix4x4> viewpoint_transformation_;
+
+ /** \brief Internal cell array. Used for optimizing updatePointCloud. */
+ vtkSmartPointer<vtkIdTypeArray> cells;
+ };
+
+ typedef std::map<std::string, CloudActor> CloudActorMap;
+ typedef std::map<std::string, vtkSmartPointer<vtkProp> > ShapeActorMap;
+}
+
--- /dev/null
+#include <q/shapes.h>
+
+inline float rad2deg (float alpha)
+{ return (alpha * 57.29578f); }
+
+inline double rad2deg (double alpha){return (alpha * 57.29578);}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides)
+{
+ vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
+ line->SetPoint1 (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+ line->SetPoint2 (coefficients.values[3]+coefficients.values[0], coefficients.values[4]+coefficients.values[1], coefficients.values[5]+coefficients.values[2]);
+
+ vtkSmartPointer<vtkTubeFilter> tuber = vtkSmartPointer<vtkTubeFilter>::New ();
+ tuber->SetInputConnection (line->GetOutputPort ());
+ tuber->SetRadius (coefficients.values[6]);
+ tuber->SetNumberOfSides (numsides);
+
+ return (tuber->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (const temp_viz::ModelCoefficients &coefficients)
+{
+ // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
+ vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+ t->Identity ();
+ t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+ Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
+ coefficients.values[4], coefficients.values[5]));
+ t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
+
+ vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+ cube->SetXLength (coefficients.values[7]);
+ cube->SetYLength (coefficients.values[8]);
+ cube->SetZLength (coefficients.values[9]);
+
+ vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+ tf->SetTransform (t);
+ tf->SetInputConnection (cube->GetOutputPort ());
+
+ return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth)
+{
+ // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
+ vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+ t->Identity ();
+ t->Translate (translation.x (), translation.y (), translation.z ());
+
+ Eigen::AngleAxisf a (rotation);
+ t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
+
+ vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+ cube->SetXLength (width);
+ cube->SetYLength (height);
+ cube->SetZLength (depth);
+
+ vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+ tf->SetTransform (t);
+ tf->SetInputConnection (cube->GetOutputPort ());
+
+ return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max)
+{
+ vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+ vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
+ cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max);
+ return (cube->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients)
+{
+ vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
+ plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+ double norm_sqr = coefficients.values[0] * coefficients.values[0]
+ + coefficients.values[1] * coefficients.values[1]
+ + coefficients.values[2] * coefficients.values[2];
+
+ plane->Push (-coefficients.values[3] / sqrt(norm_sqr));
+ return (plane->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z)
+{
+ vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
+
+
+ double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
+ coefficients.values[1] * coefficients.values[1] +
+ coefficients.values[2] * coefficients.values[2] );
+
+// double nx = coefficients.values [0] * norm;
+// double ny = coefficients.values [1] * norm;
+// double nz = coefficients.values [2] * norm;
+// double d = coefficients.values [3] * norm;
+
+// plane->SetNormal (nx, ny, nz);
+ plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
+
+ double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
+ x -= coefficients.values[0] * t * norm_sqr;
+ y -= coefficients.values[1] * t * norm_sqr;
+ z -= coefficients.values[2] * t * norm_sqr;
+ plane->SetCenter (x, y, z);
+
+ return (plane->GetOutput ());
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z)
+{
+ vtkSmartPointer<vtkDiskSource> disk = vtkSmartPointer<vtkDiskSource>::New ();
+ // Maybe the resolution should be lower e.g. 50 or 25
+ disk->SetCircumferentialResolution (100);
+ disk->SetInnerRadius (coefficients.values[2] - 0.001);
+ disk->SetOuterRadius (coefficients.values[2] + 0.001);
+ disk->SetCircumferentialResolution (20);
+
+ // An alternative to <vtkDiskSource> could be <vtkRegularPolygonSource> with <vtkTubeFilter>
+ /*
+ vtkSmartPointer<vtkRegularPolygonSource> circle = vtkSmartPointer<vtkRegularPolygonSource>::New();
+ circle->SetRadius (coefficients.values[2]);
+ circle->SetNumberOfSides (100);
+
+ vtkSmartPointer<vtkTubeFilter> tube = vtkSmartPointer<vtkTubeFilter>::New();
+ tube->SetInput (circle->GetOutput());
+ tube->SetNumberOfSides (25);
+ tube->SetRadius (0.001);
+ */
+
+ // Set the circle origin
+ vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+ t->Identity ();
+ t->Translate (coefficients.values[0], coefficients.values[1], z);
+
+ vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+ tf->SetTransform (t);
+ tf->SetInputConnection (disk->GetOutputPort ());
+ /*
+ tf->SetInputConnection (tube->GetOutputPort ());
+ */
+
+ return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution)
+{
+ // Set the sphere origin
+ vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+ t->Identity ();
+ t->Translate (center.x, center.y, center.z);
+
+ vtkSmartPointer<vtkSphereSource> s_sphere = vtkSmartPointer<vtkSphereSource>::New ();
+ s_sphere->SetRadius (radius);
+ s_sphere->SetPhiResolution (sphere_resolution);
+ s_sphere->SetThetaResolution (sphere_resolution);
+ s_sphere->LatLongTessellationOff ();
+
+ vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+ tf->SetTransform (t);
+ tf->SetInputConnection (s_sphere->GetOutputPort ());
+ tf->Update ();
+
+ return (tf->GetOutput ());
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2)
+{
+ vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
+ line->SetPoint1 (pt1.x, pt1.y, pt1.z);
+ line->SetPoint2 (pt2.x, pt2.y, pt2.z);
+ line->Update ();
+ return line->GetOutput ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
+{
+ polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+}
+
+
--- /dev/null
+#include <opencv2/viz/types.hpp>
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Color
+
+temp_viz::Color::Color() : Scalar(0, 0, 0) {}
+temp_viz::Color::Color(double gray) : Scalar(gray, gray, gray) {}
+temp_viz::Color::Color(double blue, double green, double red) : Scalar(blue, green, red) {}
+temp_viz::Color::Color(const Scalar& color) : Scalar(color) {}
+
+temp_viz::Color temp_viz::Color::black() { return Color( 0, 0, 0); }
+temp_viz::Color temp_viz::Color::green() { return Color( 0, 255, 0); }
+temp_viz::Color temp_viz::Color::blue() { return Color(255, 0, 0); }
+temp_viz::Color temp_viz::Color::cyan() { return Color(255, 255, 0); }
+
+temp_viz::Color temp_viz::Color::red() { return Color( 0, 0, 255); }
+temp_viz::Color temp_viz::Color::magenta() { return Color( 0, 255, 255); }
+temp_viz::Color temp_viz::Color::yellow() { return Color(255, 0, 255); }
+temp_viz::Color temp_viz::Color::white() { return Color(255, 255, 255); }
+
+temp_viz::Color temp_viz::Color::gray() { return Color(128, 128, 128); }
+
--- /dev/null
+#include "precomp.hpp"
\ No newline at end of file
--- /dev/null
+#include <opencv2/viz/viz3d.hpp>
+#include <q/viz3d_impl.hpp>
+
+
+temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_name))
+{
+
+}
+
+temp_viz::Viz3d::~Viz3d()
+{
+
+}
+
+
+void temp_viz::Viz3d::setBackgroundColor(const Color& color)
+{
+ impl_->setBackgroundColor(color);
+}
+
+void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String &id)
+{
+ impl_->addCoordinateSystem(scale, t, id);
+}
+
+void temp_viz::Viz3d::addPointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask)
+{
+ impl_->addPointCloud(cloud, colors, id, mask);
+}
+
+bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals, int level, float scale, const String& id)
+{
+ return impl_->addPointCloudNormals(cloud, normals, level, scale, id);
+}
+
+bool temp_viz::Viz3d::updatePointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask)
+{
+ return impl_->updatePointCloud(cloud, colors, id, mask);
+}
+
+bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String &id)
+{
+ return impl_->addPolygonMesh(mesh, Mat(), id);
+}
+
+bool temp_viz::Viz3d::updatePolygonMesh (const Mesh3d& mesh, const String &id)
+{
+ return impl_->updatePolygonMesh(mesh, Mat(), id);
+}
+
+bool temp_viz::Viz3d::addPolylineFromPolygonMesh (const Mesh3d& mesh, const String &id)
+{
+ return impl_->addPolylineFromPolygonMesh(mesh, id);
+}
+
+bool temp_viz::Viz3d::addText (const String &text, int xpos, int ypos, const Color& color, int fontsize, const String &id)
+{
+ return impl_->addText(text, xpos, ypos, color, fontsize, id);
+}
+
+bool temp_viz::Viz3d::addPolygon(const Mat& cloud, const Color& color, const String& id)
+{
+ return impl_->addPolygon(cloud, color, id);
+}
+
+bool temp_viz::Viz3d::addSphere (const cv::Point3f ¢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);
+}
--- /dev/null
+#include <opencv2/core.hpp>
+#include <q/shapes.h>
+
+#include <vtkCellData.h>
+#include <vtkSmartPointer.h>
+#include <vtkCellArray.h>
+#include <vtkProperty2D.h>
+#include <vtkMapper2D.h>
+#include <vtkLeaderActor2D.h>
+#include <q/shapes.h>
+#include <vtkAlgorithmOutput.h>
+
+#include <q/viz3d_impl.hpp>
+
+void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
+{
+ if (window_)
+ window_->SetFullScreen (mode);
+}
+
+void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
+{
+ if (window_)
+ window_->SetWindowName (name.c_str ());
+}
+
+void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
+void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
+
+void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask)
+{
+ CV_Assert(cloud.type() == CV_32FC3 && colors.type() == CV_8UC3 && colors.size() == cloud.size());
+ CV_Assert(mask.empty() || (mask.type() == CV_8U && mask.size() == cloud.size()));
+
+ vtkSmartPointer<vtkPolyData> polydata;
+
+ allocVtkPolyData(polydata);
+ //polydata = vtkSmartPointer<vtkPolyData>::New ();
+ vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
+ polydata->SetVerts (vertices);
+
+ vtkSmartPointer<vtkIdTypeArray> initcells;
+ vtkIdType nr_points = cloud.size().area();
+ vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
+
+ if (!points)
+ {
+ points = vtkSmartPointer<vtkPoints>::New ();
+ points->SetDataTypeToFloat ();
+ polydata->SetPoints (points);
+ }
+ points->SetNumberOfPoints (nr_points);
+
+ // Get a pointer to the beginning of the data array
+ float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+ if (mask.empty())
+ {
+ int j = 0;
+ for(int y = 0; y < cloud.rows; ++y)
+ {
+ const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+ for(int x = 0; x < cloud.cols; ++x)
+ memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+ }
+ }
+ else
+ {
+ int j = 0;
+ for(int y = 0; y < cloud.rows; ++y)
+ {
+ const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+ const unsigned char* mrow = mask.ptr<unsigned char>(y);
+ for(int x = 0; x < cloud.cols; ++x)
+ if (mrow[x])
+ memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+ }
+ nr_points = j;
+ points->SetNumberOfPoints (nr_points);
+ }
+
+ vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
+ updateCells (cells, initcells, nr_points);
+
+ // Set the cells and the vertices
+ vertices->SetCells (nr_points, cells);
+
+ /////////////////////////////////////////////////////////////////////////////////
+
+ // use the given geometry handler
+ polydata->Update ();
+
+ // Get the colors from the handler
+ bool has_colors = false;
+ double minmax[2];
+ vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+
+ // Get a random color
+ unsigned char* colors_data = new unsigned char[nr_points * 3];
+
+ if (mask.empty())
+ {
+ int j = 0;
+ for(int y = 0; y < colors.rows; ++y)
+ {
+ const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+ for(int x = 0; x < colors.cols; ++x)
+ memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+ }
+ }
+ else
+ {
+ int j = 0;
+ for(int y = 0; y < colors.rows; ++y)
+ {
+ const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+ const unsigned char* mrow = mask.ptr<unsigned char>(y);
+ for(int x = 0; x < colors.cols; ++x)
+ if (mrow[x])
+ memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+ }
+ }
+
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
+
+ /////////////////////////////////////////
+ has_colors = true;
+
+ if (has_colors)
+ {
+ polydata->GetPointData ()->SetScalars (scalars);
+ scalars->GetRange (minmax);
+ }
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (polydata, actor);
+ if (has_colors)
+ actor->GetMapper ()->SetScalarRange (minmax);
+
+ // Add it to all renderers
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*cloud_actor_map_)[id].actor = actor;
+ (*cloud_actor_map_)[id].cells = initcells;
+
+ const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
+ const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+
+ // Save the viewpoint transformation matrix to the global actor map
+ vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
+ convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
+ (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
+}
+
+
+bool temp_viz::Viz3d::VizImpl::updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return (false);
+
+ // Get the current poly data
+ vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
+ if (!polydata)
+ return (false);
+ vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
+ vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
+ // Copy the new point array in
+ vtkIdType nr_points = cloud.size().area();
+ points->SetNumberOfPoints (nr_points);
+
+ // Get a pointer to the beginning of the data array
+ float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+ if (mask.empty())
+ {
+ int j = 0;
+ for(int y = 0; y < cloud.rows; ++y)
+ {
+ const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+ for(int x = 0; x < cloud.cols; ++x)
+ memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+ }
+ }
+ else
+ {
+ int j = 0;
+ for(int y = 0; y < cloud.rows; ++y)
+ {
+ const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
+ const unsigned char* mrow = mask.ptr<unsigned char>(y);
+ for(int x = 0; x < cloud.cols; ++x)
+ if (mrow[x])
+ memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+ }
+ nr_points = j;
+ points->SetNumberOfPoints (nr_points);
+ }
+
+ vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
+ updateCells (cells, am_it->second.cells, nr_points);
+
+
+ // Set the cells and the vertices
+ vertices->SetCells (nr_points, cells);
+
+#if 1
+ // Get the colors from the handler
+ // vtkSmartPointer<vtkDataArray> scalars;
+ // color_handler.getColor (scalars);
+ // double minmax[2];
+ // scalars->GetRange (minmax);
+
+ // // Update the data
+ // polydata->GetPointData ()->SetScalars (scalars);
+ // polydata->Update ();
+
+ // am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
+ // am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
+
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ // Get the colors from the handler
+ bool has_colors = false;
+ double minmax[2];
+ vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+
+ // Get a random color
+ unsigned char* colors_data = new unsigned char[nr_points * 3];
+
+ if (mask.empty())
+ {
+ int j = 0;
+ for(int y = 0; y < colors.rows; ++y)
+ {
+ const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+ for(int x = 0; x < colors.cols; ++x)
+ memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+ }
+ }
+ else
+ {
+ int j = 0;
+ for(int y = 0; y < colors.rows; ++y)
+ {
+ const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
+ const unsigned char* mrow = mask.ptr<unsigned char>(y);
+ for(int x = 0; x < colors.cols; ++x)
+ if (mrow[x])
+ memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
+ }
+ }
+
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
+
+ /////////////////////////////////////////
+ has_colors = true;
+
+ if (has_colors)
+ {
+ polydata->GetPointData ()->SetScalars (scalars);
+ scalars->GetRange (minmax);
+ }
+
+#else
+ vtkSmartPointer<vtkDataArray> scalars;
+ polydata->GetPointData ()->SetScalars (scalars);
+ polydata->Update ();
+ double minmax[2];
+ minmax[0] = std::numeric_limits<double>::min ();
+ minmax[1] = std::numeric_limits<double>::max ();
+ am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
+ am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
+#endif
+
+
+ // Update the mapper
+ reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
+ return (true);
+}
+
+
+
+bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level, float scale, const std::string &id)
+{
+ CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3);
+
+ if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
+ return (false);
+
+ vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
+ vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
+
+ points->SetDataTypeToFloat ();
+ vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
+ data->SetNumberOfComponents (3);
+
+ vtkIdType nr_normals = 0;
+ float* pts = 0;
+
+ // If the cloud is organized, then distribute the normal step in both directions
+ if (cloud.cols > 1 && cloud.rows > 1)
+ {
+ vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
+ nr_normals = (static_cast<vtkIdType> ((cloud.cols - 1)/ point_step) + 1) *
+ (static_cast<vtkIdType> ((cloud.rows - 1) / point_step) + 1);
+ pts = new float[2 * nr_normals * 3];
+
+ vtkIdType cell_count = 0;
+ for (vtkIdType y = 0; y < cloud.rows; y += point_step)
+ for (vtkIdType x = 0; x < cloud.cols; x += point_step)
+ {
+ cv::Point3f p = cloud.at<cv::Point3f>(y, x);
+ cv::Point3f n = normals.at<cv::Point3f>(y, x) * scale;
+
+ pts[2 * cell_count * 3 + 0] = p.x;
+ pts[2 * cell_count * 3 + 1] = p.y;
+ pts[2 * cell_count * 3 + 2] = p.z;
+ pts[2 * cell_count * 3 + 3] = p.x + n.x;
+ pts[2 * cell_count * 3 + 4] = p.y + n.y;
+ pts[2 * cell_count * 3 + 5] = p.z + n.z;
+
+ lines->InsertNextCell (2);
+ lines->InsertCellPoint (2 * cell_count);
+ lines->InsertCellPoint (2 * cell_count + 1);
+ cell_count++;
+ }
+ }
+ else
+ {
+ nr_normals = (cloud.size().area() - 1) / level + 1 ;
+ pts = new float[2 * nr_normals * 3];
+
+ for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
+ {
+ cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
+ cv::Point3f n = normals.ptr<cv::Point3f>()[i] * scale;
+
+ pts[2 * j * 3 + 0] = p.x;
+ pts[2 * j * 3 + 1] = p.y;
+ pts[2 * j * 3 + 2] = p.z;
+ pts[2 * j * 3 + 3] = p.x + n.x;
+ pts[2 * j * 3 + 4] = p.y + n.y;
+ pts[2 * j * 3 + 5] = p.z + n.z;
+
+ lines->InsertNextCell (2);
+ lines->InsertCellPoint (2 * j);
+ lines->InsertCellPoint (2 * j + 1);
+ }
+ }
+
+ data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
+ points->SetData (data);
+
+ vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
+ polyData->SetPoints (points);
+ polyData->SetLines (lines);
+
+ vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+ mapper->SetInput (polyData);
+ mapper->SetColorModeToMapScalars();
+ mapper->SetScalarModeToUsePointData();
+
+ // create actor
+ vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+ actor->SetMapper (mapper);
+
+ // Add it to all renderers
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*cloud_actor_map_)[id].actor = actor;
+ return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addLine] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+ vtkSmartPointer<vtkDataSet> data = createLine (pt1, pt2);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+
+ Color c = vtkcolor(color);
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+
+
+inline bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
+{
+ CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
+ CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
+ CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
+
+ if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
+ return std::cout << "[addPolygonMesh] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+ // int rgb_idx = -1;
+ // std::vector<sensor_msgs::PointField> fields;
+
+
+ // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields);
+ // if (rgb_idx == -1)
+ // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields);
+
+ vtkSmartPointer<vtkUnsignedCharArray> colors_array;
+#if 1
+ if (!mesh.colors.empty())
+ {
+ colors_array = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ colors_array->SetNumberOfComponents (3);
+ colors_array->SetName ("Colors");
+
+ const unsigned char* data = mesh.colors.ptr<unsigned char>();
+
+ //TODO check mask
+ CV_Assert(mask.empty()); //because not implemented;
+
+ for(int i = 0; i < mesh.colors.cols; ++i)
+ colors_array->InsertNextTupleValue(&data[i*3]);
+
+ // temp_viz::RGB rgb_data;
+ // for (size_t i = 0; i < cloud->size (); ++i)
+ // {
+ // if (!isFinite (cloud->points[i]))
+ // continue;
+ // memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (temp_viz::RGB));
+ // unsigned char color[3];
+ // color[0] = rgb_data.r;
+ // color[1] = rgb_data.g;
+ // color[2] = rgb_data.b;
+ // colors->InsertNextTupleValue (color);
+ // }
+ }
+#endif
+
+ // Create points from polyMesh.cloud
+ vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+ vtkIdType nr_points = mesh.cloud.size().area();
+
+ points->SetNumberOfPoints (nr_points);
+
+
+ // Get a pointer to the beginning of the data array
+ float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
+
+
+ std::vector<int> lookup;
+ // If the dataset is dense (no NaNs)
+ if (mask.empty())
+ {
+ cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
+ mesh.cloud.copyTo(hdr);
+ }
+ else
+ {
+ lookup.resize (nr_points);
+
+ const unsigned char *mdata = mask.ptr<unsigned char>();
+ const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+ cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
+
+ int j = 0; // true point index
+ for (int i = 0; i < nr_points; ++i)
+ if(mdata[i])
+ {
+ lookup[i] = j;
+ out[j++] = cdata[i];
+ }
+ nr_points = j;
+ points->SetNumberOfPoints (nr_points);
+ }
+
+ // Get the maximum size of a polygon
+ int max_size_of_polygon = -1;
+ for (size_t i = 0; i < mesh.polygons.size (); ++i)
+ if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
+ max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
+
+ vtkSmartPointer<vtkLODActor> actor;
+
+ if (mesh.polygons.size () > 1)
+ {
+ // Create polys from polyMesh.polygons
+ vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
+ vtkIdType *cell = cell_array->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
+ int idx = 0;
+ if (lookup.size () > 0)
+ {
+ for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+ {
+ size_t n_points = mesh.polygons[i].vertices.size ();
+ *cell++ = n_points;
+ //cell_array->InsertNextCell (n_points);
+ for (size_t j = 0; j < n_points; j++, ++idx)
+ *cell++ = lookup[mesh.polygons[i].vertices[j]];
+ //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
+ }
+ }
+ else
+ {
+ for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+ {
+ size_t n_points = mesh.polygons[i].vertices.size ();
+ *cell++ = n_points;
+ //cell_array->InsertNextCell (n_points);
+ for (size_t j = 0; j < n_points; j++, ++idx)
+ *cell++ = mesh.polygons[i].vertices[j];
+ //cell_array->InsertCellPoint (vertices[i].vertices[j]);
+ }
+ }
+ vtkSmartPointer<vtkPolyData> polydata;
+ allocVtkPolyData (polydata);
+ cell_array->GetData ()->SetNumberOfValues (idx);
+ cell_array->Squeeze ();
+ polydata->SetStrips (cell_array);
+ polydata->SetPoints (points);
+
+ if (colors_array)
+ polydata->GetPointData ()->SetScalars (colors_array);
+
+ createActorFromVTKDataSet (polydata, actor, false);
+ }
+ else
+ {
+ vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
+ size_t n_points = mesh.polygons[0].vertices.size ();
+ polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
+
+ if (lookup.size () > 0)
+ {
+ for (size_t j = 0; j < n_points - 1; ++j)
+ polygon->GetPointIds ()->SetId (j, lookup[mesh.polygons[0].vertices[j]]);
+ }
+ else
+ {
+ for (size_t j = 0; j < n_points - 1; ++j)
+ polygon->GetPointIds ()->SetId (j, mesh.polygons[0].vertices[j]);
+ }
+ vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
+ allocVtkUnstructuredGrid (poly_grid);
+ poly_grid->Allocate (1, 1);
+ poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
+ poly_grid->SetPoints (points);
+ poly_grid->Update ();
+ if (colors_array)
+ poly_grid->GetPointData ()->SetScalars (colors_array);
+
+ createActorFromVTKDataSet (poly_grid, actor, false);
+ }
+ renderer_->AddActor (actor);
+ actor->GetProperty ()->SetRepresentationToSurface ();
+ // Backface culling renders the visualization slower, but guarantees that we see all triangles
+ actor->GetProperty ()->BackfaceCullingOff ();
+ actor->GetProperty ()->SetInterpolationToFlat ();
+ actor->GetProperty ()->EdgeVisibilityOff ();
+ actor->GetProperty ()->ShadingOff ();
+
+ // Save the pointer/ID pair to the global actor map
+ (*cloud_actor_map_)[id].actor = actor;
+ //if (vertices.size () > 1)
+ // (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
+
+ const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
+ const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+
+ // Save the viewpoint transformation matrix to the global actor map
+ vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
+ convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
+ (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
+
+ return (true);
+}
+
+
+inline bool temp_viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id)
+{
+ CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
+ CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
+ CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
+
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return (false);
+
+ // Get the current poly data
+ vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
+ if (!polydata)
+ return (false);
+ vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
+ if (!cells)
+ return (false);
+ vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
+ // Copy the new point array in
+ vtkIdType nr_points = mesh.cloud.size().area();
+ points->SetNumberOfPoints (nr_points);
+
+ // Get a pointer to the beginning of the data array
+ float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
+
+ int ptr = 0;
+ std::vector<int> lookup;
+ // If the dataset is dense (no NaNs)
+ if (mask.empty())
+ {
+ cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
+ mesh.cloud.copyTo(hdr);
+
+ }
+ else
+ {
+ lookup.resize (nr_points);
+
+ const unsigned char *mdata = mask.ptr<unsigned char>();
+ const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+ cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
+
+ int j = 0; // true point index
+ for (int i = 0; i < nr_points; ++i)
+ if(mdata[i])
+ {
+ lookup[i] = j;
+ out[j++] = cdata[i];
+ }
+ nr_points = j;
+ points->SetNumberOfPoints (nr_points);;
+ }
+
+ // Update colors
+ vtkUnsignedCharArray* colors_array = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
+
+ if (!mesh.colors.empty() && colors_array)
+ {
+ if (mask.empty())
+ {
+ const unsigned char* data = mesh.colors.ptr<unsigned char>();
+ for(int i = 0; i < mesh.colors.cols; ++i)
+ colors_array->InsertNextTupleValue(&data[i*3]);
+ }
+ else
+ {
+ const unsigned char* color = mesh.colors.ptr<unsigned char>();
+ const unsigned char* mdata = mask.ptr<unsigned char>();
+
+ int j = 0;
+ for(int i = 0; i < mesh.colors.cols; ++i)
+ if (mdata[i])
+ colors_array->SetTupleValue (j++, &color[i*3]);
+
+ }
+ }
+
+ // Get the maximum size of a polygon
+ int max_size_of_polygon = -1;
+ for (size_t i = 0; i < mesh.polygons.size (); ++i)
+ if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
+ max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
+
+ // Update the cells
+ cells = vtkSmartPointer<vtkCellArray>::New ();
+ vtkIdType *cell = cells->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
+ int idx = 0;
+ if (lookup.size () > 0)
+ {
+ for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+ {
+ size_t n_points = mesh.polygons[i].vertices.size ();
+ *cell++ = n_points;
+ for (size_t j = 0; j < n_points; j++, cell++, ++idx)
+ *cell = lookup[mesh.polygons[i].vertices[j]];
+ }
+ }
+ else
+ {
+ for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
+ {
+ size_t n_points = mesh.polygons[i].vertices.size ();
+ *cell++ = n_points;
+ for (size_t j = 0; j < n_points; j++, cell++, ++idx)
+ *cell = mesh.polygons[i].vertices[j];
+ }
+ }
+ cells->GetData ()->SetNumberOfValues (idx);
+ cells->Squeeze ();
+ // Set the the vertices
+ polydata->SetStrips (cells);
+ polydata->Update ();
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color, bool display_length, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+ // Create an Actor
+ vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
+ leader->GetPositionCoordinate()->SetCoordinateSystemToWorld ();
+ leader->GetPositionCoordinate()->SetValue (p1.x, p1.y, p1.z);
+ leader->GetPosition2Coordinate()->SetCoordinateSystemToWorld ();
+ leader->GetPosition2Coordinate()->SetValue (p2.x, p2.y, p2.z);
+ leader->SetArrowStyleToFilled();
+ leader->SetArrowPlacementToPoint2 ();
+
+ if (display_length)
+ leader->AutoLabelOn ();
+ else
+ leader->AutoLabelOff ();
+
+ Color c = vtkcolor(color);
+ leader->GetProperty ()->SetColor (c.val);
+ renderer_->AddActor (leader);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = leader;
+ return (true);
+}
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color_line, const Color& color_text, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ // Create an Actor
+ vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
+ leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
+ leader->GetPositionCoordinate ()->SetValue (p1.x, p1.y, p1.z);
+ leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
+ leader->GetPosition2Coordinate ()->SetValue (p2.x, p2.y, p2.z);
+ leader->SetArrowStyleToFilled ();
+ leader->AutoLabelOn ();
+
+ Color ct = vtkcolor(color_text);
+ leader->GetLabelTextProperty()->SetColor(ct.val);
+
+ Color cl = vtkcolor(color_line);
+ leader->GetProperty ()->SetColor (cl.val);
+ renderer_->AddActor (leader);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = leader;
+ return (true);
+}
+
+#include <vtkSphereSource.h>
+////////////////////////////////////////////////////////////////////////////////////////////
+inline bool temp_viz::Viz3d::VizImpl::addSphere (const cv::Point3f& center, float radius, const Color& color, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addSphere] A shape with id <"<<id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+ //vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
+ vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New ();
+ data->SetRadius (radius);
+ data->SetCenter (center.x, center.y, center.z);
+ data->SetPhiResolution (10);
+ data->SetThetaResolution (10);
+ data->LatLongTessellationOff ();
+ data->Update ();
+
+ // Setup actor and mapper
+ vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+ mapper->SetInputConnection (data->GetOutputPort ());
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+ actor->SetMapper (mapper);
+ //createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToSurface ();
+ actor->GetProperty ()->SetInterpolationToFlat ();
+
+ Color c = vtkcolor(color);
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetMapper ()->ImmediateModeRenderingOn ();
+ actor->GetMapper ()->StaticOn ();
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ actor->GetMapper ()->Update ();
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+inline bool temp_viz::Viz3d::VizImpl::updateSphere (const cv::Point3f ¢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<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
+ textSource->SetText (text.c_str());
+ textSource->Update ();
+
+ vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+ textMapper->SetInputConnection (textSource->GetOutputPort ());
+
+ // Since each follower may follow a different camera, we need different followers
+ vtkRenderer* renderer = renderer_;
+
+ vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
+ textActor->SetMapper (textMapper);
+ textActor->SetPosition (position.x, position.y, position.z);
+ textActor->SetScale (textScale);
+
+ Color c = vtkcolor(color);
+ textActor->GetProperty ()->SetColor (c.val);
+ textActor->SetCamera (renderer->GetActiveCamera ());
+
+ renderer->AddActor (textActor);
+ renderer->Render ();
+
+ // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
+ // for multiple viewport
+ (*shape_actor_map_)[tid] = textActor;
+
+
+ return (true);
+}
+
+inline bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
+{
+ CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);
+
+ vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+ vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
+
+ int total = cloud.size().area();
+ points->SetNumberOfPoints (total);
+ polygon->GetPointIds ()->SetNumberOfIds (total);
+
+ for (int i = 0; i < total; ++i)
+ {
+ cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
+ points->SetPoint (i, p.x, p.y, p.z);
+ polygon->GetPointIds ()->SetId (i, i);
+ }
+
+ vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
+ allocVtkUnstructuredGrid (poly_grid);
+ poly_grid->Allocate (1, 1);
+ poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
+ poly_grid->SetPoints (points);
+ poly_grid->Update ();
+
+
+ //////////////////////////////////////////////////////
+ vtkSmartPointer<vtkDataSet> data = poly_grid;
+
+ Color c = vtkcolor(color);
+
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
+
+ // Add old data
+ all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
+
+ // Add new data
+ vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
+ surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
+ vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
+ all_data->AddInput (poly_data);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (all_data->GetOutput (), actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ actor->GetProperty ()->BackfaceCullingOff ();
+
+ removeActorFromRenderer (am_it->second);
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ }
+ else
+ {
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ actor->GetProperty ()->BackfaceCullingOff ();
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ }
+
+ return (true);
+}
--- /dev/null
+#include "precomp.hpp"
+
+#include <opencv2/calib3d.hpp>
+#include <q/shapes.h>
+#include <q/viz3d_impl.hpp>
+
+#include <vtkRenderWindowInteractor.h>
+#ifndef __APPLE__
+vtkRenderWindowInteractor* vtkRenderWindowInteractorFixNew ()
+{
+ return (vtkRenderWindowInteractor::New ());
+}
+#endif
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+temp_viz::Viz3d::VizImpl::VizImpl (const std::string &name)
+ : style_ (vtkSmartPointer<temp_viz::InteractorStyle>::New ())
+ , cloud_actor_map_ (new CloudActorMap)
+ , shape_actor_map_ (new ShapeActorMap)
+ , s_lastDone_(0.0)
+{
+ renderer_ = vtkSmartPointer<vtkRenderer>::New ();
+
+ // Create a RendererWindow
+ window_ = vtkSmartPointer<vtkRenderWindow>::New ();
+
+ // Set the window size as 1/2 of the screen size
+ cv::Vec2i window_size = cv::Vec2i(window_->GetScreenSize()) / 2;
+ window_->SetSize (window_size.val);
+
+ window_->AddRenderer (renderer_);
+
+ // Create the interactor style
+ style_->Initialize ();
+ style_->setRenderer (renderer_);
+ style_->setCloudActorMap (cloud_actor_map_);
+ style_->UseTimersOn ();
+
+ /////////////////////////////////////////////////
+ interactor_ = vtkSmartPointer <vtkRenderWindowInteractor>::Take (vtkRenderWindowInteractorFixNew ());
+
+ //win_->PointSmoothingOn ();
+ //win_->LineSmoothingOn ();
+ //win_->PolygonSmoothingOn ();
+ window_->AlphaBitPlanesOff ();
+ window_->PointSmoothingOff ();
+ window_->LineSmoothingOff ();
+ window_->PolygonSmoothingOff ();
+ window_->SwapBuffersOn ();
+ window_->SetStereoTypeToAnaglyph ();
+
+ interactor_->SetRenderWindow (window_);
+ interactor_->SetInteractorStyle (style_);
+ //interactor_->SetStillUpdateRate (30.0);
+ interactor_->SetDesiredUpdateRate (30.0);
+
+ // Initialize and create timer, also create window
+ interactor_->Initialize ();
+ timer_id_ = interactor_->CreateRepeatingTimer (5000L);
+
+ // Set a simple PointPicker
+ vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
+ pp->SetTolerance (pp->GetTolerance () * 2);
+ interactor_->SetPicker (pp);
+
+ exit_main_loop_timer_callback_ = vtkSmartPointer<ExitMainLoopTimerCallback>::New ();
+ exit_main_loop_timer_callback_->viz_ = this;
+ exit_main_loop_timer_callback_->right_timer_id = -1;
+ interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
+
+ exit_callback_ = vtkSmartPointer<ExitCallback>::New ();
+ exit_callback_->viz_ = this;
+ interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_);
+
+ resetStoppedFlag ();
+
+
+ //////////////////////////////
+
+ String window_name("Viz");
+ window_name = name.empty() ? window_name : window_name + " - " + name;
+ window_->SetWindowName (window_name.c_str ());
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+temp_viz::Viz3d::VizImpl::~VizImpl ()
+{
+ if (interactor_ != NULL)
+ interactor_->DestroyTimer (timer_id_);
+
+ renderer_->Clear();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_->saveScreenshot (file); }
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::Viz3d::VizImpl::registerKeyboardCallback (boost::function<void (const cv::KeyboardEvent&)> callback)
+{
+ return (style_->registerKeyboardCallback (callback));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+boost::signals2::connection temp_viz::Viz3d::VizImpl::registerMouseCallback (boost::function<void (const cv::MouseEvent&)> callback)
+{
+ return (style_->registerMouseCallback (callback));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::spin ()
+{
+ resetStoppedFlag ();
+ window_->Render ();
+ interactor_->Start ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::spinOnce (int time, bool force_redraw)
+{
+ resetStoppedFlag ();
+
+ if (time <= 0)
+ time = 1;
+
+ if (force_redraw)
+ interactor_->Render ();
+
+ double s_now_ = cv::getTickCount() / cv::getTickFrequency();
+ if (s_lastDone_ > s_now_)
+ s_lastDone_ = s_now_;
+
+ if ((s_now_ - s_lastDone_) > (1.0 / interactor_->GetDesiredUpdateRate ()))
+ {
+ exit_main_loop_timer_callback_->right_timer_id = interactor_->CreateRepeatingTimer (time);
+ interactor_->Start ();
+ interactor_->DestroyTimer (exit_main_loop_timer_callback_->right_timer_id);
+ s_lastDone_ = s_now_;
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::addCoordinateSystem (double scale, const cv::Affine3f& affine, const std::string &id)
+{
+ vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
+ axes->SetOrigin (0, 0, 0);
+ axes->SetScaleFactor (scale);
+
+ vtkSmartPointer<vtkFloatArray> axes_colors = vtkSmartPointer<vtkFloatArray>::New ();
+ axes_colors->Allocate (6);
+ axes_colors->InsertNextValue (0.0);
+ axes_colors->InsertNextValue (0.0);
+ axes_colors->InsertNextValue (0.5);
+ axes_colors->InsertNextValue (0.5);
+ axes_colors->InsertNextValue (1.0);
+ axes_colors->InsertNextValue (1.0);
+
+ vtkSmartPointer<vtkPolyData> axes_data = axes->GetOutput ();
+ axes_data->Update ();
+ axes_data->GetPointData ()->SetScalars (axes_colors);
+
+ vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
+ axes_tubes->SetInput (axes_data);
+ axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
+ axes_tubes->SetNumberOfSides (6);
+
+ vtkSmartPointer<vtkPolyDataMapper> axes_mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+ axes_mapper->SetScalarModeToUsePointData ();
+ axes_mapper->SetInput (axes_tubes->GetOutput ());
+
+ vtkSmartPointer<vtkLODActor> axes_actor = vtkSmartPointer<vtkLODActor>::New ();
+ axes_actor->SetMapper (axes_mapper);
+
+ cv::Vec3d t = affine.translation();
+ axes_actor->SetPosition (t[0], t[1], t[2]);
+
+ cv::Matx33f m = affine.rotation();
+
+ cv::Vec3f rvec;
+ cv::Rodrigues(m, rvec);
+
+ float r_angle = cv::norm(rvec);
+ rvec *= 1.f/r_angle;
+
+ axes_actor->SetOrientation(0,0,0);
+ axes_actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]);
+ renderer_->AddActor (axes_actor);
+
+ (*shape_actor_map_)[id] = axes_actor;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeCoordinateSystem (const std::string &id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it == shape_actor_map_->end ())
+ return false;
+
+ // Remove it from all renderers
+ if (!removeActorFromRenderer(am_it->second))
+ return false;
+
+ shape_actor_map_->erase(am_it);
+ return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removePointCloud (const std::string &id)
+{
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return false;
+
+ if (removeActorFromRenderer (am_it->second.actor))
+ return cloud_actor_map_->erase (am_it), true;
+
+ return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeShape (const std::string &id)
+{
+ // Check to see if the given ID entry exists
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ // Extra step: check if there is a cloud with the same ID
+ CloudActorMap::iterator ca_it = cloud_actor_map_->find (id);
+
+ bool shape = true;
+ // Try to find a shape first
+ if (am_it == shape_actor_map_->end ())
+ {
+ // There is no cloud or shape with this ID, so just exit
+ if (ca_it == cloud_actor_map_->end ())
+ return false;
+ // Cloud found, set shape to false
+ shape = false;
+ }
+
+ // Remove the pointer/ID pair to the global actor map
+ if (shape)
+ {
+ if (removeActorFromRenderer (am_it->second))
+ {
+ shape_actor_map_->erase (am_it);
+ return (true);
+ }
+ }
+ else
+ {
+ if (removeActorFromRenderer (ca_it->second.actor))
+ {
+ cloud_actor_map_->erase (ca_it);
+ return true;
+ }
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeText3D (const std::string &id)
+{
+ // Check to see if the given ID entry exists
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it == shape_actor_map_->end ())
+ return false;
+
+ // Remove it from all renderers
+ if (removeActorFromRenderer (am_it->second))
+ return shape_actor_map_->erase (am_it), true;
+
+ return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeAllPointClouds ()
+{
+ // Check to see if the given ID entry exists
+ CloudActorMap::iterator am_it = cloud_actor_map_->begin ();
+ while (am_it != cloud_actor_map_->end () )
+ {
+ if (removePointCloud (am_it->first))
+ am_it = cloud_actor_map_->begin ();
+ else
+ ++am_it;
+ }
+ return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeAllShapes ()
+{
+ // Check to see if the given ID entry exists
+ ShapeActorMap::iterator am_it = shape_actor_map_->begin ();
+ while (am_it != shape_actor_map_->end ())
+ {
+ if (removeShape (am_it->first))
+ am_it = shape_actor_map_->begin ();
+ else
+ ++am_it;
+ }
+ return (true);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor)
+{
+ vtkLODActor* actor_to_remove = vtkLODActor::SafeDownCast (actor);
+
+
+
+ // Iterate over all actors in this renderer
+ vtkPropCollection* actors = renderer_->GetViewProps ();
+ actors->InitTraversal ();
+
+ vtkProp* current_actor = NULL;
+ while ((current_actor = actors->GetNextProp ()) != NULL)
+ {
+ if (current_actor != actor_to_remove)
+ continue;
+ renderer_->RemoveActor (actor);
+ // renderer->Render ();
+ // Found the correct viewport and removed the actor
+ return (true);
+ }
+
+ return false;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor)
+{
+ vtkActor* actor_to_remove = vtkActor::SafeDownCast (actor);
+
+ // Add it to all renderers
+ //rens_->InitTraversal ();
+
+
+ // Iterate over all actors in this renderer
+ vtkPropCollection* actors = renderer_->GetViewProps ();
+ actors->InitTraversal ();
+ vtkProp* current_actor = NULL;
+ while ((current_actor = actors->GetNextProp ()) != NULL)
+ {
+ if (current_actor != actor_to_remove)
+ continue;
+ renderer_->RemoveActor (actor);
+ // renderer->Render ();
+ // Found the correct viewport and removed the actor
+ return (true);
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor)
+{
+ vtkProp* actor_to_remove = vtkProp::SafeDownCast(actor);
+
+ vtkPropCollection* actors = renderer_->GetViewProps ();
+ actors->InitTraversal ();
+ vtkProp* current_actor = NULL;
+ while ((current_actor = actors->GetNextProp ()) != NULL)
+ {
+ if (current_actor != actor_to_remove)
+ continue;
+ renderer_->RemoveActor (actor);
+ return true;
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor, bool use_scalars)
+{
+ if (!actor)
+ actor = vtkSmartPointer<vtkLODActor>::New ();
+
+ vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+ mapper->SetInput (data);
+
+ if (use_scalars)
+ {
+ vtkSmartPointer<vtkDataArray> scalars = data->GetPointData ()->GetScalars ();
+ if (scalars)
+ {
+ cv::Vec3d minmax(scalars->GetRange());
+ mapper->SetScalarRange(minmax.val);
+ mapper->SetScalarModeToUsePointData ();
+
+ // interpolation OFF, if data is a vtkPolyData that contains only vertices, ON for anything else.
+ vtkPolyData* polyData = vtkPolyData::SafeDownCast (data);
+ bool interpolation = (polyData && polyData->GetNumberOfCells () != polyData->GetNumberOfVerts ());
+
+ mapper->SetInterpolateScalarsBeforeMapping (interpolation);
+ mapper->ScalarVisibilityOn ();
+ }
+ }
+ mapper->ImmediateModeRenderingOff ();
+
+ actor->SetNumberOfCloudPoints (int (std::max<vtkIdType> (1, data->GetNumberOfPoints () / 10)));
+ actor->GetProperty ()->SetInterpolationToFlat ();
+
+ /// FIXME disabling backface culling due to known VTK bug: vtkTextActors are not
+ /// shown when there is a vtkActor with backface culling on present in the scene
+ /// Please see VTK bug tracker for more details: http://www.vtk.org/Bug/view.php?id=12588
+ // actor->GetProperty ()->BackfaceCullingOn ();
+
+ actor->SetMapper (mapper);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setBackgroundColor (const Color& color)
+{
+ Color c = vtkcolor(color);
+ renderer_->SetBackground (c.val);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setPointCloudColor (const Color& color, const std::string &id)
+{
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it != cloud_actor_map_->end ())
+ {
+ vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+ Color c = vtkcolor(color);
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ actor->Modified ();
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::getPointCloudRenderingProperties (int property, double &value, const std::string &id)
+{
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return false;
+
+ vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+ switch (property)
+ {
+ case VIZ_POINT_SIZE:
+ {
+ value = actor->GetProperty ()->GetPointSize ();
+ actor->Modified ();
+ break;
+ }
+ case VIZ_OPACITY:
+ {
+ value = actor->GetProperty ()->GetOpacity ();
+ actor->Modified ();
+ break;
+ }
+ case VIZ_LINE_WIDTH:
+ {
+ value = actor->GetProperty ()->GetLineWidth ();
+ actor->Modified ();
+ break;
+ }
+ default:
+ CV_Assert("getPointCloudRenderingProperties: Unknown property");
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setPointCloudRenderingProperties (int property, double value, const std::string &id)
+{
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;
+
+ vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+
+ switch (property)
+ {
+ case VIZ_POINT_SIZE:
+ {
+ actor->GetProperty ()->SetPointSize (float (value));
+ actor->Modified ();
+ break;
+ }
+ case VIZ_OPACITY:
+ {
+ actor->GetProperty ()->SetOpacity (value);
+ actor->Modified ();
+ break;
+ }
+ // Turn on/off flag to control whether data is rendered using immediate
+ // mode or note. Immediate mode rendering tends to be slower but it can
+ // handle larger datasets. The default value is immediate mode off. If you
+ // are having problems rendering a large dataset you might want to consider
+ // using immediate more rendering.
+ case VIZ_IMMEDIATE_RENDERING:
+ {
+ actor->GetMapper ()->SetImmediateModeRendering (int (value));
+ actor->Modified ();
+ break;
+ }
+ case VIZ_LINE_WIDTH:
+ {
+ actor->GetProperty ()->SetLineWidth (float (value));
+ actor->Modified ();
+ break;
+ }
+ default:
+ CV_Assert("setPointCloudRenderingProperties: Unknown property");
+ }
+ return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setPointCloudSelected (const bool selected, const std::string &id)
+{
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+ if (am_it == cloud_actor_map_->end ())
+ return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;
+
+ vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (selected)
+ {
+ actor->GetProperty ()->EdgeVisibilityOn ();
+ actor->GetProperty ()->SetEdgeColor (1.0, 0.0, 0.0);
+ actor->Modified ();
+ }
+ else
+ {
+ actor->GetProperty ()->EdgeVisibilityOff ();
+ actor->Modified ();
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setShapeColor (const Color& color, const std::string &id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+
+ Color c = vtkcolor(color);
+ actor->GetMapper ()->ScalarVisibilityOff ();
+ actor->GetProperty ()->SetColor (c.val);
+ actor->GetProperty ()->SetEdgeColor (c.val);
+ actor->GetProperty ()->SetAmbient (0.8);
+ actor->GetProperty ()->SetDiffuse (0.8);
+ actor->GetProperty ()->SetSpecular (0.8);
+ actor->GetProperty ()->SetLighting (0);
+ actor->Modified ();
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::setShapeRenderingProperties (int property, double value, const std::string &id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it == shape_actor_map_->end ())
+ return std::cout << "[setShapeRenderingProperties] Could not find any shape with id <" << id << ">!\n" << std::endl, false;
+
+ vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+
+ switch (property)
+ {
+ case VIZ_POINT_SIZE:
+ {
+ actor->GetProperty ()->SetPointSize (float (value));
+ actor->Modified ();
+ break;
+ }
+ case VIZ_OPACITY:
+ {
+ actor->GetProperty ()->SetOpacity (value);
+ actor->Modified ();
+ break;
+ }
+ case VIZ_LINE_WIDTH:
+ {
+ actor->GetProperty ()->SetLineWidth (float (value));
+ actor->Modified ();
+ break;
+ }
+ case VIZ_FONT_SIZE:
+ {
+ vtkTextActor* text_actor = vtkTextActor::SafeDownCast (am_it->second);
+ vtkSmartPointer<vtkTextProperty> tprop = text_actor->GetTextProperty ();
+ tprop->SetFontSize (int (value));
+ text_actor->Modified ();
+ break;
+ }
+ case VIZ_REPRESENTATION:
+ {
+ switch (int (value))
+ {
+ case REPRESENTATION_POINTS: actor->GetProperty ()->SetRepresentationToPoints (); break;
+ case REPRESENTATION_WIREFRAME: actor->GetProperty ()->SetRepresentationToWireframe (); break;
+ case REPRESENTATION_SURFACE: actor->GetProperty ()->SetRepresentationToSurface (); break;
+ }
+ actor->Modified ();
+ break;
+ }
+ case VIZ_SHADING:
+ {
+ switch (int (value))
+ {
+ case SHADING_FLAT: actor->GetProperty ()->SetInterpolationToFlat (); break;
+ case SHADING_GOURAUD:
+ {
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
+ {
+ std::cout << "[temp_viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Gouraud shading was requested. Estimating normals...\n" << std::endl;
+
+ vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
+ normals->SetInput (actor->GetMapper ()->GetInput ());
+ normals->Update ();
+ vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
+ }
+ actor->GetProperty ()->SetInterpolationToGouraud ();
+ break;
+ }
+ case SHADING_PHONG:
+ {
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
+ {
+ std::cout << "[temp_viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Phong shading was requested. Estimating normals...\n" << std::endl;
+ vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
+ normals->SetInput (actor->GetMapper ()->GetInput ());
+ normals->Update ();
+ vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
+ }
+ actor->GetProperty ()->SetInterpolationToPhong ();
+ break;
+ }
+ }
+ actor->Modified ();
+ break;
+ }
+ default:
+ CV_Assert("setShapeRenderingProperties: Unknown property");
+
+ }
+ return true;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::initCameraParameters ()
+{
+ Camera camera_temp;
+ // Set default camera parameters to something meaningful
+ camera_temp.clip = Vec2d(0.01, 1000.01);
+
+ // Look straight along the z-axis
+ camera_temp.focal = Vec3d(0.0, 0.0, 1.0);
+
+ // Position the camera at the origin
+ camera_temp.pos = Vec3d(0.0, 0.0, 0.0);
+
+ // Set the up-vector of the camera to be the y-axis
+ camera_temp.view_up = Vec3d(0.0, 1.0, 0.0);
+
+ // Set the camera field of view to about
+ camera_temp.fovy = 0.8575;
+ camera_temp.window_size = Vec2i(window_->GetScreenSize()) / 2;
+ camera_temp.window_pos = Vec2i(0, 0);
+
+ setCameraParameters (camera_temp);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::cameraParamsSet () const { return (camera_set_); }
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::updateCamera ()
+{
+ std::cout << "[temp_viz::PCLVisualizer::updateCamera()] This method was deprecated, just re-rendering all scenes now." << std::endl;
+ //rens_->InitTraversal ();
+ // Update the camera parameters
+
+ renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::updateShapePose (const std::string &id, const cv::Affine3f& pose)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+
+ vtkLODActor* actor;
+
+ if (am_it == shape_actor_map_->end ())
+ return (false);
+ else
+ actor = vtkLODActor::SafeDownCast (am_it->second);
+
+ vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+
+ convertToVtkMatrix (pose.matrix, matrix);
+
+ actor->SetUserMatrix (matrix);
+ actor->Modified ();
+
+ return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::getCameras (temp_viz::Camera& camera)
+{
+ vtkCamera* active_camera = renderer_->GetActiveCamera ();
+
+ camera.pos = cv::Vec3d(active_camera->GetPosition());
+ camera.focal = cv::Vec3d(active_camera->GetFocalPoint());
+ camera.clip = cv::Vec2d(active_camera->GetClippingRange());
+ camera.view_up = cv::Vec3d(active_camera->GetViewUp());
+
+ camera.fovy = active_camera->GetViewAngle()/ 180.0 * CV_PI;
+ camera.window_size = cv::Vec2i(renderer_->GetRenderWindow()->GetSize());
+ camera.window_pos = cv::Vec2d::all(0);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+cv::Affine3f temp_viz::Viz3d::VizImpl::getViewerPose ()
+{
+ vtkCamera& camera = *renderer_->GetActiveCamera ();
+
+ Vec3d pos(camera.GetPosition());
+ Vec3d view_up(camera.GetViewUp());
+ Vec3d focal(camera.GetFocalPoint());
+
+ Vec3d y_axis = normalized(view_up);
+ Vec3d z_axis = normalized(focal - pos);
+ Vec3d x_axis = normalized(y_axis.cross(z_axis));
+
+ cv::Matx33d R;
+ R(0, 0) = x_axis[0];
+ R(0, 1) = y_axis[0];
+ R(0, 2) = z_axis[0];
+
+ R(1, 0) = x_axis[1];
+ R(1, 1) = y_axis[1];
+ R(1, 2) = z_axis[1];
+
+ R(2, 0) = x_axis[2];
+ R(2, 1) = y_axis[2];
+ R(2, 2) = z_axis[2];
+
+ return cv::Affine3f(R, pos);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::resetCamera ()
+{
+ renderer_->ResetCamera ();
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraPosition (const cv::Vec3d& pos, const cv::Vec3d& view, const cv::Vec3d& up)
+{
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetPosition (pos[0], pos[1], pos[2]);
+ cam->SetFocalPoint (view[0], view[1], view[2]);
+ cam->SetViewUp (up[0], up[1], up[2]);
+ renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraPosition (double pos_x, double pos_y, double pos_z, double up_x, double up_y, double up_z)
+{
+ //rens_->InitTraversal ();
+
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetPosition (pos_x, pos_y, pos_z);
+ cam->SetViewUp (up_x, up_y, up_z);
+ renderer_->Render ();
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics)
+{
+ // Position = extrinsic translation
+ cv::Vec3f pos_vec = extrinsics.translation();
+
+
+ // Rotate the view vector
+ cv::Matx33f rotation = extrinsics.rotation();
+ cv::Vec3f y_axis (0.f, 1.f, 0.f);
+ cv::Vec3f up_vec (rotation * y_axis);
+
+ // Compute the new focal point
+ cv::Vec3f z_axis (0.f, 0.f, 1.f);
+ cv::Vec3f focal_vec = pos_vec + rotation * z_axis;
+
+ // Get the width and height of the image - assume the calibrated centers are at the center of the image
+ Eigen::Vector2i window_size;
+ window_size[0] = static_cast<int> (intrinsics(0, 2));
+ window_size[1] = static_cast<int> (intrinsics(1, 2));
+
+ // Compute the vertical field of view based on the focal length and image heigh
+ double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI;
+
+ //rens_->InitTraversal ();
+
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]);
+ cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]);
+ cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]);
+ cam->SetUseHorizontalViewAngle (0);
+ cam->SetViewAngle (fovy);
+ cam->SetClippingRange (0.01, 1000.01);
+ window_->SetSize (window_size[0], window_size[1]);
+
+ renderer_->Render ();
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraParameters (const temp_viz::Camera &camera)
+{
+ //rens_->InitTraversal ();
+
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetPosition (camera.pos[0], camera.pos[1], camera.pos[2]);
+ cam->SetFocalPoint (camera.focal[0], camera.focal[1], camera.focal[2]);
+ cam->SetViewUp (camera.view_up[0], camera.view_up[1], camera.view_up[2]);
+ cam->SetClippingRange (camera.clip.val);
+ cam->SetUseHorizontalViewAngle (0);
+ cam->SetViewAngle (camera.fovy * 180.0 / M_PI);
+
+ window_->SetSize (static_cast<int> (camera.window_size[0]), static_cast<int> (camera.window_size[1]));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraClipDistances (double near, double far)
+{
+ //rens_->InitTraversal ();
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetClippingRange (near, far);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setCameraFieldOfView (double fovy)
+{
+ //rens_->InitTraversal ();
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetUseHorizontalViewAngle (0);
+ cam->SetViewAngle (fovy * 180.0 / M_PI);
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::resetCameraViewpoint (const std::string &id)
+{
+ vtkSmartPointer<vtkMatrix4x4> camera_pose;
+ static CloudActorMap::iterator it = cloud_actor_map_->find (id);
+ if (it != cloud_actor_map_->end ())
+ camera_pose = it->second.viewpoint_transformation_;
+ else
+ return;
+
+ // Prevent a segfault
+ if (!camera_pose)
+ return;
+
+ // set all renderer to this viewpoint
+ //rens_->InitTraversal ();
+
+
+ vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
+ cam->SetPosition (camera_pose->GetElement (0, 3),
+ camera_pose->GetElement (1, 3),
+ camera_pose->GetElement (2, 3));
+
+ cam->SetFocalPoint (camera_pose->GetElement (0, 3) - camera_pose->GetElement (0, 2),
+ camera_pose->GetElement (1, 3) - camera_pose->GetElement (1, 2),
+ camera_pose->GetElement (2, 3) - camera_pose->GetElement (2, 2));
+
+ cam->SetViewUp (camera_pose->GetElement (0, 1),
+ camera_pose->GetElement (1, 1),
+ camera_pose->GetElement (2, 1));
+
+ renderer_->SetActiveCamera (cam);
+ renderer_->ResetCameraClippingRange ();
+ renderer_->Render ();
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addCylinder] A shape with id <"<<id <<"> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkDataSet> data = createCylinder (coefficients);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkDataSet> data = createCube (coefficients);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ Eigen::Vector3f t(translation[0], translation[1], translation[2]);
+ Eigen::Quaternionf q(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+
+ vtkSmartPointer<vtkDataSet> data = createCube (t, q, width, height, depth);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
+ const Color& color , const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
+
+ vtkSmartPointer<vtkDataSet> data = createCube (x_min, x_max, y_min, y_max, z_min, z_max);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+
+ Color c = vtkcolor(color);
+ actor->GetProperty ()->SetColor (c.val);
+ renderer_->AddActor (actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addModelFromPolyData] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (polydata, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string & id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addModelFromPolyData] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
+ trans_filter->SetTransform (transform);
+ trans_filter->SetInput (polydata);
+ trans_filter->Update();
+
+ // Create an Actor
+ vtkSmartPointer <vtkLODActor> actor;
+ createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, const std::string &id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.." << std::endl, false;
+
+ vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New ();
+ reader->SetFileName (filename.c_str ());
+
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (reader->GetOutput (), actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ renderer_->AddActor(actor);
+
+ (*shape_actor_map_)[id] = actor;
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id)
+{
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl, false;
+
+ vtkSmartPointer <vtkPLYReader > reader = vtkSmartPointer<vtkPLYReader>::New ();
+ reader->SetFileName (filename.c_str ());
+
+ vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
+ trans_filter->SetTransform (transform);
+ trans_filter->SetInputConnection (reader->GetOutputPort ());
+
+ vtkSmartPointer <vtkLODActor> actor;
+ createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ renderer_->AddActor(actor);
+
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Add a plane from a set of given model coefficients
+ * \param coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
+ * \param id the plane id/name (default: "plane")
+ * \param viewport (optional) the id of the new viewport (default: 0)
+ */
+bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addPlane] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkDataSet> data = createPlane (coefficients);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ // actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
+ actor->GetProperty ()->SetLighting (false);
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addPlane] A shape with id <" << id << "> already exists! Please choose a different id and retry.\n" << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkDataSet> data = createPlane (coefficients, x, y, z);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ {
+ std::cout << "[addCircle] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.\n" << std::endl;
+ return (false);
+ }
+
+ vtkSmartPointer<vtkDataSet> data = create2DCircle (coefficients);
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting (false);
+
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
+{
+ std::string tid = id.empty() ? text : id;
+
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addText] A text with id <"<<id <<"> already exists! Please choose a different id and retry.\n" << std::endl, false;
+
+ // Create an Actor
+ vtkSmartPointer<vtkTextActor> actor = vtkSmartPointer<vtkTextActor>::New ();
+ actor->SetPosition (xpos, ypos);
+ actor->SetInput (text.c_str ());
+
+ vtkSmartPointer<vtkTextProperty> tprop = actor->GetTextProperty ();
+ tprop->SetFontSize (fontsize);
+ tprop->SetFontFamilyToArial ();
+ tprop->SetJustificationToLeft ();
+ tprop->BoldOn ();
+
+ Color c = vtkcolor(color);
+ tprop->SetColor (c.val);
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[tid] = actor;
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool temp_viz::Viz3d::VizImpl::updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
+{
+ std::string tid = id.empty() ? text : id;
+
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
+ if (am_it == shape_actor_map_->end ())
+ return false;
+
+ // Retrieve the Actor
+ vtkTextActor *actor = vtkTextActor::SafeDownCast (am_it->second);
+
+ actor->SetPosition (xpos, ypos);
+ actor->SetInput (text.c_str ());
+
+ vtkTextProperty* tprop = actor->GetTextProperty ();
+ tprop->SetFontSize (fontsize);
+
+ Color c = vtkcolor(color);
+ tprop->SetColor (c.val);
+
+ actor->Modified ();
+
+ return (true);
+}
+
+bool temp_viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
+{
+ CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
+
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+ if (am_it != shape_actor_map_->end ())
+ return std::cout << "[addPolylineFromPolygonMesh] A shape with id <"<< id << "> already exists! Please choose a different id and retry.\n" << std::endl, false;
+
+ vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
+ poly_points->SetNumberOfPoints (mesh.cloud.size().area());
+
+ const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
+ for (int i = 0; i < mesh.cloud.cols; ++i)
+ poly_points->InsertPoint (i, cdata[i].x, cdata[i].y,cdata[i].z);
+
+
+ // Create a cell array to store the lines in and add the lines to it
+ vtkSmartPointer <vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
+ vtkSmartPointer <vtkPolyData> polyData;
+ allocVtkPolyData (polyData);
+
+ for (size_t i = 0; i < mesh.polygons.size (); i++)
+ {
+ vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
+ polyLine->GetPointIds()->SetNumberOfIds(mesh.polygons[i].vertices.size());
+ for(unsigned int k = 0; k < mesh.polygons[i].vertices.size(); k++)
+ {
+ polyLine->GetPointIds()->SetId(k,mesh. polygons[i].vertices[k]);
+ }
+
+ cells->InsertNextCell (polyLine);
+ }
+
+ // Add the points to the dataset
+ polyData->SetPoints (poly_points);
+
+ // Add the lines to the dataset
+ polyData->SetLines (cells);
+
+ // Setup actor and mapper
+ vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+ mapper->SetInput (polyData);
+
+ vtkSmartPointer <vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
+ actor->SetMapper (mapper);
+ renderer_->AddActor(actor);
+
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ return (true);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToSurfaceForAllActors ()
+{
+ vtkActorCollection * actors = renderer_->GetActors ();
+ actors->InitTraversal ();
+ vtkActor * actor;
+ while ((actor = actors->GetNextActor ()) != NULL)
+ actor->GetProperty ()->SetRepresentationToSurface ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToPointsForAllActors ()
+{
+ vtkActorCollection * actors = renderer_->GetActors ();
+ actors->InitTraversal ();
+ vtkActor * actor;
+ while ((actor = actors->GetNextActor ()) != NULL)
+ actor->GetProperty ()->SetRepresentationToPoints ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::setRepresentationToWireframeForAllActors ()
+{
+ vtkActorCollection * actors = renderer_->GetActors ();
+ actors->InitTraversal ();
+ vtkActor *actor;
+ while ((actor = actors->GetNextActor ()) != NULL)
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::updateCells (vtkSmartPointer<vtkIdTypeArray> &cells, vtkSmartPointer<vtkIdTypeArray> &initcells, vtkIdType nr_points)
+{
+ // If no init cells and cells has not been initialized...
+ if (!cells)
+ cells = vtkSmartPointer<vtkIdTypeArray>::New ();
+
+ // If we have less values then we need to recreate the array
+ if (cells->GetNumberOfTuples () < nr_points)
+ {
+ cells = vtkSmartPointer<vtkIdTypeArray>::New ();
+
+ // If init cells is given, and there's enough data in it, use it
+ if (initcells && initcells->GetNumberOfTuples () >= nr_points)
+ {
+ cells->DeepCopy (initcells);
+ cells->SetNumberOfComponents (2);
+ cells->SetNumberOfTuples (nr_points);
+ }
+ else
+ {
+ // If the number of tuples is still too small, we need to recreate the array
+ cells->SetNumberOfComponents (2);
+ cells->SetNumberOfTuples (nr_points);
+ vtkIdType *cell = cells->GetPointer (0);
+ // Fill it with 1s
+ std::fill_n (cell, nr_points * 2, 1);
+ cell++;
+ for (vtkIdType i = 0; i < nr_points; ++i, cell += 2)
+ *cell = i;
+ // Save the results in initcells
+ initcells = vtkSmartPointer<vtkIdTypeArray>::New ();
+ initcells->DeepCopy (cells);
+ }
+ }
+ else
+ {
+ // The assumption here is that the current set of cells has more data than needed
+ cells->SetNumberOfComponents (2);
+ cells->SetNumberOfTuples (nr_points);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata)
+{
+ polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata)
+{
+ polydata = vtkSmartPointer<vtkPolyData>::New ();
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::Viz3d::VizImpl::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
+{
+ polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix)
+{
+ // set rotation
+ Eigen::Matrix3f rot = orientation.toRotationMatrix ();
+ for (int i = 0; i < 3; i++)
+ for (int k = 0; k < 3; k++)
+ vtk_matrix->SetElement (i, k, rot (i, k));
+
+ // set translation
+ vtk_matrix->SetElement (0, 3, origin (0));
+ vtk_matrix->SetElement (1, 3, origin (1));
+ vtk_matrix->SetElement (2, 3, origin (2));
+ vtk_matrix->SetElement (3, 3, 1.0f);
+}
+
+void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix)
+{
+ for (int i = 0; i < 4; i++)
+ for (int k = 0; k < 4; k++)
+ vtk_matrix->SetElement (i, k, m (i, k));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
+{
+ for (int i = 0; i < 4; i++)
+ for (int k = 0; k < 4; k++)
+ m (i,k) = static_cast<float> (vtk_matrix->GetElement (i, k));
+}
--- /dev/null
+#include "test_precomp.hpp"
+
+CV_TEST_MAIN("cv")
--- /dev/null
+#include "test_precomp.hpp"
--- /dev/null
+#ifdef __GNUC__
+# pragma GCC diagnostic ignored "-Wmissing-declarations"
+# if defined __clang__ || defined __APPLE__
+# pragma GCC diagnostic ignored "-Wmissing-prototypes"
+# pragma GCC diagnostic ignored "-Wextra"
+# endif
+#endif
+
+#ifndef __OPENCV_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include "opencv2/ts.hpp"
+#include "opencv2/core/core_c.h"
+#include <iostream>
+
+#endif
--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ // By downloading, copying, installing or using the software you agree to this license.
+ // If you do not agree to this license, do not download, install,
+ // copy or use the software.
+ //
+ //
+ // License Agreement
+ // For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+ // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ // * Redistribution's of source code must retain the above copyright notice,
+ // this list of conditions and the following disclaimer.
+ //
+ // * Redistribution's in binary form must reproduce the above copyright notice,
+ // this list of conditions and the following disclaimer in the documentation
+ // and / or other materials provided with the distribution.
+ //
+ // * The name of the copyright holders may not be used to endorse or promote products
+ // derived from this software without specific prior written permission.
+ //
+ // This software is provided by the copyright holders and contributors "as is" and
+ // any express or implied warranties, including, but not limited to, the implied
+ // warranties of merchantability and fitness for a particular purpose are disclaimed.
+ // In no event shall the Intel Corporation or contributors be liable for any direct,
+ // indirect, incidental, special, exemplary, or consequential damages
+ // (including, but not limited to, procurement of substitute goods or services;
+ // loss of use, data, or profits; or business interruption) however caused
+ // and on any theory of liability, whether in contract, strict liability,
+ // or tort (including negligence or otherwise) arising in any way out of
+ // the use of this software, even if advised of the possibility of such damage.
+ //
+ //M*/
+#include "test_precomp.hpp"
+#include <opencv2/viz.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <fstream>
+#include <string>
+
+#include <opencv2/viz/types.hpp>
+#include <opencv2/viz/mesh_load.hpp>
+
+
+cv::Mat cvcloud_load()
+{
+ cv::Mat cloud(1, 20000, CV_32FC3);
+ std::ifstream ifs("d:/cloud_dragon.ply");
+
+ std::string str;
+ for(size_t i = 0; i < 11; ++i)
+ std::getline(ifs, str);
+
+ cv::Point3f* data = cloud.ptr<cv::Point3f>();
+ for(size_t i = 0; i < 20000; ++i)
+ ifs >> data[i].x >> data[i].y >> data[i].z;
+
+ return cloud;
+}
+
+TEST(Viz_viz3d, accuracy)
+{
+ temp_viz::Viz3d v("abc");
+ //v.spin();
+
+ v.setBackgroundColor();
+
+ v.addCoordinateSystem(1.0, cv::Affine3f::Identity());
+
+ cv::Mat cloud = cvcloud_load();
+
+
+ cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
+ v.addPointCloud(cloud, colors);
+ cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));
+
+ v.addPointCloudNormals(cloud, normals, 100, 0.02, "n");
+
+
+ temp_viz::ModelCoefficients mc;
+ mc.values.resize(4);
+ mc.values[0] = mc.values[1] = mc.values[2] = mc.values[3] = 1;
+ v.addPlane(mc);
+
+
+ temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply");
+ v.addPolygonMesh(*mesh, "pq");
+
+ v.spinOnce(1000, true);
+
+ v.removeCoordinateSystem();
+
+ for(int i = 0; i < mesh->cloud.cols; ++i)
+ mesh->cloud.ptr<cv::Point3f>()[i] += cv::Point3f(1, 1, 1);
+
+ v.updatePolygonMesh(*mesh, "pq");
+
+
+ for(int i = 0; i < mesh->cloud.cols; ++i)
+ mesh->cloud.ptr<cv::Point3f>()[i] -= cv::Point3f(2, 2, 2);
+ v.addPolylineFromPolygonMesh(*mesh);
+
+
+ v.addText("===Abd sadfljsadlk", 100, 100, cv::Scalar(255, 0, 0), 15);
+ for(int i = 0; i < cloud.cols; ++i)
+ cloud.ptr<cv::Point3f>()[i].x *=2;
+
+ colors.setTo(cv::Scalar(255, 0, 0));
+
+ v.addSphere(cv::Point3f(0, 0, 0), 0.3, temp_viz::Color::blue());
+
+ cv::Mat cvpoly(1, 5, CV_32FC3);
+ cv::Point3f* pdata = cvpoly.ptr<cv::Point3f>();
+ pdata[0] = cv::Point3f(0, 0, 0);
+ pdata[1] = cv::Point3f(0, 1, 1);
+ pdata[2] = cv::Point3f(3, 1, 2);
+ pdata[3] = cv::Point3f(0, 2, 4);
+ pdata[4] = cv::Point3f(7, 2, 3);
+ v.addPolygon(cvpoly, temp_viz::Color::white());
+
+ v.updatePointCloud(cloud, colors);
+ v.spin();
+}
+
projects / profile / ivi / opencv.git / commitdiff