diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index dbc5fc4b8..06de8a46e 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -14,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; @@ -86,18 +88,19 @@ namespace temp_viz template inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } - Point3d operator*(const Affine3f& affine, const Point3d& point); + Vec3d operator*(const Affine3f& affine, const Vec3d& vec); inline bool isNaN( float x ) { - union { float f; unsigned int u; } v = { x }; - return ((v.u & 0x7f800000) == 0x7f800000) && (v.u & 0x007fffff); + unsigned int *u = (reinterpret_cast(&x)); + return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff); } inline bool isNaN( double x ) { - union { double d; unsigned int u[2]; } v = { x }; - return (v.u[1] & 0x7ff00000) == 0x7ff00000 && - (v.u[0] != 0 || (v.u[1] & 0x000fffff) != 0); + // Here u has two elements + unsigned int *u = (reinterpret_cast(&x)); + return (u[1] & 0x7ff00000) == 0x7ff00000 && + (u[0] != 0 || (u[1] & 0x000fffff) != 0); } } diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 169666141..95ead4a7a 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -446,6 +446,88 @@ void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer &vt void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion &orientation, vtkSmartPointer &vtk_matrix); void convertToEigenMatrix (const vtkSmartPointer &vtk_matrix, Eigen::Matrix4f &m); + +template 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 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 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]; + } +} + } diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp index 9586f6bc7..138b52e45 100644 --- a/modules/viz/src/types.cpp +++ b/modules/viz/src/types.cpp @@ -22,12 +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::Point3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Point3d& point) +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::Point3d result; - result.x = m.val[0] * point.x + m.val[1] * point.y + m.val[ 2] * point.z + m.val[ 3]; - result.y = m.val[4] * point.x + m.val[5] * point.y + m.val[ 6] * point.z + m.val[ 7]; - result.z = m.val[8] * point.x + m.val[9] * point.y + m.val[10] * point.z + m.val[11]; + 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 diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 91e4d6cb6..e61549196 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/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 polydata; vtkSmartPointer vertices; vtkSmartPointer 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((static_cast (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(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((static_cast (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((static_cast (points->GetData ()))->GetPointer (0)); - - // If a point is NaN, ignore it - for(int y = 0; y < cloud.rows; ++y) - { - const Point3d* crow = cloud.ptr(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((static_cast (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(y); - const Point3f* cloud_row = cloud.ptr(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(y); - const Point3d* cloud_row = cloud.ptr(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(&(*scalars))->SetArray (reinterpret_cast(colors_data), 3 * nr_points, 0);