createSphere and showSphere implementations

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);

modules/viz/src/q/shapes.h

@@ -13,6 +13,7 @@ namespace temp_viz
     CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
     CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Point3f& pt, const Quaternionf& qt,  );
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth);
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max);

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);
 

modules/viz/src/shapes.cpp

@@ -71,6 +71,19 @@ vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, cons
     return (cube->GetOutput ());
 }
 
+vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
+{
+    vtkSmartPointer<vtkSphereSource> sphere = vtkSmartPointer<vtkSphereSource>::New ();
+    sphere->SetRadius (radius);
+    sphere->SetCenter (pt.x, pt.y, pt.z);
+    sphere->SetPhiResolution (10);
+    sphere->SetThetaResolution (10);
+    sphere->LatLongTessellationOff ();
+    sphere->Update ();
+    
+    return (sphere->GetOutput ());
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////
 vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides)
 {

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);

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<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
+        reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createSphere(pt, radius));
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        actor->Modified ();
+    }
+    else
+    {
+        // Create a plane
+        vtkSmartPointer<vtkDataSet> data = createSphere(pt, radius);
+
+        // Create an Actor
+        vtkSmartPointer<vtkLODActor> actor;
+        createActorFromVTKDataSet (data, actor);
+        //  actor->GetProperty ()->SetRepresentationToWireframe ();
+        actor->GetProperty ()->SetRepresentationToSurface ();
+        actor->GetProperty ()->SetLighting (false);
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        renderer_->AddActor(actor);
+
+        // Save the pointer/ID pair to the global actor map
+        (*shape_actor_map_)[id] = actor;
+    }
+}
+
 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();
     }
 

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;