refactored showPointCloud to for shorter code. Implemented NanFilter::copy<T>() function

modules/viz/include/opencv2/viz/types.hpp

@@ -31,6 +31,10 @@ namespace temp_viz
     typedef cv::InputArray InputArray;
     using cv::Point3_;
     using cv::Vec;
+    using cv::Mat_;
+    using cv::DataDepth;
+    using cv::DataType;
+
 
 
 
@@ -87,9 +91,10 @@ namespace temp_viz
 
     inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); }
 
+
+
     template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
-    
-    Vec3d operator*(const Affine3f& affine, const Vec3d& vec);
+
     
     inline bool isNan(float x)
     {

modules/viz/src/q/viz3d_impl.hpp

@@ -447,84 +447,65 @@ void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<
 void convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m);
 
 
-
-
-template<typename _Tp, typename _Ts, typename _Tc> inline int copy_non_nan_loop(_Tp *d, const Mat& s, const Mat& c)
+struct NanFilter
 {
-    CV_Assert(s.size() == c.size());
-    int j = 0;
-    for(int y = 0; y < s.rows; ++y)
+    template<typename _Tp, typename _Msk>
+    struct Impl
     {
-        const _Ts* srow = s.ptr<_Ts>(y);
-        const _Tc* crow = c.ptr<_Tc>(y);
-        for(int x = 0; x < s.cols; ++x)
-            if (!isNan(crow[x]))
-                d[j++] = _Tp((srow[x])[0], (srow[x])[1], (srow[x])[2]);
-    }
-    return j;
-}
+        typedef Vec<_Tp, 3> _Out;
 
-/** \brief Assign a value to a variable if another variable is not NaN
-    * \param[in] d the destination variable
-    * \param[in] s the source variable
-    * \param[in] c the values to be controlled if NaN (can be different from s)
-    * \param[out] j number of points that are copied
-    */
-template<typename _Tp> inline int copy_non_nans(_Tp *d, const Mat& s, const Mat& c)
-{
-    CV_Assert(s.size() == c.size());
-    
-    int j = 0;
-    if (s.channels() > 3)
-    {
-        if (s.type() == CV_32FC4)
+        static _Out* copy(const Mat& source, _Out* output, const Mat& nan_mask)
         {
-            switch(c.type())
-            {
-            case CV_32FC3: j = copy_non_nan_loop<_Tp, Vec4f, Vec3f>(d, s, c); break;
-            case CV_32FC4: j = copy_non_nan_loop<_Tp, Vec4f, Vec4f>(d, s, c); break;
-            case CV_64FC3: j = copy_non_nan_loop<_Tp, Vec4f, Vec3d>(d, s, c); break;
-            case CV_64FC4: j = copy_non_nan_loop<_Tp, Vec4f, Vec4d>(d, s, c); break;
-            }
-        }
-        else if (s.type() == CV_64FC4)
-        {
-            switch(c.type())
+            CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
+            CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
+            CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
+
+            int s_chs = source.channels();
+            int m_chs = nan_mask.channels();
+
+            for(int y = 0; y < source.rows; ++y)
             {
-            case CV_32FC3: j = copy_non_nan_loop<_Tp, Vec4d, Vec3f>(d, s, c); break;
-            case CV_32FC4: j = copy_non_nan_loop<_Tp, Vec4d, Vec4f>(d, s, c); break;
-            case CV_64FC3: j = copy_non_nan_loop<_Tp, Vec4d, Vec3d>(d, s, c); break;
-            case CV_64FC4: j = copy_non_nan_loop<_Tp, Vec4d, Vec4d>(d, s, c); break;
+                const _Tp* srow = source.ptr<_Tp>(y);
+                const _Msk* mrow = nan_mask.ptr<_Msk>(y);
+
+                for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
+                    if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
+                        *output++ = _Out(srow);
             }
+            return output;
         }
-    }
-    else
+    };
+
+    template<typename _Tp>
+    static inline Vec<_Tp, 3>* copy(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
     {
-        switch(c.type())
-        {
-        case CV_32FC3: j = copy_non_nan_loop<_Tp, _Tp, Vec3f>(d, s, c); break;
-        case CV_32FC4: j = copy_non_nan_loop<_Tp, _Tp, Vec4f>(d, s, c); break;
-        case CV_64FC3: j = copy_non_nan_loop<_Tp, _Tp, Vec3d>(d, s, c); break;
-        case CV_64FC4: j = copy_non_nan_loop<_Tp, _Tp, Vec4d>(d, s, c); break;
-        }
+        CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
+
+        typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
+        const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copy, &NanFilter::Impl<_Tp, double>::copy };
+
+        return table[nan_mask.depth() - 5](source, output, nan_mask);
     }
-    return j;
-}
+};
 
-/** \brief Transform points in an array
-    * \param[in] d the destination variable
-    * \param[in] lenth the length of the d array
-    * \param[in] pose affine transform to be applied on each point in d
-    */
-template<typename _Tp> inline void transform_non_nans(_Tp* d, int length, const Affine3f& pose = Affine3f::Identity())
+struct ApplyAffine
 {
-    for (int i = 0; i < length; ++i)
+    const Affine3f& affine_;
+    ApplyAffine(const Affine3f& affine) : affine_(affine) {}
+
+    template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine * p; }
+
+    template<typename _Tp> Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v)
     {
-        d[i] = pose * d[i];
+        const float* m = affine_.matrix.val;
+
+        Vec<_Tp, 3> result;
+        result[0] = m[0] * v[0] + m[1] * v[1] + m[ 2] * v[2] + m[ 3];
+        result[1] = m[4] * v[0] + m[5] * v[1] + m[ 6] * v[2] + m[ 7];
+        result[2] = m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11];
+        return result;
     }
-}
+};
 
 }
 
-
-

modules/viz/src/types.cpp

@@ -22,12 +22,3 @@ temp_viz::Color temp_viz::Color::white()   { return Color(255, 255, 255); }
 
 temp_viz::Color temp_viz::Color::gray()    { return Color(128, 128, 128); }
 
-temp_viz::Vec3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Vec3d& vec)
-{
-    const temp_viz::Matx44f& m = affine.matrix;
-    temp_viz::Vec3d result;
-    result[0] = m.val[0] * vec[0] + m.val[1] * vec[1] + m.val[ 2] * vec[2] + m.val[ 3];
-    result[1] = m.val[4] * vec[0] + m.val[5] * vec[1] + m.val[ 6] * vec[2] + m.val[ 7];
-    result[2] = m.val[8] * vec[0] + m.val[9] * vec[1] + m.val[10] * vec[2] + m.val[11];
-    return result;
-}
\ No newline at end of file

modules/viz/src/viz3d.cpp

@@ -99,3 +99,4 @@ void temp_viz::Viz3d::registerMouseCallback(void (*callback)(const cv::MouseEven
 }
 
 bool temp_viz::Viz3d::wasStopped() const { return impl_->wasStopped(); }
+

modules/viz/src/viz3d_impl.cpp

@@ -2,6 +2,27 @@
 #include <q/shapes.h>
 #include <q/viz3d_impl.hpp>
 
+namespace temp_viz
+{
+    template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
+
+    template<> Vec3f* vtkpoints_data<float>(vtkSmartPointer<vtkPoints>& points)
+    {
+        CV_Assert(points->GetDataType() == VTK_FLOAT);
+        vtkDataArray *data = points->GetData();
+        float *pointer = static_cast<vtkFloatArray*>(data)->GetPointer(0);
+        return reinterpret_cast<Vec3f*>(pointer);
+    }
+
+    template<> Vec3d* vtkpoints_data<double>(vtkSmartPointer<vtkPoints>& points)
+    {
+        CV_Assert(points->GetDataType() == VTK_DOUBLE);
+        vtkDataArray *data = points->GetData();
+        double *pointer = static_cast<vtkDoubleArray*>(data)->GetPointer(0);
+        return reinterpret_cast<Vec3d*>(pointer);
+    }
+}
+
 void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
 {
     if (window_)
@@ -21,14 +42,14 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 {
     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(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());
         
     vtkSmartPointer<vtkPolyData> polydata;
     vtkSmartPointer<vtkCellArray> vertices;
     vtkSmartPointer<vtkPoints> points;
     vtkSmartPointer<vtkIdTypeArray> initcells;
-    vtkIdType nr_points;
+    vtkIdType nr_points = cloud.total();
 
     // If the cloud already exists, update otherwise create new one
     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
@@ -41,7 +62,6 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
         vertices = vtkSmartPointer<vtkCellArray>::New ();
         polydata->SetVerts (vertices);
 
-        nr_points = cloud.total();
         points = polydata->GetPoints ();
 
         if (!points)
@@ -67,29 +87,28 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
             points->SetDataTypeToFloat ();
         else if (cloud.depth() == CV_64F)
             points->SetDataTypeToDouble ();
-        // Copy the new point array in
-        nr_points = cloud.total();
+
         points->SetNumberOfPoints (nr_points);
     }
 
-    int j = 0;
-    
     if (cloud.depth() == CV_32F)
     {
         // Get a pointer to the beginning of the data array
-        Vec3f *data = reinterpret_cast<Vec3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
-        j = copy_non_nans(data, cloud, cloud);
-        transform_non_nans(data,j,pose);
+        Vec3f *data_beg = vtkpoints_data<float>(points);
+        Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud);
+        std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
+        nr_points = data_end - data_beg;
+
     }
     else if (cloud.depth() == CV_64F)
     {
         // Get a pointer to the beginning of the data array
-        Vec3d *data = reinterpret_cast<Vec3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
-        j = copy_non_nans(data, cloud, cloud);
-        transform_non_nans(data,j,pose);
+        Vec3d *data_beg = vtkpoints_data<double>(points);
+        Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud);
+        std::transform(data_beg, data_end, data_beg, ApplyAffine(pose));
+        nr_points = data_end - data_beg;
     }
 
-    nr_points = j;
     points->SetNumberOfPoints (nr_points);
 
     vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
@@ -102,19 +121,17 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
     // Set the cells and the vertices
     vertices->SetCells (nr_points, cells);
 
-    // Get the colors from the handler
-    Vec2d minmax;
-    vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-    scalars->SetNumberOfComponents (3);
-    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
-
     // Get a random color
     Vec3b* colors_data = new Vec3b[nr_points];
-    j = copy_non_nans(colors_data, colors, cloud);
+    NanFilter::copy(colors, colors_data, cloud);
 
-    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0);
+    vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+    scalars->SetNumberOfComponents (3);
+    scalars->SetNumberOfTuples (nr_points);
+    scalars->SetArray (colors_data->val, 3 * nr_points, 0);
 
     // Assign the colors
+    Vec2d minmax;
     polydata->GetPointData ()->SetScalars (scalars);
     scalars->GetRange (minmax.val);
 
@@ -133,8 +150,8 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
         (*cloud_actor_map_)[id].actor = actor;
         (*cloud_actor_map_)[id].cells = initcells;
 
-        const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
-        const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+        const Eigen::Vector4f sensor_origin = Eigen::Vector4f::Zero ();
+        const Eigen::Quaternionf sensor_orientation = Eigen::Quaternionf::Identity ();
 
         // Save the viewpoint transformation matrix to the global actor map
         vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();