Merge remote-tracking branch 'remotes/ozan/refactoring_shapeMethods' into viz
Conflicts: modules/viz/test/test_viz3d.cpp
This commit is contained in:
commit
2229d91c50
@ -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");
|
||||
|
@ -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>
|
||||
|
@ -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 ¢er, float radius, int sphere_resolution = 10);
|
||||
|
||||
/** \brief Create a cylinder shape from a set of model coefficients.
|
||||
* \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
|
||||
* \param[in] numsides (optional) the number of sides used for rendering the cylinder
|
||||
*
|
||||
* \code
|
||||
* // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCylinder)
|
||||
* // Eigen::Vector3f pt_on_axis, axis_direction;
|
||||
* // float radius;
|
||||
*
|
||||
* temp_viz::ModelCoefficients cylinder_coeff;
|
||||
* cylinder_coeff.values.resize (7); // We need 7 values
|
||||
* cylinder_coeff.values[0] = pt_on_axis.x ();
|
||||
* cylinder_coeff.values[1] = pt_on_axis.y ();
|
||||
* cylinder_coeff.values[2] = pt_on_axis.z ();
|
||||
*
|
||||
* cylinder_coeff.values[3] = axis_direction.x ();
|
||||
* cylinder_coeff.values[4] = axis_direction.y ();
|
||||
* cylinder_coeff.values[5] = axis_direction.z ();
|
||||
*
|
||||
* cylinder_coeff.values[6] = radius;
|
||||
*
|
||||
* vtkSmartPointer<vtkDataSet> data = temp_viz::createCylinder (cylinder_coeff, numsides);
|
||||
* \endcode
|
||||
*
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides = 30);
|
||||
|
||||
|
||||
/** \brief Create a planar shape from a set of model coefficients.
|
||||
* \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
|
||||
*
|
||||
* \code
|
||||
* // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelPlane)
|
||||
* // Eigen::Vector4f plane_parameters;
|
||||
*
|
||||
* temp_viz::ModelCoefficients plane_coeff;
|
||||
* plane_coeff.values.resize (4); // We need 4 values
|
||||
* plane_coeff.values[0] = plane_parameters.x ();
|
||||
* plane_coeff.values[1] = plane_parameters.y ();
|
||||
* plane_coeff.values[2] = plane_parameters.z ();
|
||||
* plane_coeff.values[3] = plane_parameters.w ();
|
||||
*
|
||||
* vtkSmartPointer<vtkDataSet> data = temp_viz::createPlane (plane_coeff);
|
||||
* \endcode
|
||||
*
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients);
|
||||
|
||||
/** \brief Create a planar shape from a set of model coefficients.
|
||||
* \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
|
||||
* \param[in] x,y,z projection of this point on the plane is used to get the center of the plane.
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z);
|
||||
|
||||
/** \brief Create a 2d circle shape from a set of model coefficients.
|
||||
* \param[in] coefficients the model coefficients (x, y, radius)
|
||||
* \param[in] z (optional) specify a z value (default: 0)
|
||||
*
|
||||
* \code
|
||||
* // The following are given (or computed using sample consensus techniques -- see SampleConsensusModelCircle2D)
|
||||
* // float x, y, radius;
|
||||
*
|
||||
* temp_viz::ModelCoefficients circle_coeff;
|
||||
* circle_coeff.values.resize (3); // We need 3 values
|
||||
* circle_coeff.values[0] = x;
|
||||
* circle_coeff.values[1] = y;
|
||||
* circle_coeff.values[2] = radius;
|
||||
*
|
||||
* vtkSmartPointer<vtkDataSet> data = temp_viz::create2DCircle (circle_coeff, z);
|
||||
* \endcode
|
||||
*
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z = 0.0);
|
||||
|
||||
|
||||
/** \brief Creaet a cube shape from a set of model coefficients.
|
||||
* \param[in] coefficients the cube coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const temp_viz::ModelCoefficients &coefficients);
|
||||
|
||||
/** \brief Creaet a cube shape from a set of model coefficients.
|
||||
*
|
||||
* \param[in] translation a translation to apply to the cube from 0,0,0
|
||||
* \param[in] rotation a quaternion-based rotation to apply to the cube
|
||||
* \param[in] width the cube's width
|
||||
* \param[in] height the cube's height
|
||||
* \param[in] depth the cube's depth
|
||||
* \ingroup visualization
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth);
|
||||
|
||||
/** \brief Create a cube from a set of bounding points
|
||||
* \param[in] x_min is the minimum x value of the box
|
||||
* \param[in] x_max is the maximum x value of the box
|
||||
* \param[in] y_min is the minimum y value of the box
|
||||
* \param[in] y_max is the maximum y value of the box
|
||||
* \param[in] z_min is the minimum z value of the box
|
||||
* \param[in] z_max is the maximum z value of the box
|
||||
* \param[in] id the cube id/name (default: "cube")
|
||||
* \param[in] viewport (optional) the id of the new viewport (default: 0)
|
||||
*/
|
||||
CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max);
|
||||
|
||||
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.
|
||||
*/
|
||||
|
@ -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 ¢er, float radius, const Color& color, const std::string &id = "sphere");
|
||||
bool updateSphere (const cv::Point3f ¢er, float radius, const Color& color, const std::string &id = "sphere");
|
||||
|
||||
// Add a vtkPolydata as a mesh
|
||||
bool addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id = "PolyData");
|
||||
@ -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
|
||||
|
@ -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 ());
|
||||
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]);
|
||||
|
||||
Eigen::AngleAxisf a (rotation);
|
||||
t->RotateWXYZ (rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
|
||||
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<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 ());
|
||||
return (plane->GetOutput ());
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max)
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const cv::Point3f& pt, double radius)
|
||||
{
|
||||
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
|
||||
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
|
||||
cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max);
|
||||
return (cube->GetOutput ());
|
||||
vtkSmartPointer<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)
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max)
|
||||
{
|
||||
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<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::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z)
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
|
||||
{
|
||||
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<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::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z)
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
|
||||
{
|
||||
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);
|
||||
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);
|
||||
|
||||
// An alternative to <vtkDiskSource> could be <vtkRegularPolygonSource> with <vtkTubeFilter>
|
||||
/*
|
||||
vtkSmartPointer<vtkRegularPolygonSource> circle = vtkSmartPointer<vtkRegularPolygonSource>::New();
|
||||
circle->SetRadius (coefficients.values[2]);
|
||||
circle->SetNumberOfSides (100);
|
||||
// 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);
|
||||
|
||||
vtkSmartPointer<vtkTubeFilter> tube = vtkSmartPointer<vtkTubeFilter>::New();
|
||||
tube->SetInput (circle->GetOutput());
|
||||
tube->SetNumberOfSides (25);
|
||||
tube->SetRadius (0.001);
|
||||
*/
|
||||
// The Y axis is Z cross X
|
||||
vtkMath::Cross(normalizedZ, normalizedX, normalizedY);
|
||||
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
|
||||
|
||||
// Set the circle origin
|
||||
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
|
||||
t->Identity ();
|
||||
t->Translate (coefficients.values[0], coefficients.values[1], z);
|
||||
// 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]);
|
||||
}
|
||||
|
||||
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
|
||||
tf->SetTransform (t);
|
||||
tf->SetInputConnection (disk->GetOutputPort ());
|
||||
/*
|
||||
tf->SetInputConnection (tube->GetOutputPort ());
|
||||
*/
|
||||
// Apply the transforms
|
||||
vtkSmartPointer<vtkTransform> transform =
|
||||
vtkSmartPointer<vtkTransform>::New();
|
||||
transform->Translate(startPoint);
|
||||
transform->Concatenate(matrix);
|
||||
transform->Scale(length, length, length);
|
||||
|
||||
return (tf->GetOutput ());
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution)
|
||||
{
|
||||
// Set the sphere origin
|
||||
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
|
||||
t->Identity ();
|
||||
t->Translate (center.x, center.y, center.z);
|
||||
|
||||
vtkSmartPointer<vtkSphereSource> s_sphere = vtkSmartPointer<vtkSphereSource>::New ();
|
||||
s_sphere->SetRadius (radius);
|
||||
s_sphere->SetPhiResolution (sphere_resolution);
|
||||
s_sphere->SetThetaResolution (sphere_resolution);
|
||||
s_sphere->LatLongTessellationOff ();
|
||||
|
||||
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
|
||||
tf->SetTransform (t);
|
||||
tf->SetInputConnection (s_sphere->GetOutputPort ());
|
||||
tf->Update ();
|
||||
|
||||
return (tf->GetOutput ());
|
||||
// Transform the polydata
|
||||
vtkSmartPointer<vtkTransformPolyDataFilter> transformPD =
|
||||
vtkSmartPointer<vtkTransformPolyDataFilter>::New();
|
||||
transformPD->SetTransform(transform);
|
||||
transformPD->SetInputConnection(arrowSource->GetOutputPort());
|
||||
|
||||
return (transformPD->GetOutput());
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -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 ¢er, 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)
|
||||
{
|
||||
return impl_->addPlane(coefficients, id);
|
||||
impl_->showLine(id, pt1, pt2, color);
|
||||
}
|
||||
|
||||
bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id)
|
||||
void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Color &color)
|
||||
{
|
||||
return impl_->addPlane(coefficients, x, y, z, id);
|
||||
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_->getShapePose(id);
|
||||
}
|
||||
|
||||
bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose)
|
||||
{
|
||||
return impl_->setShapePose(id, pose);
|
||||
}
|
||||
|
||||
bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)
|
||||
|
@ -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);
|
||||
|
||||
Color c = vtkcolor(color);
|
||||
actor->GetProperty ()->SetColor (c.val);
|
||||
actor->GetMapper ()->ScalarVisibilityOff ();
|
||||
renderer_->AddActor (actor);
|
||||
|
||||
// Save the pointer/ID pair to the global actor map
|
||||
(*shape_actor_map_)[id] = actor;
|
||||
return (true);
|
||||
// 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);
|
||||
// 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);
|
||||
|
||||
// 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 ¢er, float radius, const Color& color, const std::string &id)
|
||||
{
|
||||
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
|
||||
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
|
||||
if (am_it == shape_actor_map_->end ())
|
||||
return (false);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
// Get the actor pointer
|
||||
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
|
||||
vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
|
||||
vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
|
||||
|
||||
src->SetCenter(center.x, center.y, center.z);
|
||||
src->SetRadius(radius);
|
||||
src->Update ();
|
||||
Color c = vtkcolor(color);
|
||||
actor->GetProperty ()->SetColor (c.val);
|
||||
actor->Modified ();
|
||||
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
bool temp_viz::Viz3d::VizImpl::addText3D (const std::string &text, const cv::Point3f& position, const Color& color, double textScale, const std::string &id)
|
||||
{
|
||||
|
@ -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)
|
||||
{
|
||||
|
@ -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));
|
||||
|
Loading…
Reference in New Issue
Block a user