cloudwidget private implementation for duplicate code, add cv_assert(exists),…

cloudwidget private implementation for duplicate code, add cv_assert(exists), fix bug point_step bug in CloudNormalsWidget

modules/viz/include/opencv2/viz/widgets.hpp

@@ -103,6 +103,8 @@ namespace temp_viz
     public:
         CloudWidget(InputArray _cloud, InputArray _colors);
         CloudWidget(InputArray _cloud, const Color &color = Color::white());
+    private:
+        struct CreateCloudWidget;
     };
     
     class CV_EXPORTS CloudNormalsWidget : public Widget

modules/viz/src/simple_widgets.cpp

@@ -304,14 +304,10 @@ temp_viz::TextWidget::TextWidget(const String &text, const Point2i &pos, int fon
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// point cloud widget implementation
 
-temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
+struct temp_viz::CloudWidget::CreateCloudWidget
 {
-    Mat cloud = _cloud.getMat();
-    Mat colors = _colors.getMat();
-    CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
-    CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
-    
-    vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this));
+    static inline vtkSmartPointer<vtkPolyData> create(const Mat &cloud, vtkIdType &nr_points)
+    {
         vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New ();
         vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
         
@@ -319,7 +315,7 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
         
         vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
         vtkSmartPointer<vtkIdTypeArray> initcells;
-    vtkIdType nr_points = cloud.total();
+        nr_points = cloud.total();
         
         points = polydata->GetPoints ();
 
@@ -340,7 +336,6 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
             Vec3f *data_beg = vtkpoints_data<float>(points);
             Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
             nr_points = data_end - data_beg;
-
         }
         else if (cloud.depth() == CV_64F)
         {
@@ -394,7 +389,22 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
         
         // Set the cells and the vertices
         vertices->SetCells (nr_points, cells);
+        return polydata;
+    }
+};
+
+temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors)
+{
+    Mat cloud = _cloud.getMat();
+    Mat colors = _colors.getMat();
+    CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
+    CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
+    
+    vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this));
+    vtkIdType nr_points;
+    vtkSmartPointer<vtkPolyData> polydata = CreateCloudWidget::create(cloud, nr_points);
 
+    // Filter colors
     Vec3b* colors_data = new Vec3b[nr_points];
     NanFilter::copy(colors, colors_data, cloud);
 
@@ -432,88 +442,8 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color)
     CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
     
     vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this));
-    vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New ();
-    vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
-    
-    polydata->SetVerts (vertices);
-    
-    vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
-    vtkSmartPointer<vtkIdTypeArray> initcells;
-    vtkIdType nr_points = cloud.total();
-    
-    points = polydata->GetPoints ();
-
-    if (!points)
-    {
-        points = vtkSmartPointer<vtkPoints>::New ();
-        if (cloud.depth() == CV_32F)
-            points->SetDataTypeToFloat();
-        else if (cloud.depth() == CV_64F)
-            points->SetDataTypeToDouble();
-        polydata->SetPoints (points);
-    }
-    points->SetNumberOfPoints (nr_points);
-    
-    if (cloud.depth() == CV_32F)
-    {
-        // Get a pointer to the beginning of the data array
-        Vec3f *data_beg = vtkpoints_data<float>(points);
-        Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
-        nr_points = data_end - data_beg;
-
-    }
-    else if (cloud.depth() == CV_64F)
-    {
-        // Get a pointer to the beginning of the data array
-        Vec3d *data_beg = vtkpoints_data<double>(points);
-        Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud);
-        nr_points = data_end - data_beg;
-    }
-    points->SetNumberOfPoints (nr_points);
-    
-    // Update cells
-    vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
-    // If no init cells and cells has not been initialized...
-    if (!cells)
-        cells = vtkSmartPointer<vtkIdTypeArray>::New ();
-
-    // If we have less values then we need to recreate the array
-    if (cells->GetNumberOfTuples () < nr_points)
-    {
-        cells = vtkSmartPointer<vtkIdTypeArray>::New ();
-
-        // If init cells is given, and there's enough data in it, use it
-        if (initcells && initcells->GetNumberOfTuples () >= nr_points)
-        {
-            cells->DeepCopy (initcells);
-            cells->SetNumberOfComponents (2);
-            cells->SetNumberOfTuples (nr_points);
-        }
-        else
-        {
-            // If the number of tuples is still too small, we need to recreate the array
-            cells->SetNumberOfComponents (2);
-            cells->SetNumberOfTuples (nr_points);
-            vtkIdType *cell = cells->GetPointer (0);
-            // Fill it with 1s
-            std::fill_n (cell, nr_points * 2, 1);
-            cell++;
-            for (vtkIdType i = 0; i < nr_points; ++i, cell += 2)
-                *cell = i;
-            // Save the results in initcells
-            initcells = vtkSmartPointer<vtkIdTypeArray>::New ();
-            initcells->DeepCopy (cells);
-        }
-    }
-    else
-    {
-        // The assumption here is that the current set of cells has more data than needed
-        cells->SetNumberOfComponents (2);
-        cells->SetNumberOfTuples (nr_points);
-    }
-    
-    // Set the cells and the vertices
-    vertices->SetCells (nr_points, cells);
+    vtkIdType nr_points;
+    vtkSmartPointer<vtkPolyData> polydata = CreateCloudWidget::create(cloud, nr_points);
     
     vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
     mapper->SetInput (polydata);
@@ -557,7 +487,7 @@ struct temp_viz::CloudNormalsWidget::ApplyCloudNormals
             {
                 const _Tp *prow = cloud.ptr<_Tp>(y);
                 const _Tp *nrow = normals.ptr<_Tp>(y);
-                for (vtkIdType x = 0; x < cloud.cols; x += point_step + cch)
+                for (vtkIdType x = 0; x < cloud.cols; x += point_step * cch)
                 {
                     pts[2 * cell_count * 3 + 0] = prow[x];
                     pts[2 * cell_count * 3 + 1] = prow[x+1];

modules/viz/src/viz3d_impl.cpp

@@ -889,9 +889,7 @@ bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id)
 {
     WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
     bool exists = wam_itr != widget_actor_map_->end();
-    
-    if (!exists)
-        return std::cout << "[removeWidget] A widget with id <" << id << "> does not exist!" << std::endl, false;
+    CV_Assert(exists);
 
     if (!removeActorFromRenderer (wam_itr->second.actor))
         return false;
@@ -904,10 +902,8 @@ bool temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &p
 {
     WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
     bool exists = wam_itr != widget_actor_map_->end();
-    if (!exists)
-    {
-        return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false;
-    }
+    CV_Assert(exists);
+    
     vtkLODActor *actor;
     if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor)))
     {
@@ -923,10 +919,8 @@ bool temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f
 {
     WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
     bool exists = wam_itr != widget_actor_map_->end();
-    if (!exists)
-    {
-        return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false;
-    }
+    CV_Assert(exists);
+    
     vtkLODActor *actor;
     if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor)))
     {
@@ -952,10 +946,8 @@ temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) con
 {
     WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
     bool exists = wam_itr != widget_actor_map_->end();
-    if (!exists)
-    {
-        return Affine3f();
-    }
+    CV_Assert(exists);
+    
     vtkLODActor *actor;
     if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor)))
     {