From 1ae5918fa88684f1a7ffb479b4bee613d65be7d5 Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Tue, 11 Jun 2013 18:38:20 +0400 Subject: [PATCH] refactoring of copy_non_nans --- modules/viz/include/opencv2/viz/types.hpp | 25 +++--- modules/viz/src/q/viz3d_impl.hpp | 104 +++++++++++----------- modules/viz/src/viz3d_impl.cpp | 8 +- 3 files changed, 69 insertions(+), 68 deletions(-) diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index 06de8a46ed..aa18d18cf9 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include #include @@ -30,6 +29,8 @@ namespace temp_viz typedef cv::Size Size; typedef cv::Point Point; typedef cv::InputArray InputArray; + using cv::Point3_; + using cv::Vec; @@ -90,17 +91,21 @@ namespace temp_viz Vec3d operator*(const Affine3f& affine, const Vec3d& vec); - inline bool isNaN( float x ) + inline bool isNan(float x) { - unsigned int *u = (reinterpret_cast(&x)); - return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff); + unsigned int *u = reinterpret_cast(&x); + return ((u[0] & 0x7f800000) == 0x7f800000) && (u[0] & 0x007fffff); } - inline bool isNaN( double x ) + inline bool isNan(double x) { - // Here u has two elements - unsigned int *u = (reinterpret_cast(&x)); - return (u[1] & 0x7ff00000) == 0x7ff00000 && - (u[0] != 0 || (u[1] & 0x000fffff) != 0); - } + unsigned int *u = reinterpret_cast(&x); + return (u[1] & 0x7ff00000) == 0x7ff00000 && (u[0] != 0 || (u[1] & 0x000fffff) != 0); + } + + template inline bool isNan(const Vec<_Tp, cn>& v) + { return isNan(v.val[0]) || isNan(v.val[1]) || isNan(v.val[2]); } + + template inline bool isNan(const Point3_<_Tp>& p) + { return isNan(p.x) || isNan(p.y) || isNan(p.z); } } diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 95ead4a7a5..be663b75ad 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -24,9 +24,9 @@ public: void setWindowName (const std::string &name); /** \brief Register a callback function for keyboard input - * \param[in] callback function that will be registered as a callback for a keyboard event - * \param[in] cookie for passing user data to callback - */ + * \param[in] callback function that will be registered as a callback for a keyboard event + * \param[in] cookie for passing user data to callback + */ void registerKeyboardCallback(void (*callback)(const cv::KeyboardEvent&, void*), void* cookie = 0); /** \brief Register a callback function for mouse events @@ -96,13 +96,13 @@ public: bool addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = ""); bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud"); - + /** \brief If the id exists, updates the point cloud; otherwise, adds a new point cloud to the scene - * \param[in] id a variable to identify the point cloud - * \param[in] cloud cloud input in x,y,z coordinates - * \param[in] colors color input in the same order of the points or single uniform color - * \param[in] pose transform to be applied on the point cloud - */ + * \param[in] id a variable to identify the point cloud + * \param[in] cloud cloud input in x,y,z coordinates + * \param[in] colors color input in the same order of the points or single uniform color + * \param[in] pose transform to be applied on the point cloud + */ void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity()); bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); @@ -447,70 +447,66 @@ void convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion< void convertToEigenMatrix (const vtkSmartPointer &vtk_matrix, Eigen::Matrix4f &m); -template inline int copy_non_nan_loop(_Tp *d, InputArray _s, InputArray _c) + + +template inline int copy_non_nan_loop(_Tp *d, const Mat& s, const Mat& 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]); - } + 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; } /** \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[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) + */ +template inline int copy_non_nans(_Tp *d, const Mat& s, const Mat& 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; - } - } + 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; - } + 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; } @@ -519,12 +515,12 @@ template inline int copy_non_nans(_Tp *d, InputArray _s, InputArra * \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]; + d[i] = pose * d[i]; } } diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index e61549196a..24bfed1384 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -78,15 +78,15 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou { // Get a pointer to the beginning of the data array Vec3f *data = reinterpret_cast((static_cast (points->GetData ()))->GetPointer (0)); - j = copy_non_nans(data, cloud, cloud); - transform_non_nans(data,j,pose); + j = copy_non_nans(data, cloud, cloud); + transform_non_nans(data,j,pose); } else if (cloud.depth() == CV_64F) { // Get a pointer to the beginning of the data array Vec3d *data = reinterpret_cast((static_cast (points->GetData ()))->GetPointer (0)); - j = copy_non_nans(data, cloud, cloud); - transform_non_nans(data,j,pose); + j = copy_non_nans(data, cloud, cloud); + transform_non_nans(data,j,pose); } nr_points = j;