Merge pull request #6 from ozantonkal/implementing_addPointCloud

Implementing add point cloud

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

 #pragma once
 
 #include <vector>
+#include <boost/concept_check.hpp>
 #include <opencv2/core/cvdef.h>
 #include <opencv2/core.hpp>
 #include <opencv2/core/affine.hpp>
@@ -13,7 +14,9 @@ namespace temp_viz
     typedef std::string String;
 
     typedef cv::Vec3d Vec3d;
+    typedef cv::Vec3f Vec3f;
     typedef cv::Vec4d Vec4d;
+    typedef cv::Vec4f Vec4f;
     typedef cv::Vec2d Vec2d;
     typedef cv::Vec2i Vec2i;
     typedef cv::Vec3b Vec3b;
@@ -84,4 +87,20 @@ 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 )
+    {
+	unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
+	return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff);
+    }
+    
+    inline bool isNaN( double x )
+    {
+	// Here u has two elements
+	unsigned int *u = (reinterpret_cast<unsigned int *>(&x));
+	return (u[1] & 0x7ff00000) == 0x7ff00000 && 
+	(u[0] != 0 || (u[1] & 0x000fffff) != 0);
+    }
 }

modules/viz/src/q/viz3d_impl.hpp

@@ -446,6 +446,88 @@ void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer<vtkMatrix4x4> &vt
 void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
 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, InputArray _s, InputArray _c)
+{
+    Mat s = _s.getMat();
+    Mat c = _c.getMat();
+    CV_Assert(s.size() == c.size());
+    int j = 0;
+    for(int y = 0; y < s.rows; ++y)
+    {
+	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][0]) && !isNaN(crow[x][1]) && !isNaN(crow[x][2]))
+	    {
+		d[j++] = _Tp((srow[x])[0], (srow[x])[1], (srow[x])[2]);
+	    }
+    }
+    return j;
+}
+
+/** \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, InputArray _s, InputArray _c)
+{
+    Mat s = _s.getMat();
+    Mat c = _c.getMat();
+    CV_Assert(s.size() == c.size());
+    
+    int j = 0;
+    if (s.channels() > 3)
+    {
+	if (s.type() == CV_32FC4)
+	{
+	    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())
+	    {
+		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;
+	    }
+	}
+    }
+    else
+    {
+	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;
+	}
+    }
+    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())
+{
+    for (int i = 0; i < length; ++i)
+    {
+	d[i] = pose * d[i];
+    }
+}
+
 }
 
 

modules/viz/src/types.cpp

@@ -22,3 +22,12 @@ 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_impl.cpp

@@ -17,14 +17,13 @@ void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
 void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
 void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
 
-
 void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, InputArray _colors, const Affine3f& pose)
 {
     Mat cloud = _cloud.getMat();
     Mat colors = _colors.getMat();
-    CV_Assert((cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3));
+    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;
@@ -74,35 +73,20 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
     }
 
     int j = 0;
-    if (cloud.type() == CV_32FC3)
+    
+    if (cloud.depth() == CV_32F)
     {
         // Get a pointer to the beginning of the data array
-        Point3f *data = reinterpret_cast<Point3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
-
-        // Scan through the data and apply mask where point is NAN
-        for(int y = 0; y < cloud.rows; ++y)
-        {
-            const Point3f* crow = cloud.ptr<Point3f>(y);
-            for(int x = 0; x < cloud.cols; ++x)
-
-            //TODO implementa templated copy_if() or copy_non_nans() and use everywhere.
-            if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
-                data[j++] =  pose * crow[x];
-         }
+        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);
     }
-    else if (cloud.type() == CV_64FC3)
+    else if (cloud.depth() == CV_64F)
     {
         // Get a pointer to the beginning of the data array
-        Point3d *data = reinterpret_cast<Point3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
-
-        // If a point is NaN, ignore it
-        for(int y = 0; y < cloud.rows; ++y)
-        {
-            const Point3d* crow = cloud.ptr<Point3d>(y);
-            for(int x = 0; x < cloud.cols; ++x)
-            if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
-                data[j++] = pose * crow[x];
-        }
+        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);
     }
 
     nr_points = j;
@@ -126,32 +110,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 
     // Get a random color
     Vec3b* colors_data = new Vec3b[nr_points];
-
-    j = 0;
-    if (cloud.type() == CV_32FC3)
-    {
-        for(int y = 0; y < colors.rows; ++y)
-        {
-            const Vec3b* crow = colors.ptr<Vec3b>(y);
-            const Point3f* cloud_row = cloud.ptr<Point3f>(y);
-
-            for(int x = 0; x < colors.cols; ++x)
-            if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1)
-                colors_data[j++] = crow[x];
-        }
-    }
-    else if (cloud.type() == CV_64FC3)
-    {
-        for(int y = 0; y < colors.rows; ++y)
-        {
-            const Vec3b* crow = colors.ptr<Vec3b>(y);
-            const Point3d* cloud_row = cloud.ptr<Point3d>(y);
-
-            for(int x = 0; x < colors.cols; ++x)
-            if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1)
-                colors_data[j++] = crow[x];
-        }
-    }
+    j = copy_non_nans(colors_data, colors, cloud);
 
     reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(colors_data), 3 * nr_points, 0);