diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index eb3b4b719..dd708491e 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -104,4 +104,12 @@ namespace temp_viz CloudWidget(InputArray _cloud, InputArray _colors); CloudWidget(InputArray _cloud, const Color &color = Color::white()); }; + + class CV_EXPORTS CloudNormalsWidget : public Widget + { + public: + CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f); + private: + struct ApplyCloudNormals; + }; } diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index ae3536ff5..700543628 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -524,12 +524,143 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ()); mapper->SetInterpolateScalarsBeforeMapping (interpolation); - mapper->ScalarVisibilityOn (); + mapper->ScalarVisibilityOff (); mapper->ImmediateModeRenderingOff (); actor->SetNumberOfCloudPoints (int (std::max (1, polydata->GetNumberOfPoints () / 10))); actor->GetProperty ()->SetInterpolationToFlat (); actor->GetProperty ()->BackfaceCullingOn (); + actor->GetProperty ()->SetColor(c.val); actor->SetMapper (mapper); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// cloud normals widget implementation + +struct temp_viz::CloudNormalsWidget::ApplyCloudNormals +{ + template + struct Impl + { + static vtkSmartPointer applyOrganized(const cv::Mat &cloud, const cv::Mat& normals, + int level, float scale, _Tp *&pts, vtkIdType &nr_normals) + { + vtkIdType point_step = static_cast (sqrt (double (level))); + nr_normals = (static_cast ((cloud.cols - 1)/ point_step) + 1) * + (static_cast ((cloud.rows - 1) / point_step) + 1); + vtkSmartPointer lines = vtkSmartPointer::New(); + + pts = new _Tp[2 * nr_normals * 3]; + + int cch = cloud.channels(); + vtkIdType cell_count = 0; + for (vtkIdType y = 0; y < cloud.rows; y += point_step) + { + const _Tp *prow = cloud.ptr<_Tp>(y); + const _Tp *nrow = normals.ptr<_Tp>(y); + for (vtkIdType x = 0; x < cloud.cols; x += point_step + cch) + { + pts[2 * cell_count * 3 + 0] = prow[x]; + pts[2 * cell_count * 3 + 1] = prow[x+1]; + pts[2 * cell_count * 3 + 2] = prow[x+2]; + pts[2 * cell_count * 3 + 3] = prow[x] + nrow[x] * scale; + pts[2 * cell_count * 3 + 4] = prow[x+1] + nrow[x+1] * scale; + pts[2 * cell_count * 3 + 5] = prow[x+2] + nrow[x+2] * scale; + + lines->InsertNextCell (2); + lines->InsertCellPoint (2 * cell_count); + lines->InsertCellPoint (2 * cell_count + 1); + cell_count++; + } + } + return lines; + } + + static vtkSmartPointer applyUnorganized(const cv::Mat &cloud, const cv::Mat& normals, + int level, float scale, _Tp *&pts, vtkIdType &nr_normals) + { + vtkSmartPointer lines = vtkSmartPointer::New(); + nr_normals = (cloud.size().area() - 1) / level + 1 ; + pts = new _Tp[2 * nr_normals * 3]; + + int cch = cloud.channels(); + const _Tp *p = cloud.ptr<_Tp>(); + const _Tp *n = normals.ptr<_Tp>(); + for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level * cch) + { + + pts[2 * j * 3 + 0] = p[i]; + pts[2 * j * 3 + 1] = p[i+1]; + pts[2 * j * 3 + 2] = p[i+2]; + pts[2 * j * 3 + 3] = p[i] + n[i] * scale; + pts[2 * j * 3 + 4] = p[i+1] + n[i+1] * scale; + pts[2 * j * 3 + 5] = p[i+2] + n[i+2] * scale; + + lines->InsertNextCell (2); + lines->InsertCellPoint (2 * j); + lines->InsertCellPoint (2 * j + 1); + } + return lines; + } + }; + + template + static inline vtkSmartPointer apply(const cv::Mat &cloud, const cv::Mat& normals, + int level, float scale, _Tp *&pts, vtkIdType &nr_normals) + { + if (cloud.cols > 1 && cloud.rows > 1) + return ApplyCloudNormals::Impl<_Tp>::applyOrganized(cloud, normals, level, scale, pts, nr_normals); + else + return ApplyCloudNormals::Impl<_Tp>::applyUnorganized(cloud, normals, level, scale, pts, nr_normals); + } +}; + +temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale) +{ + Mat cloud = _cloud.getMat(); + Mat normals = _normals.getMat(); + CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); + CV_Assert(cloud.size() == normals.size() && cloud.type() == normals.type()); + + vtkSmartPointer points = vtkSmartPointer::New(); + vtkSmartPointer lines = vtkSmartPointer::New(); + vtkIdType nr_normals = 0; + + if (cloud.depth() == CV_32F) + { + points->SetDataTypeToFloat(); + + vtkSmartPointer data = vtkSmartPointer::New (); + data->SetNumberOfComponents (3); + + float* pts = 0; + lines = ApplyCloudNormals::apply(cloud, normals, level, scale, pts, nr_normals); + data->SetArray (&pts[0], 2 * nr_normals * 3, 0); + points->SetData (data); + } + else + { + points->SetDataTypeToDouble(); + + vtkSmartPointer data = vtkSmartPointer::New (); + data->SetNumberOfComponents (3); + + double* pts = 0; + lines = ApplyCloudNormals::apply(cloud, normals, level, scale, pts, nr_normals); + data->SetArray (&pts[0], 2 * nr_normals * 3, 0); + points->SetData (data); + } + + vtkSmartPointer polyData = vtkSmartPointer::New(); + polyData->SetPoints (points); + polyData->SetLines (lines); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput (polyData); + mapper->SetColorModeToMapScalars(); + mapper->SetScalarModeToUsePointData(); + + vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + actor->SetMapper(mapper); } \ No newline at end of file diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 2cbb09072..ad4ceb822 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -52,16 +52,18 @@ cv::Mat cvcloud_load() { - cv::Mat cloud(1, 20000, CV_32FC3); + cv::Mat cloud(1, 20000, CV_64FC4); std::ifstream ifs("cloud_dragon.ply"); std::string str; for(size_t i = 0; i < 11; ++i) std::getline(ifs, str); - cv::Point3f* data = cloud.ptr(); - for(size_t i = 0; i < 20000; ++i) - ifs >> data[i].x >> data[i].y >> data[i].z; + cv::Vec4d* data = cloud.ptr(); + for(size_t i = 0; i < 20000; ++i){ + ifs >> data[i][0] >> data[i][1] >> data[i][2]; + data[i][3] = 1.0; + } return cloud; } @@ -100,7 +102,7 @@ TEST(Viz_viz3d, accuracy) temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity()); temp_viz::TextWidget tw("TEST", cv::Point2i(100,100), 20); temp_viz::CloudWidget pcw(cloud, colors); - temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(255,255,255)); + temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(0,255,255)); // v.showWidget("line", lw); // v.showWidget("plane", pw); @@ -111,12 +113,18 @@ TEST(Viz_viz3d, accuracy) // v.showWidget("cube", cuw); v.showWidget("coordinateSystem", csw); // v.showWidget("text",tw); - v.showWidget("pcw",pcw); +// v.showWidget("pcw",pcw); v.showWidget("pcw2",pcw2); temp_viz::LineWidget lw2 = lw; // v.showPointCloud("cld",cloud, colors); + cv::Mat normals(cloud.size(), cloud.type(), cv::Scalar(0, 10, 0)); + +// v.addPointCloudNormals(cloud, normals, 100, 0.02, "n"); + temp_viz::CloudNormalsWidget cnw(cloud, normals); + v.showWidget("n", cnw); + while(!v.wasStopped()) { // Creating new point cloud with id cloud1 @@ -134,9 +142,10 @@ TEST(Viz_viz3d, accuracy) cw.setPose(cloudPosition); cyw.setPose(cloudPosition); lw.setPose(cloudPosition); - cuw.setPose(cloudPosition); - v.showWidget("pcw",pcw, cloudPosition); - v.showWidget("pcw2",pcw2, cloudPosition2); + cuw.setPose(cloudPosition); +// cnw.setPose(cloudPosition); +// v.showWidget("pcw",pcw, cloudPosition); +// v.showWidget("pcw2",pcw2, cloudPosition2); // v.showWidget("plane", pw, cloudPosition); angle_x += 0.1f; @@ -152,9 +161,7 @@ TEST(Viz_viz3d, accuracy) v.spinOnce(1, true); } -// cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0)); -// -// v.addPointCloudNormals(cloud, normals, 100, 0.02, "n"); + // // // temp_viz::ModelCoefficients mc;