From b387581d4f89bf96906c9c95b18022da2268618d Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 17 Jun 2013 16:32:52 +0200 Subject: [PATCH 01/11] initial refactoring --- modules/viz/src/q/shapes.h | 11 ++++++- modules/viz/src/shapes.cpp | 65 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 75 insertions(+), 1 deletion(-) diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h index 3ff63750d5..9233ec45f0 100644 --- a/modules/viz/src/q/shapes.h +++ b/modules/viz/src/q/shapes.h @@ -8,7 +8,16 @@ namespace temp_viz { CV_EXPORTS vtkSmartPointer createLine (const cv::Point3f& pt1, const cv::Point3f& pt2); CV_EXPORTS vtkSmartPointer createSphere (const cv::Point3f ¢er, float radius, int sphere_resolution = 10); - + CV_EXPORTS vtkSmartPointer createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30); + CV_EXPORTS vtkSmartPointer createPlane (const Vec4f& coefs); + CV_EXPORTS vtkSmartPointer createPlane (const Vec4f& coefs, const Point3f& pt); + CV_EXPORTS vtkSmartPointer create2DCircle (const Point3f& pt, double radius); + CV_EXPORTS vtkSmartPointer createCube(const Point3f& pt_min, const Point3f& pt_max); +// CV_EXPORTS vtkSmartPointer createCube (const Point3f& pt, const Quaternionf& qt, ); +// CV_EXPORTS vtkSmartPointer createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth); +// CV_EXPORTS vtkSmartPointer createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max); +// +// /** \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 diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp index 955cf38839..baa4cbde31 100644 --- a/modules/viz/src/shapes.cpp +++ b/modules/viz/src/shapes.cpp @@ -5,6 +5,71 @@ inline float rad2deg (float alpha) inline double rad2deg (double alpha){return (alpha * 57.29578);} +vtkSmartPointer temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides) +{ + const cv::Point3f pt2 = pt_on_axis + axis_direction; + vtkSmartPointer line = vtkSmartPointer::New (); + line->SetPoint1 (pt_on_axis.x, pt_on_axis.y, pt_on_axis.z); + line->SetPoint2 (pt2.x, pt2.y, pt2.z); + + vtkSmartPointer tuber = vtkSmartPointer::New (); + tuber->SetInputConnection (line->GetOutputPort ()); + tuber->SetRadius (radius); + tuber->SetNumberOfSides (numsides); + return (tuber->GetOutput ()); +} + +vtkSmartPointer temp_viz::createPlane (const cv::Vec4f& coefs) +{ + vtkSmartPointer plane = vtkSmartPointer::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 temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt) +{ + vtkSmartPointer plane = vtkSmartPointer::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]); + + 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]); + + return (plane->GetOutput ()); +} + +vtkSmartPointer temp_viz::create2DCircle (const cv::Point3f& pt, double radius) +{ + vtkSmartPointer disk = vtkSmartPointer::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 t = vtkSmartPointer::New (); + t->Identity (); + t->Translate (pt.x, pt.y, pt.z); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (disk->GetOutputPort ()); + + return (tf->GetOutput ()); +} + +vtkSmartPointer temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max) +{ + vtkSmartPointer cube = vtkSmartPointer::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 temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides) From 26a68232078eee2a66c265561f4d9c3f211ce43e Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Fri, 21 Jun 2013 12:48:27 +0200 Subject: [PATCH 02/11] showLine implementation --- modules/viz/include/opencv2/viz/viz3d.hpp | 2 + modules/viz/src/q/viz3d_impl.hpp | 3 +- modules/viz/src/viz3d.cpp | 5 +++ modules/viz/src/viz3d_impl.cpp | 49 ++++++++++++++--------- modules/viz/test/test_viz3d.cpp | 7 ++-- 5 files changed, 42 insertions(+), 24 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index d7e47074dd..ab76302cbe 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -31,6 +31,8 @@ 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); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 7afa86a7ac..af43a9b503 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -137,9 +137,10 @@ 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); 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"); diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index dfade9926d..7e0d324dff 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -78,6 +78,11 @@ void temp_viz::Viz3d::spinOnce (int time, bool force_redraw) impl_->spinOnce(time, force_redraw); } +void temp_viz::Viz3d::showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color) +{ + impl_->showLine(id, pt1, pt2, color); +} + bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, const String &id) { return impl_->addPlane(coefficients, id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 2ee07ffe71..8fc4d632de 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -266,33 +266,42 @@ 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; - - vtkSmartPointer data = createLine (pt1, pt2); + bool exists = (am_it != shape_actor_map_->end()); + + // If it exists just update + if (exists) + { + vtkSmartPointer actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(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 data = createLine (pt1, pt2); - // Create an Actor - vtkSmartPointer actor; - createActorFromVTKDataSet (data, actor); - actor->GetProperty ()->SetRepresentationToWireframe (); + // Create an Actor + vtkSmartPointer 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; + } } - - bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 5c9141f988..e74ee1301d 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -86,18 +86,19 @@ 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; - + 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)); 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)); angle_x += 0.1f; angle_y -= 0.1f; From f94c2414e65914c406683ad68e0f9ed4abf896c1 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Fri, 21 Jun 2013 14:00:22 +0200 Subject: [PATCH 03/11] showPlane implementation --- modules/viz/include/opencv2/viz/viz3d.hpp | 2 + modules/viz/src/q/viz3d_impl.hpp | 24 +-------- modules/viz/src/viz3d.cpp | 8 +-- modules/viz/src/viz3d_impl.cpp | 63 +++++++++++++++++++++++ modules/viz/src/viz_main.cpp | 56 -------------------- modules/viz/test/test_viz3d.cpp | 12 +++-- 6 files changed, 80 insertions(+), 85 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index ab76302cbe..9d0e60f688 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -33,6 +33,8 @@ namespace temp_viz 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); + void showPlane(const String &id, const Vec4f &coefs); + void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index af43a9b503..bff67e71f5 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -139,6 +139,8 @@ public: } 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); + void showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); @@ -178,28 +180,6 @@ public: */ 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") diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 7e0d324dff..5f0342a400 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -83,14 +83,14 @@ void temp_viz::Viz3d::showLine(const String &id, const Point3f &pt1, const Point impl_->showLine(id, pt1, pt2, color); } -bool temp_viz::Viz3d::addPlane (const ModelCoefficients &coefficients, const String &id) +void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs) { - return impl_->addPlane(coefficients, id); + impl_->showPlane(id, coefs); } -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 Point3f &pt) { - return impl_->addPlane(coefficients, x, y, z, id); + impl_->showPlane(id, coefs, pt); } bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 8fc4d632de..15938f8204 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -302,6 +302,69 @@ void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt } } +void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coefs) +{ + // Check if this Id already exists + ShapeActorMap::iterator am_it = shape_actor_map_->find (id); + bool exists = (am_it != shape_actor_map_->end()); + + // If it exists just update + if (exists) + { + vtkSmartPointer actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs)); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = createPlane (coefs); + + // Create an Actor + vtkSmartPointer 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; + } +} + +void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt) +{ + // Check if this Id already exists + ShapeActorMap::iterator am_it = shape_actor_map_->find (id); + bool exists = (am_it != shape_actor_map_->end()); + + // If it exists just update + if (exists) + { + vtkSmartPointer actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs, pt)); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = createPlane (coefs, pt); + + // Create an Actor + vtkSmartPointer 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; + } +} + + bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index 1ea8368af9..7abb00d10c 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -1088,62 +1088,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 <"< already exists! Please choose a different id and retry." << std::endl; - return (false); - } - - vtkSmartPointer data = createPlane (coefficients); - - // Create an Actor - vtkSmartPointer 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 data = createPlane (coefficients, x, y, z); - - // Create an Actor - vtkSmartPointer 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) { diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index e74ee1301d..17a6dc4768 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -93,19 +93,25 @@ TEST(Viz_viz3d, accuracy) int col_green = 0; int col_red = 0; + v.showPlane("plane1", cv::Vec4f(1.0f,1.0f,1.0f,1.0f)); + 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)); 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)); + 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; From 93fe2f6e4d20b38d5e0a43c38adced41912bbc24 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Fri, 21 Jun 2013 14:09:12 +0200 Subject: [PATCH 04/11] showPlane with color --- modules/viz/include/opencv2/viz/viz3d.hpp | 4 ++-- modules/viz/src/q/viz3d_impl.hpp | 4 ++-- modules/viz/src/viz3d.cpp | 8 ++++---- modules/viz/src/viz3d_impl.cpp | 16 ++++++++++++---- modules/viz/test/test_viz3d.cpp | 4 +--- 5 files changed, 21 insertions(+), 15 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 9d0e60f688..806f782478 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -33,8 +33,8 @@ namespace temp_viz 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); - void showPlane(const String &id, const Vec4f &coefs); - void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt); + void showPlane(const String &id, const Vec4f &coefs, const Color &color); + void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index bff67e71f5..fb1855de95 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -139,8 +139,8 @@ public: } 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); - void showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt); + 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); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 5f0342a400..ec8cf73c9c 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -83,14 +83,14 @@ void temp_viz::Viz3d::showLine(const String &id, const Point3f &pt1, const Point impl_->showLine(id, pt1, pt2, color); } -void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs) +void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Color &color) { - impl_->showPlane(id, coefs); + impl_->showPlane(id, coefs, color); } -void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Point3f &pt) +void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color) { - impl_->showPlane(id, coefs, pt); + impl_->showPlane(id, coefs, pt, color); } bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 15938f8204..dfa64d9310 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -302,17 +302,19 @@ void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt } } -void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coefs) +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 actor = vtkLODActor::SafeDownCast (am_it->second); reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); actor->Modified (); } else @@ -326,6 +328,8 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe // 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 @@ -333,17 +337,19 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe } } -void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt) +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 actor = vtkLODActor::SafeDownCast (am_it->second); reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs, pt)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); actor->Modified (); } else @@ -357,6 +363,8 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coe // 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 diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 17a6dc4768..3d3d2c108d 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -93,8 +93,6 @@ TEST(Viz_viz3d, accuracy) int col_green = 0; int col_red = 0; - v.showPlane("plane1", cv::Vec4f(1.0f,1.0f,1.0f,1.0f)); - while(!v.wasStopped()) { // Creating new point cloud with id cloud1 @@ -104,7 +102,7 @@ TEST(Viz_viz3d, accuracy) 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)); + 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)); angle_x += 0.1f; angle_y -= 0.1f; From 55683e7b3b9e84e40f03fc25ef86e7e1f04477c6 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 26 Jun 2013 10:46:26 +0200 Subject: [PATCH 05/11] showCube implementation --- modules/viz/include/opencv2/viz/viz3d.hpp | 1 + modules/viz/src/q/viz3d_impl.hpp | 1 + modules/viz/src/viz3d.cpp | 5 ++++ modules/viz/src/viz3d_impl.cpp | 35 +++++++++++++++++++++++ modules/viz/test/test_viz3d.cpp | 1 + 5 files changed, 43 insertions(+) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 806f782478..624798b81f 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -35,6 +35,7 @@ namespace temp_viz void showLine(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color); void showPlane(const String &id, const Vec4f &coefs, const Color &color); void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color); + void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index fb1855de95..3ba3e19ca1 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -141,6 +141,7 @@ public: 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); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index ec8cf73c9c..c5bd70c8d4 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -93,6 +93,11 @@ void temp_viz::Viz3d::showPlane(const String &id, const Vec4f &coefs, const Poin 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); +} + bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) { return impl_->removeCoordinateSystem(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index dfa64d9310..6c32a55292 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -372,6 +372,41 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coe } } +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 actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(createCube(pt1, pt2)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = createCube (pt1, pt2); + + // Create an Actor + vtkSmartPointer 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; + } +} + bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 3d3d2c108d..4c3490ce07 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -103,6 +103,7 @@ TEST(Viz_viz3d, accuracy) 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)); angle_x += 0.1f; angle_y -= 0.1f; From 98edabd42cee3b0d336a4edf0ffb86c8e930174a Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 26 Jun 2013 11:13:34 +0200 Subject: [PATCH 06/11] showCylinder implementation --- modules/viz/include/opencv2/viz/viz3d.hpp | 9 +++--- modules/viz/src/q/viz3d_impl.hpp | 1 + modules/viz/src/viz3d.cpp | 5 ++++ modules/viz/src/viz3d_impl.cpp | 34 +++++++++++++++++++++++ modules/viz/test/test_viz3d.cpp | 2 ++ 5 files changed, 47 insertions(+), 4 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 624798b81f..3db89969bb 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -32,10 +32,11 @@ namespace temp_viz 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); - void showPlane(const String &id, const Vec4f &coefs, const Color &color); - void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color); - void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color); + 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)); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 3ba3e19ca1..07e7508a11 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -142,6 +142,7 @@ public: 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); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index c5bd70c8d4..c9bf0d9203 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -98,6 +98,11 @@ void temp_viz::Viz3d::showCube(const String &id, const Point3f &pt1, const Point 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); +} + bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) { return impl_->removeCoordinateSystem(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 6c32a55292..63187c52d4 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -407,6 +407,40 @@ void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, c } } +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 actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(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 data = createCylinder(pt_on_axis, axis_direction, radius, num_sides); + + // Create an Actor + vtkSmartPointer 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; + } +} bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 4c3490ce07..1fd51318e7 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -104,6 +104,8 @@ TEST(Viz_viz3d, accuracy) 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)); + angle_x += 0.1f; angle_y -= 0.1f; From fd6eeac6aa692a9c1a2e3e38f7a8080b87039afe Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 26 Jun 2013 11:32:12 +0200 Subject: [PATCH 07/11] showCylinder and showCircle implementations --- modules/viz/include/opencv2/viz/viz3d.hpp | 1 + modules/viz/src/q/viz3d_impl.hpp | 1 + modules/viz/src/viz3d.cpp | 5 ++++ modules/viz/src/viz3d_impl.cpp | 35 +++++++++++++++++++++++ modules/viz/test/test_viz3d.cpp | 2 +- 5 files changed, 43 insertions(+), 1 deletion(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 3db89969bb..0f5937a7bb 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -37,6 +37,7 @@ namespace temp_viz 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)); bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 07e7508a11..e5a6cbaa15 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -143,6 +143,7 @@ public: 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); bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index c9bf0d9203..887d10062c 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -103,6 +103,11 @@ void temp_viz::Viz3d::showCylinder(const String &id, const Point3f &pt_on_axis, 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); +} + bool temp_viz::Viz3d::removeCoordinateSystem (const String &id) { return impl_->removeCoordinateSystem(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 63187c52d4..0c9718f57c 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -442,6 +442,41 @@ void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt } } +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 actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(create2DCircle(pt, radius)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = create2DCircle(pt, radius); + + // Create an Actor + vtkSmartPointer 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; + } +} + bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 1fd51318e7..1d09a16440 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -105,7 +105,7 @@ TEST(Viz_viz3d, accuracy) 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.showCircle("circle1", cv::Point3f(0,0,0), fabs(pos_x), temp_viz::Color(0,255,0)); angle_x += 0.1f; angle_y -= 0.1f; From 17bdc29d5b3271056c9b4f85fff1a35e1bb920cf Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 26 Jun 2013 12:52:43 +0200 Subject: [PATCH 08/11] setShapePose and getShapePose implementations --- modules/viz/include/opencv2/viz/viz3d.hpp | 3 ++ modules/viz/src/q/viz3d_impl.hpp | 3 ++ modules/viz/src/viz3d.cpp | 10 +++++ modules/viz/src/viz3d_impl.cpp | 45 +++++++++++++++++++++++ modules/viz/src/viz_main.cpp | 7 ++++ modules/viz/test/test_viz3d.cpp | 19 +++++----- 6 files changed, 78 insertions(+), 9 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 0f5937a7bb..386d71141e 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -38,6 +38,9 @@ namespace temp_viz 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)); + + 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"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index e5a6cbaa15..29c3de6248 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -144,6 +144,8 @@ public: 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); + 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 addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); @@ -422,6 +424,7 @@ private: //void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer &vtk_matrix); void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer &vtk_matrix); +void convertToCvMatrix (const vtkSmartPointer &vtk_matrix, cv::Matx44f &m); /** \brief Convert origin and orientation to vtkMatrix4x4 * \param[in] origin the point cloud origin diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 887d10062c..5aa0fe20c9 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -108,6 +108,16 @@ void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double rad impl_->showCircle(id, pt, radius, 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) { return impl_->removeCoordinateSystem(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 0c9718f57c..0a93e9c6b2 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -477,6 +477,51 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, } } +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 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 matrix = vtkSmartPointer::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) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index 7abb00d10c..a55247b7bf 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -1335,6 +1335,13 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointerSetElement (i, k, m (i, k)); } +void temp_viz::convertToCvMatrix (const vtkSmartPointer &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 &vtk_matrix, Eigen::Matrix4f &m) { diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 1d09a16440..5307c4ad78 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -92,20 +92,21 @@ TEST(Viz_viz3d, accuracy) int col_blue = 0; int col_green = 0; int col_red = 0; + v.showCircle("circle1", cv::Point3f(0,0,0), fabs(1.0f), temp_viz::Color(0,255,0)); 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)); - 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.showCircle("circle1", cv::Point3f(0,0,0), fabs(pos_x), temp_viz::Color(0,255,0)); +// 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); angle_x += 0.1f; angle_y -= 0.1f; From c4a07b7531c06a64a58e93be97b41bd20d902a91 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Thu, 27 Jun 2013 11:51:36 +0300 Subject: [PATCH 09/11] createSphere and showSphere implementations --- modules/viz/include/opencv2/viz/viz3d.hpp | 1 + modules/viz/src/q/shapes.h | 1 + modules/viz/src/q/viz3d_impl.hpp | 2 ++ modules/viz/src/shapes.cpp | 13 ++++++++ modules/viz/src/viz3d.cpp | 5 +++ modules/viz/src/viz3d_impl.cpp | 37 ++++++++++++++++++++++- modules/viz/test/test_viz3d.cpp | 4 ++- 7 files changed, 61 insertions(+), 2 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 386d71141e..3a1a14a75b 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -38,6 +38,7 @@ namespace temp_viz 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)); Affine3f getShapePose(const String &id); bool setShapePose(const String &id, const Affine3f &pose); diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h index 9233ec45f0..e0665e056d 100644 --- a/modules/viz/src/q/shapes.h +++ b/modules/viz/src/q/shapes.h @@ -13,6 +13,7 @@ namespace temp_viz CV_EXPORTS vtkSmartPointer createPlane (const Vec4f& coefs, const Point3f& pt); CV_EXPORTS vtkSmartPointer create2DCircle (const Point3f& pt, double radius); CV_EXPORTS vtkSmartPointer createCube(const Point3f& pt_min, const Point3f& pt_max); + CV_EXPORTS vtkSmartPointer createSphere (const Point3f& pt, double radius); // CV_EXPORTS vtkSmartPointer createCube (const Point3f& pt, const Quaternionf& qt, ); // CV_EXPORTS vtkSmartPointer createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth); // CV_EXPORTS vtkSmartPointer createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 29c3de6248..2b768949f3 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -144,6 +144,8 @@ public: 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); + Affine3f getShapePose (const String &id); bool setShapePose (const String &id, const Affine3f &pose); diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp index baa4cbde31..586b6b5a13 100644 --- a/modules/viz/src/shapes.cpp +++ b/modules/viz/src/shapes.cpp @@ -71,6 +71,19 @@ vtkSmartPointer temp_viz::createCube(const cv::Point3f& pt_min, cons return (cube->GetOutput ()); } +vtkSmartPointer temp_viz::createSphere (const Point3f& pt, double radius) +{ + vtkSmartPointer sphere = vtkSmartPointer::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 temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides) { diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 5aa0fe20c9..d209ae5d2b 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -108,6 +108,11 @@ void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double rad 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); +} + cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id) { return impl_->getShapePose(id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 0a93e9c6b2..f9d1ff40e7 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -477,6 +477,41 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, } } +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 actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(createSphere(pt, radius)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = createSphere(pt, radius); + + // Create an Actor + vtkSmartPointer 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 @@ -485,7 +520,7 @@ cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String &id) if (!exists) { - std::cout << "[getShapePose] A shape with id " << id << " does not exist!" << std::endl; + std::cout << "[getShapePose] A shape with id <" << id << "> does not exist!" << std::endl; return Affine3f(); } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 5307c4ad78..fa1d604868 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -92,7 +92,8 @@ TEST(Viz_viz3d, accuracy) int col_blue = 0; int col_green = 0; int col_red = 0; - v.showCircle("circle1", cv::Point3f(0,0,0), fabs(1.0f), temp_viz::Color(0,255,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)); while(!v.wasStopped()) { @@ -107,6 +108,7 @@ TEST(Viz_viz3d, accuracy) // 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); angle_x += 0.1f; angle_y -= 0.1f; From 4a19ff3a7c1a65d99fa974800d2b2011f31b5975 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Thu, 27 Jun 2013 12:33:19 +0300 Subject: [PATCH 10/11] createArrow and showArrow implementation without label --- modules/viz/include/opencv2/viz/viz3d.hpp | 1 + modules/viz/src/precomp.hpp | 1 + modules/viz/src/q/shapes.h | 1 + modules/viz/src/q/viz3d_impl.hpp | 1 + modules/viz/src/shapes.cpp | 55 +++++++++++++++++++++++ modules/viz/src/viz3d.cpp | 5 +++ modules/viz/src/viz3d_impl.cpp | 36 +++++++++++++++ modules/viz/test/test_viz3d.cpp | 2 + 8 files changed, 102 insertions(+) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 3a1a14a75b..2b3e6e7344 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -39,6 +39,7 @@ namespace temp_viz 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); diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp index 9076a3bb47..de7104455c 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h index e0665e056d..1ebc6ae80a 100644 --- a/modules/viz/src/q/shapes.h +++ b/modules/viz/src/q/shapes.h @@ -14,6 +14,7 @@ namespace temp_viz CV_EXPORTS vtkSmartPointer create2DCircle (const Point3f& pt, double radius); CV_EXPORTS vtkSmartPointer createCube(const Point3f& pt_min, const Point3f& pt_max); CV_EXPORTS vtkSmartPointer createSphere (const Point3f& pt, double radius); + CV_EXPORTS vtkSmartPointer createArrow (const Point3f& pt1, const Point3f& pt2); // CV_EXPORTS vtkSmartPointer createCube (const Point3f& pt, const Quaternionf& qt, ); // CV_EXPORTS vtkSmartPointer createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth); // CV_EXPORTS vtkSmartPointer createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 2b768949f3..50d50bb2c3 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -145,6 +145,7 @@ public: 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); diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp index 586b6b5a13..3f9fcd23e0 100644 --- a/modules/viz/src/shapes.cpp +++ b/modules/viz/src/shapes.cpp @@ -84,6 +84,61 @@ vtkSmartPointer temp_viz::createSphere (const Point3f& pt, double ra return (sphere->GetOutput ()); } +vtkSmartPointer temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2) +{ + vtkSmartPointer arrowSource = vtkSmartPointer::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 matrix = vtkSmartPointer::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 transform = + vtkSmartPointer::New(); + transform->Translate(startPoint); + transform->Concatenate(matrix); + transform->Scale(length, length, length); + + // Transform the polydata + vtkSmartPointer transformPD = + vtkSmartPointer::New(); + transformPD->SetTransform(transform); + transformPD->SetInputConnection(arrowSource->GetOutputPort()); + + return (transformPD->GetOutput()); +} + //////////////////////////////////////////////////////////////////////////////////////////// vtkSmartPointer temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides) { diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index d209ae5d2b..65d036d9e3 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -113,6 +113,11 @@ void temp_viz::Viz3d::showSphere (const String &id, const Point3f &pt, double ra 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); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index f9d1ff40e7..af564b34c2 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -512,6 +512,42 @@ void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt, } } +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 actor = vtkLODActor::SafeDownCast (am_it->second); + reinterpret_cast(actor->GetMapper ())->SetInput(createArrow(pt1,pt2)); + actor->GetProperty ()->SetColor (c.val); + actor->GetMapper ()->ScalarVisibilityOff (); + actor->Modified (); + } + else + { + // Create a plane + vtkSmartPointer data = createArrow(pt1,pt2); + + // Create an Actor + vtkSmartPointer 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 diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index fa1d604868..8a7472c514 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -94,6 +94,7 @@ TEST(Viz_viz3d, accuracy) 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)); while(!v.wasStopped()) { @@ -109,6 +110,7 @@ TEST(Viz_viz3d, accuracy) // 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; From 1830059969d6aa20b51d668c2e77b1971b504116 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Thu, 27 Jun 2013 13:05:52 +0300 Subject: [PATCH 11/11] removed redundant addShape methods --- modules/viz/src/q/shapes.h | 113 -------------------- modules/viz/src/q/viz3d_impl.hpp | 78 -------------- modules/viz/src/shapes.cpp | 172 ------------------------------- modules/viz/src/viz3d.cpp | 5 - modules/viz/src/viz3d_impl.cpp | 66 ------------ modules/viz/src/viz_main.cpp | 130 ----------------------- 6 files changed, 564 deletions(-) diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h index 1ebc6ae80a..be83cd59e6 100644 --- a/modules/viz/src/q/shapes.h +++ b/modules/viz/src/q/shapes.h @@ -15,119 +15,6 @@ namespace temp_viz CV_EXPORTS vtkSmartPointer createCube(const Point3f& pt_min, const Point3f& pt_max); CV_EXPORTS vtkSmartPointer createSphere (const Point3f& pt, double radius); CV_EXPORTS vtkSmartPointer createArrow (const Point3f& pt1, const Point3f& pt2); -// CV_EXPORTS vtkSmartPointer createCube (const Point3f& pt, const Quaternionf& qt, ); -// CV_EXPORTS vtkSmartPointer createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth); -// CV_EXPORTS vtkSmartPointer createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max); -// -// - /** \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 data = temp_viz::createCylinder (cylinder_coeff, numsides); - * \endcode - * - * \ingroup visualization - */ - CV_EXPORTS vtkSmartPointer 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 data = temp_viz::createPlane (plane_coeff); - * \endcode - * - * \ingroup visualization - */ - CV_EXPORTS vtkSmartPointer 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 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 data = temp_viz::create2DCircle (circle_coeff, z); - * \endcode - * - * \ingroup visualization - */ - CV_EXPORTS vtkSmartPointer 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 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 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 createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max); - /** \brief Allocate a new unstructured grid smartpointer. For internal use only. * \param[out] polydata the resultant unstructured grid. */ diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 50d50bb2c3..cf6d7db3be 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -153,8 +153,6 @@ public: bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon"); bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow"); bool addArrow (const 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 polydata, const std::string & id = "PolyData"); @@ -162,82 +160,6 @@ public: bool addModelFromPLYFile (const std::string &filename, const std::string &id = "PLYModel"); bool addModelFromPLYFile (const std::string &filename, vtkSmartPointer 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 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 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 (); diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp index 3f9fcd23e0..362b615df1 100644 --- a/modules/viz/src/shapes.cpp +++ b/modules/viz/src/shapes.cpp @@ -139,178 +139,6 @@ vtkSmartPointer temp_viz::createArrow (const Point3f& pt1, const Poi return (transformPD->GetOutput()); } -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides) -{ - vtkSmartPointer line = vtkSmartPointer::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 tuber = vtkSmartPointer::New (); - tuber->SetInputConnection (line->GetOutputPort ()); - tuber->SetRadius (coefficients.values[6]); - tuber->SetNumberOfSides (numsides); - - return (tuber->GetOutput ()); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createCube (const temp_viz::ModelCoefficients &coefficients) -{ - // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth] - vtkSmartPointer t = vtkSmartPointer::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 cube = vtkSmartPointer::New (); - cube->SetXLength (coefficients.values[7]); - cube->SetYLength (coefficients.values[8]); - cube->SetZLength (coefficients.values[9]); - - vtkSmartPointer tf = vtkSmartPointer::New (); - tf->SetTransform (t); - tf->SetInputConnection (cube->GetOutputPort ()); - - return (tf->GetOutput ()); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth) -{ - // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth] - vtkSmartPointer t = vtkSmartPointer::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 cube = vtkSmartPointer::New (); - cube->SetXLength (width); - cube->SetYLength (height); - cube->SetZLength (depth); - - vtkSmartPointer tf = vtkSmartPointer::New (); - tf->SetTransform (t); - tf->SetInputConnection (cube->GetOutputPort ()); - - return (tf->GetOutput ()); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max) -{ - vtkSmartPointer t = vtkSmartPointer::New (); - vtkSmartPointer cube = vtkSmartPointer::New (); - cube->SetBounds (x_min, x_max, y_min, y_max, z_min, z_max); - return (cube->GetOutput ()); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients) -{ - vtkSmartPointer plane = vtkSmartPointer::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 temp_viz::createPlane (const temp_viz::ModelCoefficients &coefficients, double x, double y, double z) -{ - vtkSmartPointer plane = vtkSmartPointer::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 temp_viz::create2DCircle (const temp_viz::ModelCoefficients &coefficients, double z) -{ - vtkSmartPointer disk = vtkSmartPointer::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 could be with - /* - vtkSmartPointer circle = vtkSmartPointer::New(); - circle->SetRadius (coefficients.values[2]); - circle->SetNumberOfSides (100); - - vtkSmartPointer tube = vtkSmartPointer::New(); - tube->SetInput (circle->GetOutput()); - tube->SetNumberOfSides (25); - tube->SetRadius (0.001); - */ - - // Set the circle origin - vtkSmartPointer t = vtkSmartPointer::New (); - t->Identity (); - t->Translate (coefficients.values[0], coefficients.values[1], z); - - vtkSmartPointer tf = vtkSmartPointer::New (); - tf->SetTransform (t); - tf->SetInputConnection (disk->GetOutputPort ()); - /* - tf->SetInputConnection (tube->GetOutputPort ()); - */ - - return (tf->GetOutput ()); -} - -//////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer temp_viz::createSphere (const cv::Point3f& center, float radius, int sphere_resolution) -{ - // Set the sphere origin - vtkSmartPointer t = vtkSmartPointer::New (); - t->Identity (); - t->Translate (center.x, center.y, center.z); - - vtkSmartPointer s_sphere = vtkSmartPointer::New (); - s_sphere->SetRadius (radius); - s_sphere->SetPhiResolution (sphere_resolution); - s_sphere->SetThetaResolution (sphere_resolution); - s_sphere->LatLongTessellationOff (); - - vtkSmartPointer tf = vtkSmartPointer::New (); - tf->SetTransform (t); - tf->SetInputConnection (s_sphere->GetOutputPort ()); - tf->Update (); - - return (tf->GetOutput ()); -} - //////////////////////////////////////////////////////////////////////////////////////////// vtkSmartPointer temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2) { diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 65d036d9e3..bb2974cd38 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -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(); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index af564b34c2..a8aedc18e3 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -958,72 +958,6 @@ bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3 return (true); } -#include -//////////////////////////////////////////////////////////////////////////////////////////// -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 <"< already exists! Please choose a different id and retry." << std::endl, false; - - //vtkSmartPointer data = createSphere (center.getVector4fMap (), radius); - vtkSmartPointer data = vtkSmartPointer::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 mapper = vtkSmartPointer::New (); - mapper->SetInputConnection (data->GetOutputPort ()); - - // Create an Actor - vtkSmartPointer actor = vtkSmartPointer::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) { diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index a55247b7bf..e03ace6350 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -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 <"< already exists! Please choose a different id and retry." << std::endl; - return (false); - } - - vtkSmartPointer data = createCylinder (coefficients); - - // Create an Actor - vtkSmartPointer 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 data = createCube (coefficients); - - // Create an Actor - vtkSmartPointer 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 data = createCube (t, q, width, height, depth); - - // Create an Actor - vtkSmartPointer 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 data = createCube (x_min, x_max, y_min, y_max, z_min, z_max); - - // Create an Actor - vtkSmartPointer 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 polydata, const std::string & id) { @@ -1088,31 +983,6 @@ bool temp_viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, 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 <"< already exists! Please choose a different id and retry.\n" << std::endl; - return (false); - } - - vtkSmartPointer data = create2DCircle (coefficients); - - // Create an Actor - vtkSmartPointer 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) {