generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

getShapePose/setShapePose simplification

Browse Source 
little code beautification, formatting
This commit is contained in:
Anatoly Baksheev
2013-07-01 23:29:33 +04:00
parent 2229d91c50
commit e0b7e63787
9 changed files with 170 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
modules/viz/include/opencv2/viz/viz3d.hpp
View File

@@ -32,17 +32,17 @@ namespace temp_viz
bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud"); bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud");
void showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255)); void showLine(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
void showPlane(const String &id, const Vec4f &coefs, const Color &color = Color(255,255,255)); void showPlane(const String& id, const Vec4f& coefs, const Color& color = Color::white());
void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color = Color(255,255,255)); void showPlane(const String& id, const Vec4f& coefs, const Point3f& pt, const Color& color = Color::white());
void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255)); void showCube(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
void showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color = Color(255,255,255)); void showCylinder(const String& id, const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int num_sides, const Color& color = Color::white());
void showCircle(const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255)); void showCircle(const String& id, const Point3f& pt, double radius, const Color& color = Color::white());
void showSphere (const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255)); void showSphere(const String& id, const Point3f& pt, double radius, const Color& color = Color::white());
void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255)); void showArrow(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white());
Affine3f getShapePose(const String& id); Affine3f getShapePose(const String& id);
bool setShapePose(const String &id, const Affine3f &pose); void setShapePose(const String& id, const Affine3f &pose);
bool addPlane (const ModelCoefficients &coefficients, const String& id = "plane"); bool addPlane (const ModelCoefficients &coefficients, const String& id = "plane");
bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id = "plane");

22
modules/viz/src/interactor_style.cpp
View File

@@ -154,13 +154,13 @@ temp_viz::InteractorStyle::OnKeyDown ()
{ {
if (!init_) if (!init_)
{ {
std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing" << std::endl; std::cout << "Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
return; return;
} }
if (!renderer_) if (!renderer_)
{ {
std::cout << "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl; std::cout << "No renderer given! Use SetRendererCollection () before continuing." << std::endl;
return; return;
} }
@@ -186,21 +186,9 @@ temp_viz::InteractorStyle::OnKeyDown ()
bool keymod = false; bool keymod = false;
switch (modifier_) switch (modifier_)
{ {
case KB_MOD_ALT: case KB_MOD_ALT: keymod = alt; break;
{ case KB_MOD_CTRL: keymod = ctrl; break;
keymod = alt; case KB_MOD_SHIFT: keymod = shift; break;
break;
}
case KB_MOD_CTRL:
{
keymod = ctrl;
break;
}
case KB_MOD_SHIFT:
{
keymod = shift;
break;
}
} }
std::string key (Interactor->GetKeySym ()); std::string key (Interactor->GetKeySym ());

20
modules/viz/src/precomp.hpp
View File

@@ -6,12 +6,6 @@
#include <list> #include <list>
#include <vector> #include <vector>
/*
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/signals2.hpp>
#include <boost/thread.hpp>*/
#include <Eigen/Geometry> #include <Eigen/Geometry>
#if defined __GNUC__ #if defined __GNUC__
@@ -140,24 +134,13 @@
#include <vtkImageData.h> #include <vtkImageData.h>
#include <vtkPolyDataNormals.h> #include <vtkPolyDataNormals.h>
#include <vtkMapper.h> #include <vtkMapper.h>
#include <vtkSelectionNode.h> #include <vtkSelectionNode.h>
#include <vtkAbstractPicker.h> #include <vtkAbstractPicker.h>
#include <vtkAbstractPropPicker.h> #include <vtkAbstractPropPicker.h>
#include <vtkMatrix4x4.h> #include <vtkMatrix4x4.h>
#include <vtkInteractorObserver.h> #include <vtkInteractorObserver.h>
#include <vtkMapper2D.h> #include <vtkMapper2D.h>
#include <vtkLeaderActor2D.h> #include <vtkLeaderActor2D.h>
#include <vtkAlgorithmOutput.h> #include <vtkAlgorithmOutput.h>
@@ -168,3 +151,6 @@
#define __DEPRECATED #define __DEPRECATED
#undef __DEPRECATED_DISABLED__ #undef __DEPRECATED_DISABLED__
#endif #endif
#include <q/shapes.h>

27
modules/viz/src/q/shapes.h
View File

@@ -1,22 +1,19 @@
#pragma once #pragma once
#include <Eigen/Core>
#include <opencv2/viz/types.hpp> #include <opencv2/viz/types.hpp>
#include "precomp.hpp"
namespace temp_viz namespace temp_viz
{ {
CV_EXPORTS vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2); vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10); vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30); vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs); vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt); vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius); vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max); vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius); vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
CV_EXPORTS vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2); vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2);
/** \brief Allocate a new unstructured grid smartpointer. For internal use only.
* \param[out] polydata the resultant unstructured grid. //brief Allocate a new unstructured grid smartpointer. For internal use only.
*/ void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
CV_EXPORTS void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
} }

75
modules/viz/src/q/viz3d_impl.hpp
View File

@@ -17,11 +17,11 @@ class CV_EXPORTS Viz3d::VizImpl
public: public:
typedef cv::Ptr<VizImpl> Ptr; typedef cv::Ptr<VizImpl> Ptr;
VizImpl (const std::string &name = std::string()); VizImpl (const String &name = String());
virtual ~VizImpl (); virtual ~VizImpl ();
void setFullScreen (bool mode); void setFullScreen (bool mode);
void setWindowName (const std::string &name); void setWindowName (const String &name);
/** \brief Register a callback function for keyboard input /** \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] callback function that will be registered as a callback for a keyboard event
@@ -68,34 +68,34 @@ public:
* | * |
* y * y
*/ */
void addCoordinateSystem (double scale, const cv::Affine3f& t, const std::string &id = "coordinate"); void addCoordinateSystem (double scale, const Affine3f& t, const String& id = "coordinate");
/** \brief Removes a previously added 3D axes (coordinate system) /** \brief Removes a previously added 3D axes (coordinate system)
*/ */
bool removeCoordinateSystem (const std::string &id = "coordinate"); bool removeCoordinateSystem (const String& id = "coordinate");
bool removePointCloud (const std::string &id = "cloud"); bool removePointCloud (const String& id = "cloud");
inline bool removePolygonMesh (const std::string &id = "polygon") inline bool removePolygonMesh (const String& id = "polygon")
{ {
// Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4 // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
return removePointCloud (id); return removePointCloud (id);
} }
bool removeShape (const std::string &id = "cloud"); bool removeShape (const String& id = "cloud");
bool removeText3D (const std::string &id = "cloud"); bool removeText3D (const String& id = "cloud");
bool removeAllPointClouds (); bool removeAllPointClouds ();
bool removeAllShapes (); bool removeAllShapes ();
void setBackgroundColor (const Color& color); void setBackgroundColor (const Color& color);
bool addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = ""); bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = "");
bool updateText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize = 10, const std::string &id = ""); bool updateText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = "");
/** \brief Set the pose of an existing shape. Returns false if the shape doesn't exist, true if the pose was succesfully updated. */ /** \brief Set the pose of an existing shape. Returns false if the shape doesn't exist, true if the pose was succesfully updated. */
bool updateShapePose (const std::string &id, const cv::Affine3f& pose); bool updateShapePose (const String& id, const Affine3f& pose);
bool addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = ""); bool addText3D (const String &text, const Point3f &position, const Color& color, double textScale = 1.0, const String& id = "");
bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud"); bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud");
/** \brief If the id exists, updates the point cloud; otherwise, adds a new point cloud to the scene /** \brief If the id exists, updates the point cloud; otherwise, adds a new point cloud to the scene
* \param[in] id a variable to identify the point cloud * \param[in] id a variable to identify the point cloud
@@ -106,23 +106,23 @@ public:
void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity()); void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity());
void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity()); void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity());
bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const String& id = "polygon");
bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id = "polyline"); bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline");
void setPointCloudColor (const Color& color, const std::string &id = "cloud"); void setPointCloudColor (const Color& color, const String& id = "cloud");
bool setPointCloudRenderingProperties (int property, double value, const std::string &id = "cloud"); bool setPointCloudRenderingProperties (int property, double value, const String& id = "cloud");
bool getPointCloudRenderingProperties (int property, double &value, const std::string &id = "cloud"); bool getPointCloudRenderingProperties (int property, double &value, const String& id = "cloud");
bool setShapeRenderingProperties (int property, double value, const std::string &id); bool setShapeRenderingProperties (int property, double value, const String& id);
void setShapeColor (const Color& color, const std::string &id); void setShapeColor (const Color& color, const String& id);
/** \brief Set whether the point cloud is selected or not /** \brief Set whether the point cloud is selected or not
* \param[in] selected whether the cloud is selected or not (true = selected) * \param[in] selected whether the cloud is selected or not (true = selected)
* \param[in] id the point cloud object id (default: cloud) * \param[in] id the point cloud object id (default: cloud)
*/ */
bool setPointCloudSelected (const bool selected, const std::string &id = "cloud" ); bool setPointCloudSelected (const bool selected, const String& id = "cloud" );
/** \brief Returns true when the user tried to close the window */ /** \brief Returns true when the user tried to close the window */
bool wasStopped () const { if (interactor_ != NULL) return (stopped_); else return true; } bool wasStopped () const { if (interactor_ != NULL) return (stopped_); else return true; }
@@ -138,9 +138,9 @@ public:
interactor_->TerminateApp (); interactor_->TerminateApp ();
} }
void showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color); void showLine (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
void showPlane (const String& id, const cv::Vec4f &coefs, const Color& color); void showPlane (const String& id, const cv::Vec4f &coefs, const Color& color);
void showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color); void showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, const Color& color);
void showCube (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color); void showCube (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
void showCylinder (const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color); void showCylinder (const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color);
void showCircle (const String& id, const Point3f& pt, double radius, const Color& color); void showCircle (const String& id, const Point3f& pt, double radius, const Color& color);
@@ -148,17 +148,17 @@ public:
void showArrow (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color); void showArrow (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color);
Affine3f getShapePose (const String& id); Affine3f getShapePose (const String& id);
bool setShapePose (const String &id, const Affine3f &pose); void setShapePose (const String& id, const Affine3f& pose);
bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addPolygon(const cv::Mat& cloud, const Color& color, const String& id = "polygon");
bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color, bool display_length, const String& id = "arrow");
bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color_line, const Color& color_text, const std::string &id = "arrow"); bool 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 // Add a vtkPolydata as a mesh
bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id = "PolyData"); bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const String& id = "PolyData");
bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PolyData"); bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const String& id = "PolyData");
bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel"); bool addModelFromPLYFile (const String &filename, const String& id = "PLYModel");
bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::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. */ /** \brief Changes the visual representation for all actors to surface representation. */
void setRepresentationToSurfaceForAllActors (); void setRepresentationToSurfaceForAllActors ();
@@ -187,7 +187,7 @@ public:
/** \brief Reset the camera direction from {0, 0, 0} to the center_{x, y, z} of a given dataset. /** \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) * \param[in] id the point cloud object id (default: cloud)
*/ */
void resetCameraViewpoint (const std::string &id = "cloud"); void resetCameraViewpoint (const String& id = "cloud");
/** \brief Set the camera pose given by position, viewpoint and up vector /** \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_x the x coordinate of the camera location
@@ -217,7 +217,7 @@ public:
* \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters * \param[in] 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 * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters
*/ */
void setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics); void setCameraParameters (const cv::Matx33f& intrinsics, const Affine3f& extrinsics);
/** \brief Set the camera parameters by given a full camera data structure. /** \brief Set the camera parameters by given a full camera data structure.
* \param[in] camera camera structure containing all the camera parameters. * \param[in] camera camera structure containing all the camera parameters.
@@ -237,8 +237,8 @@ public:
void getCameras (Camera& camera); void getCameras (Camera& camera);
/** \brief Get the current viewing pose. */ /** \brief Get the current viewing pose. */
cv::Affine3f getViewerPose (); Affine3f getViewerPose ();
void saveScreenshot (const std::string &file); void saveScreenshot (const String &file);
/** \brief Return a pointer to the underlying VTK Render Window used. */ /** \brief Return a pointer to the underlying VTK Render Window used. */
//vtkSmartPointer<vtkRenderWindow> getRenderWindow () { return (window_); } //vtkSmartPointer<vtkRenderWindow> getRenderWindow () { return (window_); }
@@ -351,6 +351,9 @@ private:
void convertToVtkMatrix (const cv::Matx44f& 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); 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 /** \brief Convert origin and orientation to vtkMatrix4x4
* \param[in] origin the point cloud origin * \param[in] origin the point cloud origin
* \param[in] orientation the point cloud orientation * \param[in] orientation the point cloud orientation

24
modules/viz/src/shapes.cpp
View File

@@ -1,8 +1,6 @@
#include <q/shapes.h> #include "precomp.hpp"
inline float rad2deg (float alpha)
{ return (alpha * 57.29578f); }
inline float rad2deg(float alpha) { return (alpha * 57.29578f); }
inline double rad2deg(double alpha) { return (alpha * 57.29578); } inline double rad2deg(double alpha) { return (alpha * 57.29578); }
vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides) vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides)
@@ -23,9 +21,9 @@ vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const cv::Vec4f& coefs)
{ {
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New (); vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
plane->SetNormal (coefs[0], coefs[1], coefs[2]); plane->SetNormal (coefs[0], coefs[1], coefs[2]);
double norm = cv::norm (cv::Vec3f (coefs[0], coefs[1], coefs[2])); double norm = cv::norm(cv::Vec3f(coefs.val));
plane->Push (-coefs[3] / norm); plane->Push (-coefs[3] / norm);
return (plane->GetOutput ()); return plane->GetOutput ();
} }
vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt) vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt)
@@ -36,8 +34,7 @@ vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const
plane->SetNormal(coefs[0], coefs[1], coefs[2]); plane->SetNormal(coefs[0], coefs[1], coefs[2]);
double t = coefs3.dot(pt) + coefs[3]; double t = coefs3.dot(pt) + coefs[3];
cv::Vec3f p_center; cv::Vec3f p_center = pt - coefs3 * t * norm_sqr;
p_center = pt - coefs3 * t * norm_sqr;
plane->SetCenter (p_center[0], p_center[1], p_center[2]); plane->SetCenter (p_center[0], p_center[1], p_center[2]);
return (plane->GetOutput ()); return (plane->GetOutput ());
@@ -61,14 +58,14 @@ vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const cv::Point3f& pt, dou
tf->SetTransform (t); tf->SetTransform (t);
tf->SetInputConnection (disk->GetOutputPort ()); tf->SetInputConnection (disk->GetOutputPort ());
return (tf->GetOutput ()); return tf->GetOutput ();
} }
vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max) vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max)
{ {
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New (); vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z);
return (cube->GetOutput ()); return cube->GetOutput ();
} }
vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius) vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
@@ -81,7 +78,7 @@ vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double ra
sphere->LatLongTessellationOff (); sphere->LatLongTessellationOff ();
sphere->Update (); sphere->Update ();
return (sphere->GetOutput ()); return sphere->GetOutput ();
} }
vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2) vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
@@ -136,10 +133,9 @@ vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Poi
transformPD->SetTransform(transform); transformPD->SetTransform(transform);
transformPD->SetInputConnection(arrowSource->GetOutputPort()); transformPD->SetInputConnection(arrowSource->GetOutputPort());
return (transformPD->GetOutput()); return transformPD->GetOutput();
} }
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2) vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2)
{ {
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New (); vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
@@ -148,7 +144,7 @@ vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const
line->Update (); line->Update ();
return line->GetOutput (); return line->GetOutput ();
} }
//////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata) void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
{ {
polydata = vtkSmartPointer<vtkUnstructuredGrid>::New (); polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();

7
modules/viz/src/viz3d.cpp
View File

@@ -9,10 +9,9 @@ temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_nam
temp_viz::Viz3d::~Viz3d() temp_viz::Viz3d::~Viz3d()
{ {
delete impl_;
} }
void temp_viz::Viz3d::setBackgroundColor(const Color& color) void temp_viz::Viz3d::setBackgroundColor(const Color& color)
{ {
impl_->setBackgroundColor(color); impl_->setBackgroundColor(color);
@@ -118,9 +117,9 @@ cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id)
return impl_->getShapePose(id); return impl_->getShapePose(id);
} }
bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose) void temp_viz::Viz3d::setShapePose(const String& id, const Affine3f &pose)
{ {
return impl_->setShapePose(id, pose); impl_->setShapePose(id, pose);
} }
bool temp_viz::Viz3d::removeCoordinateSystem (const String& id) bool temp_viz::Viz3d::removeCoordinateSystem (const String& id)

61
modules/viz/src/viz3d_impl.cpp
View File

@@ -53,7 +53,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
// If the cloud already exists, update otherwise create new one // If the cloud already exists, update otherwise create new one
CloudActorMap::iterator am_it = cloud_actor_map_->find (id); CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
bool exist = (am_it == cloud_actor_map_->end()); bool exist = am_it == cloud_actor_map_->end();
if (exist) if (exist)
{ {
// Add as new cloud // Add as new cloud
@@ -181,7 +181,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
// If the cloud already exists, update otherwise create new one // If the cloud already exists, update otherwise create new one
CloudActorMap::iterator am_it = cloud_actor_map_->find (id); CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
bool exist = (am_it == cloud_actor_map_->end()); bool exist = am_it == cloud_actor_map_->end();
if (exist) if (exist)
{ {
// Add as new cloud // Add as new cloud
@@ -292,7 +292,7 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3); CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3);
if (cloud_actor_map_->find (id) != cloud_actor_map_->end ()) if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
return (false); return false;
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New(); vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New(); vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
@@ -380,11 +380,11 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
} }
void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color) void temp_viz::Viz3d::VizImpl::showLine (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color)
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -420,7 +420,7 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -451,11 +451,11 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe
} }
} }
void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color) void temp_viz::Viz3d::VizImpl::showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, const Color& color)
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -490,7 +490,7 @@ void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, c
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -525,7 +525,7 @@ void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -560,7 +560,7 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt,
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -595,7 +595,7 @@ void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt,
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
if (exists) if (exists)
@@ -630,7 +630,7 @@ void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1,
{ {
// Check if this Id already exists // Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); bool exists = am_it != shape_actor_map_->end();
Color c = vtkcolor(color); Color c = vtkcolor(color);
// If it exists just update // If it exists just update
@@ -664,47 +664,22 @@ void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1,
cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String& id) cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String& id)
{ {
// Get the shape with the id and return the pose
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
if (!exists)
{
std::cout << "[getShapePose] A shape with id <" << id << "> does not exist!" << std::endl;
return Affine3f();
}
CV_Assert(am_it != shape_actor_map_->end());
vtkLODActor* actor = vtkLODActor::SafeDownCast(am_it->second); vtkLODActor* actor = vtkLODActor::SafeDownCast(am_it->second);
return Affine3f(convertToMatx(actor->GetUserMatrix()));
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
Matx44f pose_mat;
convertToCvMatrix(matrix, pose_mat);
Affine3f pose;
pose.matrix = pose_mat;
return pose;
} }
bool temp_viz::Viz3d::VizImpl::setShapePose (const String &id, const Affine3f &pose) void temp_viz::Viz3d::VizImpl::setShapePose (const String& id, const Affine3f &pose)
{ {
ShapeActorMap::iterator am_it = shape_actor_map_->find (id); ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end()); CV_Assert(am_it != shape_actor_map_->end());
if (!exists)
{
return false;
}
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second); vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New (); vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
convertToVtkMatrix (pose.matrix, matrix);
actor->SetUserMatrix (matrix); actor->SetUserMatrix (matrix);
actor->Modified (); actor->Modified ();
return (true);
} }
bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)

21
modules/viz/src/viz_main.cpp
View File

@@ -97,14 +97,12 @@ void temp_viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_
///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::Viz3d::VizImpl::registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie) void temp_viz::Viz3d::VizImpl::registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie)
{ {
// Register the callback function in the interactor style
style_->registerMouseCallback(callback, cookie); style_->registerMouseCallback(callback, cookie);
} }
///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::Viz3d::VizImpl::registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie) void temp_viz::Viz3d::VizImpl::registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie)
{ {
// Register the callback function in the interactor style
style_->registerKeyboardCallback(callback, cookie); style_->registerKeyboardCallback(callback, cookie);
} }
@@ -1205,6 +1203,15 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatr
vtk_matrix->SetElement (i, k, m (i, k)); vtk_matrix->SetElement (i, k, m (i, k));
} }
vtkSmartPointer<vtkMatrix4x4> temp_viz::convertToVtkMatrix (const cv::Matx44f &m)
{
vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::New();
for (int i = 0; i < 4; i++)
for (int k = 0; k < 4; k++)
vtk_matrix->SetElement(i, k, m(i, k));
return vtk_matrix;
}
void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m) void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m)
{ {
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
@@ -1212,6 +1219,16 @@ void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matri
m(i,k) = vtk_matrix->GetElement (i, k); m(i,k) = vtk_matrix->GetElement (i, k);
} }
cv::Matx44f temp_viz::convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matrix)
{
cv::Matx44f m;
for (int i = 0; i < 4; i++)
for (int k = 0; k < 4; k++)
m(i, k) = vtk_matrix->GetElement (i, k);
return m;
}
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m) void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
{ {