From 1b51ee385bff79bba6943cdbec6bd35704cbe6c9 Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Tue, 11 Jun 2013 21:30:19 +0400 Subject: [PATCH] refactored showPointCloud to for shorter code. Implemented NanFilter::copy() function --- modules/viz/include/opencv2/viz/types.hpp | 9 +- modules/viz/src/q/viz3d_impl.hpp | 111 +++++++++------------- modules/viz/src/types.cpp | 9 -- modules/viz/src/viz3d.cpp | 1 + modules/viz/src/viz3d_impl.cpp | 65 ++++++++----- 5 files changed, 95 insertions(+), 100 deletions(-) diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index aa18d18cf9..2328dc073c 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/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 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) { diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index be663b75ad..616da8a010 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -447,84 +447,65 @@ 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, 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 + 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 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 + 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 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 Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine * p; } + + template 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; } -} +}; } - - diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp index 138b52e453..0baed3a727 100644 --- a/modules/viz/src/types.cpp +++ b/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 diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 68fcf48708..2047c09804 100644 --- a/modules/viz/src/viz3d.cpp +++ b/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(); } + diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 24bfed1384..1ad0678898 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -2,6 +2,27 @@ #include #include +namespace temp_viz +{ + template Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer& points); + + template<> Vec3f* vtkpoints_data(vtkSmartPointer& points) + { + CV_Assert(points->GetDataType() == VTK_FLOAT); + vtkDataArray *data = points->GetData(); + float *pointer = static_cast(data)->GetPointer(0); + return reinterpret_cast(pointer); + } + + template<> Vec3d* vtkpoints_data(vtkSmartPointer& points) + { + CV_Assert(points->GetDataType() == VTK_DOUBLE); + vtkDataArray *data = points->GetData(); + double *pointer = static_cast(data)->GetPointer(0); + return reinterpret_cast(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 polydata; vtkSmartPointer vertices; vtkSmartPointer points; vtkSmartPointer 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::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((static_cast (points->GetData ()))->GetPointer (0)); - j = copy_non_nans(data, cloud, cloud); - transform_non_nans(data,j,pose); + Vec3f *data_beg = vtkpoints_data(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((static_cast (points->GetData ()))->GetPointer (0)); - j = copy_non_nans(data, cloud, cloud); - transform_non_nans(data,j,pose); + Vec3d *data_beg = vtkpoints_data(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 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 scalars = vtkSmartPointer::New (); - scalars->SetNumberOfComponents (3); - reinterpret_cast(&(*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(&(*scalars))->SetArray (reinterpret_cast(colors_data), 3 * nr_points, 0); + vtkSmartPointer scalars = vtkSmartPointer::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& 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 transformation = vtkSmartPointer::New();