+++ /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);
-}
class CV_EXPORTS Mesh3d
{
public:
- typedef cv::Ptr<Mesh3d> Ptr;
+ typedef Ptr<Mesh3d> Ptr;
Mat cloud, colors;
std::vector<Vertices> polygons;
+
+ static Mesh3d::Ptr mesh_load(const String& file);
+
};
/////////////////////////////////////////////////////////////////////////////
-#include <q/common.h>
+#include <common.h>
#include <cstdlib>
#include <opencv2/viz/types.hpp>
--- /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, const 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;
+ }
+ };
+
+}
#include "precomp.hpp"
-#include <q/interactor_style.h>
+#include "interactor_style.h"
//#include <q/visualization/vtk/vtkVertexBufferObjectMapper.h>
--- /dev/null
+#pragma once
+
+#include "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 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] ccallback function that will be registered as a callback for a mouse event
+ * \param[in] cookie for passing user data to callback
+ */
+ void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0);
+
+ /** \brief Register a callback function for keyboard events
+ * \param[in] callback a function that will be registered as a callback for a keyboard event
+ * \param[in] cookie user data passed to the callback function
+ */
+ void registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void * cookie = 0);
+
+ /** \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_;
+
+ /** \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_;
+
+ /** \brief KeyboardEvent callback function pointer*/
+ void (*keyboardCallback_)(const KeyboardEvent&, void*);
+ /** \brief KeyboardEvent callback user data*/
+ void *keyboard_callback_cookie_;
+
+ /** \brief MouseEvent callback function pointer */
+ void (*mouseCallback_)(const MouseEvent&, void*);
+ /** \brief MouseEvent callback user data */
+ void *mouse_callback_cookie_;
+ };
+}
-#include <opencv2/viz/mesh_load.hpp>
-
#include "precomp.hpp"
#include <vtkPLYReader.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
-temp_viz::Mesh3d::Ptr temp_viz::mesh_load(const String& file)
+temp_viz::Mesh3d::Ptr temp_viz::Mesh3d::mesh_load(const String& file)
{
Mesh3d::Ptr mesh = new Mesh3d();
#endif
-#include <q/viz3d_impl.hpp>
+#include "viz3d_impl.hpp"
#include <opencv2/core.hpp>
#include <opencv2/viz.hpp>
#include "opencv2/viz/widget_accessor.hpp"
+++ /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, const 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 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] ccallback function that will be registered as a callback for a mouse event
- * \param[in] cookie for passing user data to callback
- */
- void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0);
-
- /** \brief Register a callback function for keyboard events
- * \param[in] callback a function that will be registered as a callback for a keyboard event
- * \param[in] cookie user data passed to the callback function
- */
- void registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void * cookie = 0);
-
- /** \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_;
-
- /** \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_;
-
- /** \brief KeyboardEvent callback function pointer*/
- void (*keyboardCallback_)(const KeyboardEvent&, void*);
- /** \brief KeyboardEvent callback user data*/
- void *keyboard_callback_cookie_;
-
- /** \brief MouseEvent callback function pointer */
- void (*mouseCallback_)(const MouseEvent&, void*);
- /** \brief MouseEvent callback user data */
- void *mouse_callback_cookie_;
- };
-}
+++ /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
-{
-
-struct Viz3d::VizImpl
-{
-public:
- typedef cv::Ptr<VizImpl> Ptr;
-
- VizImpl (const String &name = String());
-
- virtual ~VizImpl ();
- void setFullScreen (bool mode);
- void setWindowName (const String &name);
-
- /** \brief Register a callback function for keyboard input
- * \param[in] callback function that will be registered as a callback for a keyboard event
- * \param[in] cookie for passing user data to callback
- */
- void registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie = 0);
-
- /** \brief Register a callback function for mouse events
- * \param[in] ccallback function that will be registered as a callback for a mouse event
- * \param[in] cookie for passing user data to callback
- */
- void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0);
-
- void spin ();
- void spinOnce (int time = 1, bool force_redraw = false);
-
- bool removePointCloud (const String& id = "cloud");
- inline bool removePolygonMesh (const String& id = "polygon")
- {
- // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
- return removePointCloud (id);
- }
- bool removeShape (const String& id = "cloud");
-
- bool removeText3D (const String& id = "cloud");
- bool removeAllPointClouds ();
- bool removeAllShapes ();
-
- void setBackgroundColor (const Color& color);
-
- bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
- bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
-
- bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline");
-
- bool setPointCloudRenderingProperties (int property, double value, const String& id = "cloud");
- bool getPointCloudRenderingProperties (int property, double &value, const String& id = "cloud");
-
- bool setShapeRenderingProperties (int property, double value, const 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 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 String& id = "polygon");
- bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color, bool display_length, const String& id = "arrow");
- bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color_line, const Color& color_text, const String& id = "arrow");
-
- // Add a vtkPolydata as a mesh
- bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const String& id = "PolyData");
- bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const String& id = "PolyData");
- bool addModelFromPLYFile (const String &filename, const String& id = "PLYModel");
- bool addModelFromPLYFile (const String &filename, vtkSmartPointer<vtkTransform> transform, const String& id = "PLYModel");
-
- /** \brief Changes the visual representation for all actors to surface representation. */
- void setRepresentationToSurfaceForAllActors ();
-
- /** \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 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 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. */
- Affine3f getViewerPose ();
- void saveScreenshot (const 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);
-
- void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity());
- void removeWidget(const String &id);
- Widget getWidget(const String &id) const;
-
- void setWidgetPose(const String &id, const Affine3f &pose);
- void updateWidgetPose(const String &id, const Affine3f &pose);
- Affine3f getWidgetPose(const String &id) const;
-
- void all_data();
-
-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 Internal list with actor pointers and name IDs for all widget actors */
- cv::Ptr<WidgetActorMap> widget_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);
-void convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m);
-
-vtkSmartPointer<vtkMatrix4x4> convertToVtkMatrix (const cv::Matx44f &m);
-cv::Matx44f convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matrix);
-
-/** \brief Convert origin and orientation to vtkMatrix4x4
- * \param[in] origin the point cloud origin
- * \param[in] orientation the point cloud orientation
- * \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);
-
-
-struct NanFilter
-{
- template<typename _Tp, typename _Msk>
- struct Impl
- {
- typedef Vec<_Tp, 3> _Out;
-
- static _Out* copy(const Mat& source, _Out* output, const Mat& nan_mask)
- {
- CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
- CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
- CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
-
- int s_chs = source.channels();
- int m_chs = nan_mask.channels();
-
- for(int y = 0; y < source.rows; ++y)
- {
- const _Tp* srow = source.ptr<_Tp>(y);
- const _Msk* mrow = nan_mask.ptr<_Msk>(y);
-
- for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
- if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
- *output++ = _Out(srow);
- }
- return output;
- }
- };
-
- template<typename _Tp>
- static inline Vec<_Tp, 3>* copy(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
- {
- CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
-
- typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
- const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copy, &NanFilter::Impl<_Tp, double>::copy };
-
- return table[nan_mask.depth() - 5](source, output, nan_mask);
- }
-};
-
-struct ApplyAffine
-{
- const Affine3f& affine_;
- ApplyAffine(const Affine3f& affine) : affine_(affine) {}
-
- template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) const { return affine_ * p; }
-
- template<typename _Tp> Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v) const
- {
- const float* m = affine_.matrix.val;
-
- Vec<_Tp, 3> result;
- result[0] = (_Tp)(m[0] * v[0] + m[1] * v[1] + m[ 2] * v[2] + m[ 3]);
- result[1] = (_Tp)(m[4] * v[0] + m[5] * v[1] + m[ 6] * v[2] + m[ 7]);
- result[2] = (_Tp)(m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11]);
- return result;
- }
-
-private:
- ApplyAffine(const ApplyAffine&);
- ApplyAffine& operator=(const ApplyAffine&);
-};
-
-}
-
+++ /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;
- };
-
- // TODO This will be used to contain both cloud and shape actors
- struct CV_EXPORTS WidgetActor
- {
- vtkSmartPointer<vtkProp> actor;
- vtkSmartPointer<vtkMatrix4x4> viewpoint_transformation_;
- vtkSmartPointer<vtkIdTypeArray> cells;
- };
-
- typedef std::map<std::string, CloudActor> CloudActorMap;
- typedef std::map<std::string, vtkSmartPointer<vtkProp> > ShapeActorMap;
- typedef std::map<std::string, WidgetActor> WidgetActorMap;
-}
-
#include <opencv2/viz/viz3d.hpp>
-#include <q/viz3d_impl.hpp>
+#include "viz3d_impl.hpp"
temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_name))
+++ /dev/null
-#include "precomp.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); }
-
-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);
-}
-
-
-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);
-}
-
-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);
-}
-
-void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
-{
- WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- if (exists)
- {
- // Remove it if it exists and add it again
- removeActorFromRenderer(wam_itr->second.actor);
- }
- // Get the actor and set the user matrix
- vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(widget));
- if (actor)
- {
- // If the actor is 3D, apply pose
- vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
- actor->SetUserMatrix (matrix);
- actor->Modified();
- }
- // If the actor is a vtkFollower, then it should always face the camera
- vtkFollower *follower = vtkFollower::SafeDownCast(actor);
- if (follower)
- {
- follower->SetCamera(renderer_->GetActiveCamera());
- }
-
- renderer_->AddActor(WidgetAccessor::getProp(widget));
- (*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
-}
-
-void temp_viz::Viz3d::VizImpl::removeWidget(const String &id)
-{
- WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- CV_Assert(exists);
- CV_Assert(removeActorFromRenderer (wam_itr->second.actor));
- widget_actor_map_->erase(wam_itr);
-}
-
-temp_viz::Widget temp_viz::Viz3d::VizImpl::getWidget(const String &id) const
-{
- WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- CV_Assert(exists);
-
- Widget widget;
- WidgetAccessor::setProp(widget, wam_itr->second.actor);
- return widget;
-}
-
-void temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose)
-{
- WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- CV_Assert(exists);
-
- vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
- CV_Assert(actor);
-
- vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
- actor->SetUserMatrix (matrix);
- actor->Modified ();
-}
-
-void temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose)
-{
- WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- CV_Assert(exists);
-
- vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
- CV_Assert(actor);
-
- vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
- if (!matrix)
- {
- setWidgetPose(id, pose);
- return ;
- }
- Matx44f matrix_cv = convertToMatx(matrix);
- Affine3f updated_pose = pose * Affine3f(matrix_cv);
- matrix = convertToVtkMatrix(updated_pose.matrix);
-
- actor->SetUserMatrix (matrix);
- actor->Modified ();
-}
-
-temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) const
-{
- WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
- bool exists = wam_itr != widget_actor_map_->end();
- CV_Assert(exists);
-
- vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
- CV_Assert(actor);
-
- vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
- Matx44f matrix_cv = convertToMatx(matrix);
- return Affine3f(matrix_cv);
-}
--- /dev/null
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/viz/events.hpp>
+#include "interactor_style.h"
+#include "viz_types.h"
+#include "common.h"
+#include <opencv2/viz/types.hpp>
+#include <opencv2/core/affine.hpp>
+#include <opencv2/viz/viz3d.hpp>
+
+
+struct temp_viz::Viz3d::VizImpl
+{
+public:
+ typedef cv::Ptr<VizImpl> Ptr;
+
+ VizImpl (const String &name = String());
+
+ virtual ~VizImpl ();
+ void setFullScreen (bool mode);
+ void setWindowName (const String &name);
+
+ void registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie = 0);
+ void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0);
+
+ void spin ();
+ void spinOnce (int time = 1, bool force_redraw = false);
+
+ bool removePointCloud (const String& id = "cloud");
+ inline bool removePolygonMesh (const String& id = "polygon")
+ {
+ // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
+ return removePointCloud (id);
+ }
+ bool removeShape (const String& id = "cloud");
+
+ bool removeText3D (const String& id = "cloud");
+ bool removeAllPointClouds ();
+ bool removeAllShapes ();
+
+ void setBackgroundColor (const Color& color);
+
+ bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
+ bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
+
+ bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline");
+
+ bool setPointCloudRenderingProperties (int property, double value, const String& id = "cloud");
+ bool getPointCloudRenderingProperties (int property, double &value, const String& id = "cloud");
+
+ bool setShapeRenderingProperties (int property, double value, const 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 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 String& id = "polygon");
+ bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color, bool display_length, const String& id = "arrow");
+ bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color_line, const Color& color_text, const String& id = "arrow");
+
+ // Add a vtkPolydata as a mesh
+ bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const String& id = "PolyData");
+ bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const String& id = "PolyData");
+ bool addModelFromPLYFile (const String &filename, const String& id = "PLYModel");
+ bool addModelFromPLYFile (const String &filename, vtkSmartPointer<vtkTransform> transform, const String& id = "PLYModel");
+
+ /** \brief Changes the visual representation for all actors to surface representation. */
+ void setRepresentationToSurfaceForAllActors ();
+
+ /** \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 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 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. */
+ Affine3f getViewerPose ();
+ void saveScreenshot (const 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);
+
+ void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity());
+ void removeWidget(const String &id);
+ Widget getWidget(const String &id) const;
+
+ void setWidgetPose(const String &id, const Affine3f &pose);
+ void updateWidgetPose(const String &id, const Affine3f &pose);
+ Affine3f getWidgetPose(const String &id) const;
+
+ void all_data();
+
+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 Internal list with actor pointers and name IDs for all widget actors */
+ cv::Ptr<WidgetActorMap> widget_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);
+};
+
+
+
+namespace temp_viz
+{
+
+//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);
+void convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m);
+
+vtkSmartPointer<vtkMatrix4x4> convertToVtkMatrix (const cv::Matx44f &m);
+cv::Matx44f convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matrix);
+
+/** \brief Convert origin and orientation to vtkMatrix4x4
+ * \param[in] origin the point cloud origin
+ * \param[in] orientation the point cloud orientation
+ * \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);
+
+
+struct NanFilter
+{
+ template<typename _Tp, typename _Msk>
+ struct Impl
+ {
+ typedef Vec<_Tp, 3> _Out;
+
+ static _Out* copy(const Mat& source, _Out* output, const Mat& nan_mask)
+ {
+ CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
+ CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
+ CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
+
+ int s_chs = source.channels();
+ int m_chs = nan_mask.channels();
+
+ for(int y = 0; y < source.rows; ++y)
+ {
+ const _Tp* srow = source.ptr<_Tp>(y);
+ const _Msk* mrow = nan_mask.ptr<_Msk>(y);
+
+ for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
+ if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
+ *output++ = _Out(srow);
+ }
+ return output;
+ }
+ };
+
+ template<typename _Tp>
+ static inline Vec<_Tp, 3>* copy(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
+ {
+ CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
+
+ typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
+ const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copy, &NanFilter::Impl<_Tp, double>::copy };
+
+ return table[nan_mask.depth() - 5](source, output, nan_mask);
+ }
+};
+
+struct ApplyAffine
+{
+ const Affine3f& affine_;
+ ApplyAffine(const Affine3f& affine) : affine_(affine) {}
+
+ template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) const { return affine_ * p; }
+
+ template<typename _Tp> Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v) const
+ {
+ const float* m = affine_.matrix.val;
+
+ Vec<_Tp, 3> result;
+ result[0] = (_Tp)(m[0] * v[0] + m[1] * v[1] + m[ 2] * v[2] + m[ 3]);
+ result[1] = (_Tp)(m[4] * v[0] + m[5] * v[1] + m[ 6] * v[2] + m[ 7]);
+ result[2] = (_Tp)(m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11]);
+ return result;
+ }
+
+private:
+ ApplyAffine(const ApplyAffine&);
+ ApplyAffine& operator=(const ApplyAffine&);
+};
+
+}
+
#include "precomp.hpp"
#include <opencv2/calib3d.hpp>
-#include <q/viz3d_impl.hpp>
+#include "viz3d_impl.hpp"
#include <vtkRenderWindowInteractor.h>
#ifndef __APPLE__
for (int k = 0; k < 4; k++)
m (i,k) = static_cast<float> (vtk_matrix->GetElement (i, k));
}
+
+
+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); }
+
+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);
+}
+
+
+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);
+}
+
+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);
+}
+
+void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
+{
+ WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ if (exists)
+ {
+ // Remove it if it exists and add it again
+ removeActorFromRenderer(wam_itr->second.actor);
+ }
+ // Get the actor and set the user matrix
+ vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(widget));
+ if (actor)
+ {
+ // If the actor is 3D, apply pose
+ vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
+ actor->SetUserMatrix (matrix);
+ actor->Modified();
+ }
+ // If the actor is a vtkFollower, then it should always face the camera
+ vtkFollower *follower = vtkFollower::SafeDownCast(actor);
+ if (follower)
+ {
+ follower->SetCamera(renderer_->GetActiveCamera());
+ }
+
+ renderer_->AddActor(WidgetAccessor::getProp(widget));
+ (*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
+}
+
+void temp_viz::Viz3d::VizImpl::removeWidget(const String &id)
+{
+ WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ CV_Assert(exists);
+ CV_Assert(removeActorFromRenderer (wam_itr->second.actor));
+ widget_actor_map_->erase(wam_itr);
+}
+
+temp_viz::Widget temp_viz::Viz3d::VizImpl::getWidget(const String &id) const
+{
+ WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ CV_Assert(exists);
+
+ Widget widget;
+ WidgetAccessor::setProp(widget, wam_itr->second.actor);
+ return widget;
+}
+
+void temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose)
+{
+ WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ CV_Assert(exists);
+
+ vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
+ CV_Assert(actor);
+
+ vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
+ actor->SetUserMatrix (matrix);
+ actor->Modified ();
+}
+
+void temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose)
+{
+ WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ CV_Assert(exists);
+
+ vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
+ CV_Assert(actor);
+
+ vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
+ if (!matrix)
+ {
+ setWidgetPose(id, pose);
+ return ;
+ }
+ Matx44f matrix_cv = convertToMatx(matrix);
+ Affine3f updated_pose = pose * Affine3f(matrix_cv);
+ matrix = convertToVtkMatrix(updated_pose.matrix);
+
+ actor->SetUserMatrix (matrix);
+ actor->Modified ();
+}
+
+temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) const
+{
+ WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
+ bool exists = wam_itr != widget_actor_map_->end();
+ CV_Assert(exists);
+
+ vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
+ CV_Assert(actor);
+
+ vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
+ Matx44f matrix_cv = convertToMatx(matrix);
+ return Affine3f(matrix_cv);
+}
--- /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;
+ };
+
+ // TODO This will be used to contain both cloud and shape actors
+ struct CV_EXPORTS WidgetActor
+ {
+ vtkSmartPointer<vtkProp> actor;
+ vtkSmartPointer<vtkMatrix4x4> viewpoint_transformation_;
+ vtkSmartPointer<vtkIdTypeArray> cells;
+ };
+
+ typedef std::map<std::string, CloudActor> CloudActorMap;
+ typedef std::map<std::string, vtkSmartPointer<vtkProp> > ShapeActorMap;
+ typedef std::map<std::string, WidgetActor> WidgetActorMap;
+}
+
#include <string>
#include <opencv2/viz.hpp>
-#include <opencv2/viz/mesh_load.hpp>
cv::Mat cvcloud_load()
{
float pos_x = 0.0f;
float pos_y = 0.0f;
float pos_z = 0.0f;
-// temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply");
-// v.addPolygonMesh(*mesh, "pq");
+ temp_viz::Mesh3d::Ptr mesh = temp_viz::Mesh3d::mesh_load("d:/horse.ply");
+ v.addPolygonMesh(*mesh, "pq");
int col_blue = 0;
int col_green = 0;
int col_red = 0;
temp_viz::LineWidget lw(cv::Point3f(0.0,0.0,0.0), cv::Point3f(4.0,4.0,4.0), temp_viz::Color(0,255,0));
- temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0), 5.0);
+ temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0), 50.0);
temp_viz::SphereWidget sw(cv::Point3f(0,0,0), 0.5);
temp_viz::ArrowWidget aw(cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0));
temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 0.5, 0.01, temp_viz::Color(0,255,0));
temp_viz::CylinderWidget cyw(cv::Point3f(0,0,0), cv::Point3f(-1,-1,-1), 0.5, 30, temp_viz::Color(0,255,0));
temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1));
- temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity());
+ temp_viz::CoordinateSystemWidget csw;
temp_viz::TextWidget tw("TEST", cv::Point2i(100,100), 20);
temp_viz::CloudWidget pcw(cloud, colors);
temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(0,255,255));
// v.showWidget("line", lw);
// v.showWidget("plane", pw);
-// v.showWidget("sphere", sw);
-// v.showWidget("arrow", aw);
-// v.showWidget("circle", cw);
-// v.showWidget("cylinder", cyw);
-// v.showWidget("cube", cuw);
- v.showWidget("coordinateSystem", csw);
-// v.showWidget("text",tw);
-// v.showWidget("pcw",pcw);
-// v.showWidget("pcw2",pcw2);
+ v.showWidget("sphere", sw);
+ v.spin();
+ //v.showWidget("arrow", aw);
+ //v.showWidget("circle", cw);
+ //v.showWidget("cylinder", cyw);
+ //v.showWidget("cube", cuw);
+ //v.showWidget("coordinateSystem", csw);
+ //v.showWidget("coordinateSystem2", temp_viz::CoordinateSystemWidget(2.0), cv::Affine3f(0, 0, 0, cv::Vec3f(2, 0, 0)));
+ //v.showWidget("text",tw);
+ //v.showWidget("pcw",pcw);
+ //v.showWidget("pcw2",pcw2);
// temp_viz::LineWidget lw2 = lw;
// v.showPointCloud("cld",cloud, colors);
-
+
cv::Mat normals(cloud.size(), cloud.type(), cv::Scalar(0, 10, 0));
// v.addPointCloudNormals(cloud, normals, 100, 0.02, "n");
- temp_viz::CloudNormalsWidget cnw(cloud, normals);
-// v.showWidget("n", cnw);
+ //temp_viz::CloudNormalsWidget cnw(cloud, normals);
+ //v.showWidget("n", cnw);
+
+
+
// lw = v.getWidget("n").cast<temp_viz::LineWidget>();
// pw = v.getWidget("n").cast<temp_viz::PlaneWidget>();
data[3] = cv::Vec4d(3.0,4.0,1.0,1.0);
points = points.reshape(0, 2);
- temp_viz::PolyLineWidget plw(points);
-// v.showWidget("polyline",plw);
+ temp_viz::PolyLineWidget plw(points, temp_viz::Color::green());
+ v.showWidget("polyline",plw);
// lw = v.getWidget("polyline").cast<temp_viz::LineWidget>();
- temp_viz::GridWidget gw(temp_viz::Vec2i(10,10), temp_viz::Vec2d(0.1,0.1));
- v.showWidget("grid", gw);
+ v.spin();
+
+ //temp_viz::GridWidget gw(temp_viz::Vec2i(100,100), temp_viz::Vec2d(1,1));
+ //v.showWidget("grid", gw);
lw = v.getWidget("grid").cast<temp_viz::LineWidget>();
- temp_viz::Text3DWidget t3w("OpenCV", cv::Point3f(0.0, 2.0, 0.0), 1.0, temp_viz::Color(255,255,0));
- v.showWidget("txt3d", t3w);
+ //temp_viz::Text3DWidget t3w("OpenCV", cv::Point3f(0.0, 2.0, 0.0), 1.0, temp_viz::Color(255,255,0));
+ //v.showWidget("txt3d", t3w);
// float grid_x_angle = 0.0;
while(!v.wasStopped())
lw.setColor(temp_viz::Color(col_blue, col_green, col_red));
// lw.setLineWidth(pos_x * 10);
- plw.setColor(temp_viz::Color(col_blue, col_green, col_red));
+ //plw.setColor(temp_viz::Color(col_blue, col_green, col_red));
sw.setPose(cloudPosition);
// pw.setPose(cloudPosition);
// v.setWidgetPose("n",cloudPosition);
// v.setWidgetPose("pcw2", cloudPosition);
- cnw.setColor(temp_viz::Color(col_blue, col_green, col_red));
- pcw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
+ //cnw.setColor(temp_viz::Color(col_blue, col_green, col_red));
+ //pcw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
- gw.updatePose(temp_viz::Affine3f(0.0, 0.1, 0.0, cv::Vec3f(0.0,0.0,0.0)));
+ //gw.updatePose(temp_viz::Affine3f(0.0, 0.1, 0.0, cv::Vec3f(0.0,0.0,0.0)));
angle_x += 0.1f;
angle_y -= 0.1f;
projects / platform / upstream / opencv.git / commitdiff