From 1c4cfd8eaf2dee020303733f23532ed7cabf30d3 Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Tue, 31 Dec 2013 21:03:10 +0400 Subject: [PATCH] normals support for CloudMatSource --- modules/viz/src/vtk/vtkCloudMatSource.cpp | 143 +++++++++++++++------- modules/viz/src/vtk/vtkCloudMatSource.h | 13 +- modules/viz/test/test_viz3d.cpp | 8 ++ 3 files changed, 116 insertions(+), 48 deletions(-) diff --git a/modules/viz/src/vtk/vtkCloudMatSource.cpp b/modules/viz/src/vtk/vtkCloudMatSource.cpp index f533361bca..b603a39b76 100644 --- a/modules/viz/src/vtk/vtkCloudMatSource.cpp +++ b/modules/viz/src/vtk/vtkCloudMatSource.cpp @@ -48,47 +48,86 @@ namespace cv { namespace viz { vtkStandardNewMacro(vtkCloudMatSource); - struct IsNotNan + template bool isNan(const _Tp* data) { - template bool operator()(const _Tp* data) const - { - return !isNan(data[0]) && !isNan(data[1]) && !isNan(data[2]); - } + return isNan(data[0]) || isNan(data[1]) || isNan(data[2]); + } + + template struct VtkDepthTraits; + + template<> struct VtkDepthTraits + { + const static int data_type = VTK_FLOAT; + typedef vtkFloatArray array_type; + }; + + template<> struct VtkDepthTraits + { + const static int data_type = VTK_DOUBLE; + typedef vtkDoubleArray array_type; }; }} cv::viz::vtkCloudMatSource::vtkCloudMatSource() { SetNumberOfInputPorts(0); } cv::viz::vtkCloudMatSource::~vtkCloudMatSource() {} -void cv::viz::vtkCloudMatSource::SetCloud(const Mat& cloud) +int cv::viz::vtkCloudMatSource::SetCloud(const Mat& cloud) { CV_Assert(cloud.depth() == CV_32F || cloud.depth() == CV_64F); CV_Assert(cloud.channels() == 3 || cloud.channels() == 4); - int total = cloud.depth() == CV_32F ? filterNanCopy(cloud, VTK_FLOAT) - : filterNanCopy(cloud, VTK_DOUBLE); + int total = cloud.depth() == CV_32F ? filterNanCopy(cloud) : filterNanCopy(cloud); vertices = vtkSmartPointer::New(); vertices->Allocate(vertices->EstimateSize(1, total)); vertices->InsertNextCell(total); for(int i = 0; i < total; ++i) vertices->InsertCellPoint(i); + + return total; } -void cv::viz::vtkCloudMatSource::SetColorCloud(const Mat &cloud, const Mat &colors) +int cv::viz::vtkCloudMatSource::SetColorCloud(const Mat &cloud, const Mat &colors) { - vtkCloudMatSource::SetCloud(cloud); + int total = SetCloud(cloud); if (colors.empty()) - return; + return total; CV_Assert(colors.depth() == CV_8U && colors.channels() <= 4 && colors.channels() != 2); CV_Assert(colors.size() == cloud.size()); if (cloud.depth() == CV_32F) - filterNanColorsCopy(colors, cloud); + filterNanColorsCopy(colors, cloud, total); else if (cloud.depth() == CV_64F) - filterNanColorsCopy(colors, cloud); + filterNanColorsCopy(colors, cloud, total); + + return total; +} + +int cv::viz::vtkCloudMatSource::SetColorCloudNormals(const Mat &cloud, const Mat &colors, const Mat &normals) +{ + int total = SetColorCloud(cloud, colors); + + if (normals.empty()) + return total; + + CV_Assert(normals.depth() == CV_32F || normals.depth() == CV_64F); + CV_Assert(normals.channels() == 3 || normals.channels() == 4); + CV_Assert(normals.size() == cloud.size()); + + if (normals.depth() == CV_32F && cloud.depth() == CV_32F) + filterNanNormalsCopy(colors, cloud, total); + else if (normals.depth() == CV_32F && cloud.depth() == CV_64F) + filterNanNormalsCopy(colors, cloud, total); + else if (normals.depth() == CV_64F && cloud.depth() == CV_32F) + filterNanNormalsCopy(colors, cloud, total); + else if (normals.depth() == CV_64F && cloud.depth() == CV_64F) + filterNanNormalsCopy(colors, cloud, total); + else + CV_Assert(!"Unsupported normals type"); + + return total; } int cv::viz::vtkCloudMatSource::RequestData(vtkInformation *vtkNotUsed(request), vtkInformationVector **vtkNotUsed(inputVector), vtkInformationVector *outputVector) @@ -100,27 +139,31 @@ int cv::viz::vtkCloudMatSource::RequestData(vtkInformation *vtkNotUsed(request), output->SetVerts(vertices); if (scalars) output->GetPointData()->SetScalars(scalars); + + if (normals) + output->GetPointData()->SetNormals(normals); + return 1; } template -int cv::viz::vtkCloudMatSource::filterNanCopy(const Mat& source, int dataType) +int cv::viz::vtkCloudMatSource::filterNanCopy(const Mat& cloud) { - CV_DbgAssert(DataType<_Tp>::depth == source.depth()); + CV_DbgAssert(DataType<_Tp>::depth == cloud.depth()); points = vtkSmartPointer::New(); - points->SetDataType(dataType); - points->Allocate(source.total()); - points->SetNumberOfPoints(source.total()); + points->SetDataType(VtkDepthTraits<_Tp>::data_type); + points->Allocate(cloud.total()); + points->SetNumberOfPoints(cloud.total()); - int cn = source.channels(); + int cn = cloud.channels(); int total = 0; - for (int y = 0; y < source.rows; ++y) + for (int y = 0; y < cloud.rows; ++y) { - const _Tp* srow = source.ptr<_Tp>(y); - const _Tp* send = srow + source.cols * cn; + const _Tp* srow = cloud.ptr<_Tp>(y); + const _Tp* send = srow + cloud.cols * cn; for (; srow != send; srow += cn) - if (!isNan(srow[0]) && !isNan(srow[1]) && !isNan(srow[2])) + if (!isNan(srow)) points->SetPoint(total++, srow); } points->SetNumberOfPoints(total); @@ -128,41 +171,33 @@ int cv::viz::vtkCloudMatSource::filterNanCopy(const Mat& source, int dataType) return total; } - -template -void cv::viz::vtkCloudMatSource::filterNanColorsCopy(const Mat& colors, const Mat& mask) +template +void cv::viz::vtkCloudMatSource::filterNanColorsCopy(const Mat& cloud_colors, const Mat& mask, int total) { - Mat buffer(colors.size(), CV_8UC3); - Vec3b* pos = buffer.ptr(); + Vec3b* array = new Vec3b[total]; + Vec3b* pos = array; - int s_chs = colors.channels(); + int s_chs = cloud_colors.channels(); int m_chs = mask.channels(); - - _NanPred pred; - - for (int y = 0; y < colors.rows; ++y) + for (int y = 0; y < cloud_colors.rows; ++y) { - const unsigned char* srow = colors.ptr(y); - const unsigned char* send = srow + colors.cols * colors.channels(); - const _Msk* mrow = mask.empty() ? 0 : mask.ptr<_Msk>(y); + const unsigned char* srow = cloud_colors.ptr(y); + const unsigned char* send = srow + cloud_colors.cols * cloud_colors.channels(); + const _Msk* mrow = mask.ptr<_Msk>(y); - if (colors.channels() == 1) + if (cloud_colors.channels() == 1) { for (; srow != send; srow += s_chs, mrow += m_chs) - if (pred(mrow)) + if (!isNan(mrow)) *pos++ = Vec3b(srow[0], srow[0], srow[0]); } else for (; srow != send; srow += s_chs, mrow += m_chs) - if (pred(mrow)) + if (!isNan(mrow)) *pos++ = Vec3b(srow[2], srow[1], srow[0]); } - int total = pos - buffer.ptr(); - Vec3b* array = new Vec3b[total]; - std::copy(buffer.ptr(), pos, array); - scalars = vtkSmartPointer::New(); scalars->SetName("colors"); scalars->SetNumberOfComponents(3); @@ -170,4 +205,26 @@ void cv::viz::vtkCloudMatSource::filterNanColorsCopy(const Mat& colors, const Ma scalars->SetArray(array->val, total * 3, 0); } +template +void cv::viz::vtkCloudMatSource::filterNanNormalsCopy(const Mat& cloud_normals, const Mat& mask, int total) +{ + normals = vtkSmartPointer< VtkDepthTraits<_Tn>::array_type >::New(); + normals->SetNumberOfComponents(3); + normals->SetNumberOfTuples(total); + + int s_chs = cloud_normals.channels(); + int m_chs = mask.channels(); + + int pos = 0; + for (int y = 0; y < cloud_normals.rows; ++y) + { + const _Tn* srow = cloud_normals.ptr<_Tn>(y); + const _Tn* send = srow + cloud_normals.cols * s_chs; + + const _Msk* mrow = mask.ptr<_Msk>(y); + for (; srow != send; srow += s_chs, mrow += m_chs) + if (!isNan(mrow)) + normals->SetTuple(pos++, srow); + } +} diff --git a/modules/viz/src/vtk/vtkCloudMatSource.h b/modules/viz/src/vtk/vtkCloudMatSource.h index 876ab994f6..023e723247 100644 --- a/modules/viz/src/vtk/vtkCloudMatSource.h +++ b/modules/viz/src/vtk/vtkCloudMatSource.h @@ -61,8 +61,9 @@ namespace cv static vtkCloudMatSource *New(); vtkTypeMacro(vtkCloudMatSource,vtkPolyDataAlgorithm); - virtual void SetCloud(const Mat& cloud); - virtual void SetColorCloud(const Mat &cloud, const Mat &colors = cv::Mat()); + virtual int SetCloud(const Mat& cloud); + virtual int SetColorCloud(const Mat &cloud, const Mat &colors = cv::Mat()); + virtual int SetColorCloudNormals(const Mat &cloud, const Mat &colors = cv::Mat(), const Mat &normals = cv::Mat()); protected: vtkCloudMatSource(); @@ -73,14 +74,16 @@ namespace cv vtkSmartPointer points; vtkSmartPointer vertices; vtkSmartPointer scalars; + vtkSmartPointer normals; private: vtkCloudMatSource(const vtkCloudMatSource&); // Not implemented. void operator=(const vtkCloudMatSource&); // Not implemented. - template int filterNanCopy(const Mat& source, int dataType); + template int filterNanCopy(const Mat& cloud); + template void filterNanColorsCopy(const Mat& cloud_colors, const Mat& mask, int total); - template - void filterNanColorsCopy(const Mat& colors, const Mat& mask); + template + void filterNanNormalsCopy(const Mat& cloud_normals, const Mat& mask, int total); }; } } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 9fb291ce1d..ee91023999 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -49,6 +49,14 @@ TEST(Viz_viz3d, develop) cv::Mat cloud = cv::viz::readCloud(String(cvtest::TS::ptr()->get_data_path()) + "dragon.ply"); +// for(size_t i = 0; i < cloud.total(); ++i) +// { +// if (i % 15 == 0) +// continue; +// const static float qnan = std::numeric_limits::quiet_NaN(); +// cloud.at(i) = Vec3f(qnan, qnan, qnan); +// } + cv::viz::Viz3d viz("abc"); viz.showWidget("coo", cv::viz::WCoordinateSystem());