Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge remote-tracking branch 'remotes/ozan/refactoring_shapeMethods' into viz

Browse Source 
Conflicts:
	modules/viz/test/test_viz3d.cpp
pull/1453/head
Anatoly Baksheev 12 years ago
parent 909f905b57 1830059969
commit 2229d91c50
9 changed files with 538 additions and 650 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      modules/viz/include/opencv2/viz/viz3d.hpp
  2. 1
      modules/viz/src/precomp.hpp
  3. 116
      modules/viz/src/q/shapes.h
  4. 114
      modules/viz/src/q/viz3d_impl.hpp
  5. 261
      modules/viz/src/shapes.cpp
  6. 53
      modules/viz/src/viz3d.cpp
  7. 398
      modules/viz/src/viz3d_impl.cpp
  8. 193
      modules/viz/src/viz_main.cpp
  9. 40
      modules/viz/test/test_viz3d.cpp

12
modules/viz/include/opencv2/viz/viz3d.hpp
Unescape View File

@ -31,6 +31,18 @@ namespace temp_viz
void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity());
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 showPlane(const String &id, const Vec4f &coefs, const Color &color = Color(255,255,255));
void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color = Color(255,255,255));
void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, 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(255,255,255));
void showCircle(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(255,255,255));
void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
Affine3f getShapePose(const String &id);
bool setShapePose(const String &id, const Affine3f &pose);
bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane");
bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane");

1
modules/viz/src/precomp.hpp
Unescape View File

@ -49,6 +49,7 @@
#include <vtkDiskSource.h>
#include <vtkPlaneSource.h>
#include <vtkSphereSource.h>
#include <vtkArrowSource.h>
#include <vtkIdentityTransform.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>

116
modules/viz/src/q/shapes.h
Unescape View File

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

114
modules/viz/src/q/viz3d_impl.hpp
Unescape View File

@ -137,13 +137,22 @@ public:
// This tends to close the window...
interactor_->TerminateApp ();
}
void showLine (const String &id, const cv::Point3f &pt1, const cv::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 cv::Point3f &pt, 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 showCircle (const String &id, const Point3f &pt, double radius, const Color &color);
void showSphere (const String &id, const Point3f &pt, double radius, const Color &color);
void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color);
Affine3f getShapePose (const String &id);
bool setShapePose (const String &id, const Affine3f &pose);
bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon");
bool addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id = "line");
bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow");
bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color_line, const Color& color_text, const std::string &id = "arrow");
bool addSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id = "sphere");
bool updateSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id = "sphere");
// Add a vtkPolydata as a mesh
bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id = "PolyData");
@ -151,104 +160,6 @@ public:
bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel");
bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id = "PLYModel");
/** \brief Add a cylinder from a set of given model coefficients
* \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
* \param[in] id the cylinder id/name (default: "cylinder")
*
* \code
* // The following are given (or computed using sample consensus techniques)
* // See SampleConsensusModelCylinder for more information.
* // float radius;
*
* temp_viz::ModelCoefficients cylinder_coeff;
* cylinder_coeff.values.resize (7); // We need 7 values
* cylinder_coeff.values[0] = pt_on_axis.x ();
* cylinder_coeff.values[1] = pt_on_axis.y ();
* cylinder_coeff.values[2] = pt_on_axis.z ();
*
* cylinder_coeff.values[3] = axis_direction.x ();
* cylinder_coeff.values[4] = axis_direction.y ();
* cylinder_coeff.values[5] = axis_direction.z ();
*
* cylinder_coeff.values[6] = radius;
*
* addCylinder (cylinder_coeff);
* \endcode
*/
bool addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cylinder");
/** \brief Add a plane from a set of given model coefficients
* \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
* \param[in] id the plane id/name (default: "plane")
*
* \code
* // The following are given (or computed using sample consensus techniques)
* // See SampleConsensusModelPlane for more information
*
* temp_viz::ModelCoefficients plane_coeff;
* plane_coeff.values.resize (4); // We need 4 values
* plane_coeff.values[0] = plane_parameters.x ();
* plane_coeff.values[1] = plane_parameters.y ();
* plane_coeff.values[2] = plane_parameters.z ();
* plane_coeff.values[3] = plane_parameters.w ();
*
* addPlane (plane_coeff);
* \endcode
*/
bool addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "plane");
bool addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id = "plane");
/** \brief Add a circle from a set of given model coefficients
* \param[in] coefficients the model coefficients (x, y, radius)
* \param[in] id the circle id/name (default: "circle")
*
* \code
* // The following are given (or computed using sample consensus techniques)
* // See SampleConsensusModelCircle2D for more information
* // float x, y, radius;
*
* temp_viz::ModelCoefficients circle_coeff;
* circle_coeff.values.resize (3); // We need 3 values
* circle_coeff.values[0] = x;
* circle_coeff.values[1] = y;
* circle_coeff.values[2] = radius;
*
* vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
* \endcode
*/
bool addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "circle");
/** \brief Add a cube from a set of given model coefficients
* \param[in] coefficients the model coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
* \param[in] id the cube id/name (default: "cube")
*/
bool addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id = "cube");
/** \brief Add a cube from a set of given model coefficients
* \param[in] translation a translation to apply to the cube from 0,0,0
* \param[in] rotation a quaternion-based rotation to apply to the cube
* \param[in] width the cube's width
* \param[in] height the cube's height
* \param[in] depth the cube's depth
* \param[in] id the cube id/name (default: "cube")
*/
bool addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id = "cube");
/** \brief Add a cube
* \param[in] x_min the min X coordinate
* \param[in] x_max the max X coordinate
* \param[in] y_min the min Y coordinate
* \param[in] y_max the max Y coordinate
* \param[in] z_min the min Z coordinate
* \param[in] z_max the max Z coordinate
* \param[in] r how much red (0.0 -> 1.0)
* \param[in] g how much green (0.0 -> 1.0)
* \param[in] b how much blue (0.0 -> 1.0)
* \param[in] id the cube id/name (default: "cube")
*/
bool addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max, const Color& color, const std::string &id = "cube");
/** \brief Changes the visual representation for all actors to surface representation. */
void setRepresentationToSurfaceForAllActors ();
@ -438,6 +349,7 @@ private:
//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);
/** \brief Convert origin and orientation to vtkMatrix4x4
* \param[in] origin the point cloud origin

261
modules/viz/src/shapes.cpp
Unescape View File

@ -5,177 +5,138 @@ inline float rad2deg (float alpha)
inline double rad2deg (double alpha){return (alpha * 57.29578);}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides)
vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides)
{
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
line->SetPoint1 (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
line->SetPoint2 (coefficients.values[3]+coefficients.values[0], coefficients.values[4]+coefficients.values[1], coefficients.values[5]+coefficients.values[2]);
vtkSmartPointer<vtkTubeFilter> tuber = vtkSmartPointer<vtkTubeFilter>::New ();
tuber->SetInputConnection (line->GetOutputPort ());
tuber->SetRadius (coefficients.values[6]);
tuber->SetNumberOfSides (numsides);
return (tuber->GetOutput ());
const cv::Point3f pt2 = pt_on_axis + axis_direction;
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
line->SetPoint1 (pt_on_axis.x, pt_on_axis.y, pt_on_axis.z);
line->SetPoint2 (pt2.x, pt2.y, pt2.z);
vtkSmartPointer<vtkTubeFilter> tuber = vtkSmartPointer<vtkTubeFilter>::New ();
tuber->SetInputConnection (line->GetOutputPort ());
tuber->SetRadius (radius);
tuber->SetNumberOfSides (numsides);
return (tuber->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createCube (const temp_viz::ModelCoefficients &coefficients)
vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const cv::Vec4f& coefs)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
coefficients.values[4], coefficients.values[5]));
t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (coefficients.values[7]);
cube->SetYLength (coefficients.values[8]);
cube->SetZLength (coefficients.values[9]);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
plane->SetNormal (coefs[0], coefs[1], coefs[2]);
double norm = cv::norm (cv::Vec3f (coefs[0], coefs[1], coefs[2]));
plane->Push (-coefs[3] / norm);
return (plane->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth)
vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (translation.x (), translation.y (), translation.z ());
Eigen::AngleAxisf a (rotation);
t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
cv::Point3f coefs3 (coefs[0], coefs[1], coefs[2]);
double norm_sqr = 1.0 / coefs3.dot (coefs3);
plane->SetNormal (coefs[0], coefs[1], coefs[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (width);
cube->SetYLength (height);
cube->SetZLength (depth);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
}
double t = coefs3.dot (pt) + coefs[3];
cv::Vec3f p_center;
p_center = pt - coefs3 * t * norm_sqr;
plane->SetCenter (p_center[0], p_center[1], p_center[2]);
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max)
{
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max);
return (cube->GetOutput ());
return (plane->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients)
vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const cv::Point3f& pt, double radius)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double norm_sqr = coefficients.values[0] * coefficients.values[0]
+ coefficients.values[1] * coefficients.values[1]
+ coefficients.values[2] * coefficients.values[2];
plane->Push (-coefficients.values[3] / sqrt(norm_sqr));
return (plane->GetOutput ());
vtkSmartPointer<vtkDiskSource> disk = vtkSmartPointer<vtkDiskSource>::New ();
// Maybe the resolution should be lower e.g. 50 or 25
disk->SetCircumferentialResolution (100);
disk->SetInnerRadius (radius - 0.001);
disk->SetOuterRadius (radius + 0.001);
disk->SetCircumferentialResolution (20);
// Set the circle origin
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (pt.x, pt.y, pt.z);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (disk->GetOutputPort ());
return (tf->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z)
vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
coefficients.values[1] * coefficients.values[1] +
coefficients.values[2] * coefficients.values[2] );
// double nx = coefficients.values [0] * norm;
// double ny = coefficients.values [1] * norm;
// double nz = coefficients.values [2] * norm;
// double d = coefficients.values [3] * norm;
// plane->SetNormal (nx, ny, nz);
plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
x -= coefficients.values[0] * t * norm_sqr;
y -= coefficients.values[1] * t * norm_sqr;
z -= coefficients.values[2] * t * norm_sqr;
plane->SetCenter (x, y, z);
return (plane->GetOutput ());
vtkSmartPointer<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);
return (cube->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z)
vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
{
vtkSmartPointer<vtkDiskSource> disk = vtkSmartPointer<vtkDiskSource>::New ();
// Maybe the resolution should be lower e.g. 50 or 25
disk->SetCircumferentialResolution (100);
disk->SetInnerRadius (coefficients.values[2] - 0.001);
disk->SetOuterRadius (coefficients.values[2] + 0.001);
disk->SetCircumferentialResolution (20);
// An alternative to <vtkDiskSource> could be <vtkRegularPolygonSource> with <vtkTubeFilter>
/*
vtkSmartPointer<vtkRegularPolygonSource> circle = vtkSmartPointer<vtkRegularPolygonSource>::New();
circle->SetRadius (coefficients.values[2]);
circle->SetNumberOfSides (100);
vtkSmartPointer<vtkTubeFilter> tube = vtkSmartPointer<vtkTubeFilter>::New();
tube->SetInput (circle->GetOutput());
tube->SetNumberOfSides (25);
tube->SetRadius (0.001);
*/
// Set the circle origin
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (coefficients.values[0], coefficients.values[1], z);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (disk->GetOutputPort ());
/*
tf->SetInputConnection (tube->GetOutputPort ());
*/
return (tf->GetOutput ());
vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New ();
sphere->SetRadius (radius);
sphere->SetCenter (pt.x, pt.y, pt.z);
sphere->SetPhiResolution (10);
sphere->SetThetaResolution (10);
sphere->LatLongTessellationOff ();
sphere->Update ();
return (sphere->GetOutput ());
}
////////////////////////////////////////////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution)
vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
{
// Set the sphere origin
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (center.x, center.y, center.z);
vtkSmartPointer<vtkSphereSource> s_sphere = vtkSmartPointer<vtkSphereSource>::New ();
s_sphere->SetRadius (radius);
s_sphere->SetPhiResolution (sphere_resolution);
s_sphere->SetThetaResolution (sphere_resolution);
s_sphere->LatLongTessellationOff ();
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (s_sphere->GetOutputPort ());
tf->Update ();
return (tf->GetOutput ());
vtkSmartPointer<vtkArrowSource> arrowSource = vtkSmartPointer<vtkArrowSource>::New ();
float startPoint[3], endPoint[3];
startPoint[0] = pt1.x;
startPoint[1] = pt1.y;
startPoint[2] = pt1.z;
endPoint[0] = pt2.x;
endPoint[1] = pt2.y;
endPoint[2] = pt2.z;
float normalizedX[3], normalizedY[3], normalizedZ[3];
// The X axis is a vector from start to end
vtkMath::Subtract(endPoint, startPoint, normalizedX);
float length = vtkMath::Norm(normalizedX);
vtkMath::Normalize(normalizedX);
// The Z axis is an arbitrary vecotr cross X
float arbitrary[3];
arbitrary[0] = vtkMath::Random(-10,10);
arbitrary[1] = vtkMath::Random(-10,10);
arbitrary[2] = vtkMath::Random(-10,10);
vtkMath::Cross(normalizedX, arbitrary, normalizedZ);
vtkMath::Normalize(normalizedZ);
// The Y axis is Z cross X
vtkMath::Cross(normalizedZ, normalizedX, normalizedY);
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
// Create the direction cosine matrix
matrix->Identity();
for (unsigned int i = 0; i < 3; i++)
{
matrix->SetElement(i, 0, normalizedX[i]);
matrix->SetElement(i, 1, normalizedY[i]);
matrix->SetElement(i, 2, normalizedZ[i]);
}
// Apply the transforms
vtkSmartPointer<vtkTransform> transform =
vtkSmartPointer<vtkTransform>::New();
transform->Translate(startPoint);
transform->Concatenate(matrix);
transform->Scale(length, length, length);
// Transform the polydata
vtkSmartPointer<vtkTransformPolyDataFilter> transformPD =
vtkSmartPointer<vtkTransformPolyDataFilter>::New();
transformPD->SetTransform(transform);
transformPD->SetInputConnection(arrowSource->GetOutputPort());
return (transformPD->GetOutput());
}
////////////////////////////////////////////////////////////////////////////////////////////

53
modules/viz/src/viz3d.cpp
Unescape View File

@ -63,11 +63,6 @@ bool temp_viz::Viz3d::addPolygon(const Mat& cloud, const Color& color, const Str
return impl_->addPolygon(cloud, color, id);
}
bool temp_viz::Viz3d::addSphere (const cv::Point3f &center, double radius, const Color& color, const std::string &id)
{
return impl_->addSphere(center, radius, color, id);
}
void temp_viz::Viz3d::spin()
{
impl_->spin();
@ -78,14 +73,54 @@ void temp_viz::Viz3d::spinOnce (int time, bool force_redraw)
impl_->spinOnce(time, force_redraw);
}
bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, const String &id)
void temp_viz::Viz3d::showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
{
impl_->showLine(id, pt1, pt2, color);
}
void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Color &color)
{
impl_->showPlane(id, coefs, color);
}
void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color)
{
impl_->showPlane(id, coefs, pt, color);
}
void temp_viz::Viz3d::showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
{
impl_->showCube(id, pt1, pt2, color);
}
void temp_viz::Viz3d::showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color)
{
impl_->showCylinder(id, pt_on_axis, axis_direction, radius, num_sides, color);
}
void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double radius, const Color &color)
{
impl_->showCircle(id, pt, radius, color);
}
void temp_viz::Viz3d::showSphere (const String &id, const Point3f &pt, double radius, const Color &color)
{
impl_->showSphere(id, pt, radius, color);
}
void temp_viz::Viz3d::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
{
impl_->showArrow(id,pt1,pt2,color);
}
cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id)
{
return impl_->addPlane(coefficients, id);
return impl_->getShapePose(id);
}
bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id)
bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose)
{
return impl_->addPlane(coefficients, x, y, z, id);
return impl_->setShapePose(id, pose);
}
bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)

398
modules/viz/src/viz3d_impl.cpp
Unescape View File

@ -380,32 +380,332 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addLine (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, const std::string &id)
void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addLine] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
bool exists = (am_it != shape_actor_map_->end());
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createLine(pt1,pt2));
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create new line
vtkSmartPointer<vtkDataSet> data = createLine (pt1, pt2);
vtkSmartPointer<vtkDataSet> data = createLine (pt1, pt2);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coefs, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor (actor);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createPlane(coefs));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createPlane (coefs);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createPlane(coefs, pt));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createPlane (coefs, pt);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createCube(pt1, pt2));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createCube (pt1, pt2);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createCylinder(pt_on_axis, axis_direction, radius, num_sides));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createCylinder(pt_on_axis, axis_direction, radius, num_sides);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, double radius, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(create2DCircle(pt, radius));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = create2DCircle(pt, radius);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt, double radius, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createSphere(pt, radius));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createSphere(pt, radius);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
{
// Check if this Id already exists
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
Color c = vtkcolor(color);
// If it exists just update
if (exists)
{
vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createArrow(pt1,pt2));
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
}
else
{
// Create a plane
vtkSmartPointer<vtkDataSet> data = createArrow(pt1,pt2);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
}
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);
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();
}
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
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)
{
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
bool exists = (am_it != shape_actor_map_->end());
if (!exists)
{
return false;
}
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
convertToVtkMatrix (pose.matrix, matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
return (true);
}
bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
{
@ -772,72 +1072,6 @@ bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3
return (true);
}
#include <vtkSphereSource.h>
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addSphere (const cv::Point3f& center, float radius, const Color& color, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addSphere] A shape with id <"<<id << "> already exists! Please choose a different id and retry." << std::endl, false;
//vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New ();
data->SetRadius (radius);
data->SetCenter (center.x, center.y, center.z);
data->SetPhiResolution (10);
data->SetThetaResolution (10);
data->LatLongTessellationOff ();
data->Update ();
// Setup actor and mapper
vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
mapper->SetInputConnection (data->GetOutputPort ());
// Create an Actor
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
actor->SetMapper (mapper);
//createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetInterpolationToFlat ();
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ImmediateModeRenderingOn ();
actor->GetMapper ()->StaticOn ();
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetMapper ()->Update ();
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::updateSphere (const cv::Point3f &center, float radius, const Color& color, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it == shape_actor_map_->end ())
return (false);
//////////////////////////////////////////////////////////////////////////
// Get the actor pointer
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
src->SetCenter(center.x, center.y, center.z);
src->SetRadius(radius);
src->Update ();
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
actor->Modified ();
return (true);
}
//////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addText3D (const std::string &text, const cv::Point3f& position, const Color& color, double textScale, const std::string &id)
{

193
modules/viz/src/viz_main.cpp
Unescape View File

@ -894,111 +894,6 @@ void temp_viz::Viz3d::VizImpl::resetCameraViewpoint (const std::string &id)
renderer_->Render ();
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addCylinder (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addCylinder] A shape with id <"<<id <<"> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
vtkSmartPointer<vtkDataSet> data = createCylinder (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addCube (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
vtkSmartPointer<vtkDataSet> data = createCube (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addCube (const cv::Vec3f& translation, const cv::Vec3f quaternion, double width, double height, double depth, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
Eigen::Vector3f t(translation[0], translation[1], translation[2]);
Eigen::Quaternionf q(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
vtkSmartPointer<vtkDataSet> data = createCube (t, q, width, height, depth);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
const Color& color , const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addCube] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
vtkSmartPointer<vtkDataSet> data = createCube (x_min, x_max, y_min, y_max, z_min, z_max);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
Color c = vtkcolor(color);
actor->GetProperty ()->SetColor (c.val);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id)
{
@ -1088,87 +983,6 @@ bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename,
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Add a plane from a set of given model coefficients
* \param coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
* \param id the plane id/name (default: "plane")
* \param viewport (optional) the id of the new viewport (default: 0)
*/
bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addPlane] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetLighting (false);
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
bool temp_viz::Viz3d::VizImpl::addPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addPlane] A shape with id <" << id << "> already exists! Please choose a different id and retry.\n" << std::endl;
return (false);
}
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients, x, y, z);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
/////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addCircle (const temp_viz::ModelCoefficients &coefficients, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addCircle] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.\n" << std::endl;
return (false);
}
vtkSmartPointer<vtkDataSet> data = create2DCircle (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetLighting (false);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
/////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addText (const std::string &text, int xpos, int ypos, const Color& color, int fontsize, const std::string &id)
{
@ -1391,6 +1205,13 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatr
vtk_matrix->SetElement (i, k, m (i, k));
}
void temp_viz::convertToCvMatrix (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);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
{

40
modules/viz/test/test_viz3d.cpp
Unescape View File

@ -41,9 +41,16 @@
//M*/
#include "test_precomp.hpp"
#include <opencv2/viz.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <fstream>
#include <string>
#include <opencv2/viz/types.hpp>
#include <opencv2/viz/mesh_load.hpp>
cv::Mat cvcloud_load()
{
cv::Mat cloud(1, 20000, CV_32FC3);
@ -79,36 +86,43 @@ TEST(Viz_viz3d, accuracy)
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::mesh_load("d:/horse.ply");
// v.addPolygonMesh(*mesh, "pq");
int col_blue = 0;
int col_green = 0;
int col_red = 0;
v.showCircle("circle1", cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
v.showSphere("sphere1", cv::Point3f(0,0,0), 0.5, temp_viz::Color(0,0,255));
v.showArrow("arrow1", cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0));
bool alternate = true;
while(!v.wasStopped())
{
// Creating new point cloud with id cloud1
cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z));
if (!alternate)
v.showPointCloud("cloud1", cloud, temp_viz::Color(255, 0, 0), cloudPosition);
else
v.showPointCloud("cloud1", cloud, colors, cloudPosition);
alternate = !alternate;
// v.showPointCloud("cloud1", cloud, temp_viz::Color(col_blue, col_green, col_red), cloudPosition);
// v.showLine("line1", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
// v.showLine("line2", cv::Point3f(0.0,0.0,0.0), cv::Point3f(1.0f-pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
// v.showLine("line3", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, 1.0f-pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
// v.showLine("line4", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, 1.0f-pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
// v.showPlane("plane1", cv::Vec4f(pos_x*pos_y,pos_y,pos_z,pos_x+pos_y*pos_z), temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
// v.showCube("cube1", cv::Point3f(pos_x, pos_y, pos_z), cv::Point3f(pos_x+0.5, pos_y+0.5, pos_z+0.5), temp_viz::Color(255,150,50));
// v.showCylinder("cylinder1", cv::Point3f(0,0,0), cv::Point3f(pos_x, 1.0, 1.0), 0.5, 5*pos_x+3, temp_viz::Color(0,255,0));
v.setShapePose("circle1", cloudPosition);
v.setShapePose("sphere1", cloudPosition);
v.setShapePose("arrow1", cloudPosition);
angle_x += 0.1f;
angle_y -= 0.1f;
angle_z += 0.1f;
pos_x = std::sin(angle_x);
pos_y = std::sin(angle_x);
pos_z = std::sin(angle_x);
pos_y = std::sin(angle_y);
pos_z = std::sin(angle_z);
col_blue = int(angle_x * 10) % 256;
col_green = int(angle_x * 20) % 256;
col_red = int(angle_x * 30) % 256;
v.spinOnce(10, true);
v.spinOnce(1, true);
}
// cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));

Write Preview
Loading…
Cancel
Save