diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 386d71141ea71a0c8fba1772a324af8c045264ed..3a1a14a75bb9efb90372be3373cf930ce57858f2 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 9233ec45f0848644c417f4abdbc7b08bee32cd87..e0665e056d31b8cc28fb181d2b8306978d6a8783 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 29c3de624884bc86021929c4245a3b65cbe8ecf1..2b768949f327d6e9cd763d3f2bfecd4eb86879dd 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 baa4cbde3115d1e772ace11afe2721bd21c0f270..586b6b5a1357c41d69e60f01079a479f2a85aaa0 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 5aa0fe20c96ef268a65d58873f15667080fa21e4..d209ae5d2bdc5ec2f1d44b81a2968439daade161 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 0a93e9c6b247bec16ac0fee495286d05b0ac638c..f9d1ff40e787b5134b3e53caa545040bdfb258cd 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 5307c4ad7812456aa8c601110cf27fceee69f9fa..fa1d604868016d59a28741a3de3e4319902bc826 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;