From d394e233bb181fd0bbf6bce603fb1cc9cd7ce92a Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 11:34:11 +0200 Subject: [PATCH 1/6] cube widget wireframe --- modules/viz/include/opencv2/viz/widgets.hpp | 2 +- modules/viz/src/simple_widgets.cpp | 5 ++++- modules/viz/test/test_viz3d.cpp | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index eaf9e62865..3198a07ed5 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -81,7 +81,7 @@ namespace temp_viz class CV_EXPORTS CubeWidget : public Widget { public: - CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color = Color::white()); + CubeWidget(const Point3f& pt_min, const Point3f& pt_max, bool wire_frame = true, const Color &color = Color::white()); }; class CV_EXPORTS CoordinateSystemWidget : public Widget diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 7ae8225151..8cc4867d82 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -211,7 +211,7 @@ temp_viz::CylinderWidget::CylinderWidget(const Point3f& pt_on_axis, const Point3 /////////////////////////////////////////////////////////////////////////////////////////////// /// cylinder widget implementation -temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color) +temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, bool wire_frame, const Color &color) { vtkSmartPointer cube = vtkSmartPointer::New (); cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); @@ -222,6 +222,9 @@ temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, c vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); + if (wire_frame) + actor->GetProperty ()->SetRepresentationToWireframe (); + setColor(color); } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 4c586524fb..0ec36770ac 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -96,7 +96,7 @@ TEST(Viz_viz3d, accuracy) temp_viz::ArrowWidget aw(cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0)); temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 0.5, 0.01, temp_viz::Color(0,255,0)); temp_viz::CylinderWidget cyw(cv::Point3f(0,0,0), cv::Point3f(-1,-1,-1), 0.5, 30, temp_viz::Color(0,255,0)); - temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1), temp_viz::Color(0,0,255)); + temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1)); temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity()); temp_viz::TextWidget tw("TEST", cv::Point2i(100,100), 20); @@ -106,7 +106,7 @@ TEST(Viz_viz3d, accuracy) // v.showWidget("arrow", aw); // v.showWidget("circle", cw); // v.showWidget("cylinder", cyw); -// v.showWidget("cube", cuw); + v.showWidget("cube", cuw); v.showWidget("coordinateSystem", csw); v.showWidget("text",tw); From 6e5ae98582e9aedf1cc1ba3e7f89ccee0d57e21b Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 15:12:50 +0200 Subject: [PATCH 2/6] initial cloud widget implementation, fix safedowncasts --- modules/viz/include/opencv2/viz/widgets.hpp | 7 + modules/viz/src/simple_widgets.cpp | 265 +++++++++++++++++++- modules/viz/src/viz3d_impl.cpp | 2 +- modules/viz/src/widget.cpp | 8 +- modules/viz/test/test_viz3d.cpp | 19 +- 5 files changed, 278 insertions(+), 23 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 3198a07ed5..eb3b4b719b 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -97,4 +97,11 @@ namespace temp_viz // TODO Overload setColor method, and hide setPose, updatePose, getPose methods }; + + class CV_EXPORTS CloudWidget : public Widget + { + public: + CloudWidget(InputArray _cloud, InputArray _colors); + CloudWidget(InputArray _cloud, const Color &color = Color::white()); + }; } diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 8cc4867d82..ae3536ff5f 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -1,5 +1,10 @@ #include "precomp.hpp" +namespace temp_viz +{ + template Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer& points); +} + /////////////////////////////////////////////////////////////////////////////////////////////// /// line widget implementation temp_viz::LineWidget::LineWidget(const Point3f &pt1, const Point3f &pt2, const Color &color) @@ -12,7 +17,7 @@ temp_viz::LineWidget::LineWidget(const Point3f &pt1, const Point3f &pt2, const C vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(line->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); setColor(color); @@ -20,13 +25,13 @@ temp_viz::LineWidget::LineWidget(const Point3f &pt1, const Point3f &pt2, const C void temp_viz::LineWidget::setLineWidth(float line_width) { - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->GetProperty()->SetLineWidth(line_width); } float temp_viz::LineWidget::getLineWidth() { - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); return actor->GetProperty()->GetLineWidth(); } @@ -43,7 +48,7 @@ temp_viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, double size, const Color vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(plane->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); actor->SetScale(size); @@ -64,7 +69,7 @@ temp_viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, const Point3f& pt, double vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(plane->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); actor->SetScale(size); @@ -87,7 +92,7 @@ temp_viz::SphereWidget::SphereWidget(const cv::Point3f ¢er, float radius, in vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(sphere->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); setColor(color); @@ -149,7 +154,7 @@ temp_viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& pt2, const vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(transformPD->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); setColor(color); @@ -178,7 +183,7 @@ temp_viz::CircleWidget::CircleWidget(const temp_viz::Point3f& pt, double radius, vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(tf->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); setColor(color); @@ -202,7 +207,7 @@ temp_viz::CylinderWidget::CylinderWidget(const Point3f& pt_on_axis, const Point3 vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(tuber->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); setColor(color); @@ -219,7 +224,7 @@ temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, b vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput(cube->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); if (wire_frame) @@ -259,7 +264,7 @@ temp_viz::CoordinateSystemWidget::CoordinateSystemWidget(double scale, const Aff mapper->SetScalarModeToUsePointData (); mapper->SetInput(axes_tubes->GetOutput ()); - vtkSmartPointer actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); cv::Vec3d t = affine.translation(); @@ -277,9 +282,12 @@ temp_viz::CoordinateSystemWidget::CoordinateSystemWidget(double scale, const Aff actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]); } +/////////////////////////////////////////////////////////////////////////////////////////////// +/// text widget implementation + temp_viz::TextWidget::TextWidget(const String &text, const Point2i &pos, int font_size, const Color &color) : Widget(true) { - vtkSmartPointer actor = vtkTextActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkTextActor *actor = vtkTextActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetPosition (pos.x, pos.y); actor->SetInput (text.c_str ()); @@ -292,3 +300,236 @@ temp_viz::TextWidget::TextWidget(const String &text, const Point2i &pos, int fon Color c = vtkcolor(color); tprop->SetColor (c.val); } + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// point cloud widget implementation + +temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors) +{ + 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(colors.type() == CV_8UC3 && cloud.size() == colors.size()); + + vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkSmartPointer polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + + polydata->SetVerts (vertices); + + vtkSmartPointer points = polydata->GetPoints(); + vtkSmartPointer initcells; + vtkIdType nr_points = cloud.total(); + + points = polydata->GetPoints (); + + if (!points) + { + points = vtkSmartPointer::New (); + if (cloud.depth() == CV_32F) + points->SetDataTypeToFloat(); + else if (cloud.depth() == CV_64F) + points->SetDataTypeToDouble(); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); + + if (cloud.depth() == CV_32F) + { + // Get a pointer to the beginning of the data array + Vec3f *data_beg = vtkpoints_data(points); + Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + + } + else if (cloud.depth() == CV_64F) + { + // Get a pointer to the beginning of the data array + Vec3d *data_beg = vtkpoints_data(points); + Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } + points->SetNumberOfPoints (nr_points); + + // Update cells + vtkSmartPointer cells = vertices->GetData (); + // If no init cells and cells has not been initialized... + if (!cells) + cells = vtkSmartPointer::New (); + + // If we have less values then we need to recreate the array + if (cells->GetNumberOfTuples () < nr_points) + { + cells = vtkSmartPointer::New (); + + // If init cells is given, and there's enough data in it, use it + if (initcells && initcells->GetNumberOfTuples () >= nr_points) + { + cells->DeepCopy (initcells); + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + else + { + // If the number of tuples is still too small, we need to recreate the array + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + vtkIdType *cell = cells->GetPointer (0); + // Fill it with 1s + std::fill_n (cell, nr_points * 2, 1); + cell++; + for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) + *cell = i; + // Save the results in initcells + initcells = vtkSmartPointer::New (); + initcells->DeepCopy (cells); + } + } + else + { + // The assumption here is that the current set of cells has more data than needed + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + + // Set the cells and the vertices + vertices->SetCells (nr_points, cells); + + Vec3b* colors_data = new Vec3b[nr_points]; + NanFilter::copy(colors, colors_data, cloud); + + vtkSmartPointer scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + scalars->SetNumberOfTuples (nr_points); + scalars->SetArray (colors_data->val, 3 * nr_points, 0); + + // Assign the colors + polydata->GetPointData ()->SetScalars (scalars); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput (polydata); + + cv::Vec3d minmax(scalars->GetRange()); + mapper->SetScalarRange(minmax.val); + mapper->SetScalarModeToUsePointData (); + + bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ()); + + mapper->SetInterpolateScalarsBeforeMapping (interpolation); + mapper->ScalarVisibilityOn (); + + mapper->ImmediateModeRenderingOff (); + + actor->SetNumberOfCloudPoints (int (std::max (1, polydata->GetNumberOfPoints () / 10))); + actor->GetProperty ()->SetInterpolationToFlat (); + actor->GetProperty ()->BackfaceCullingOn (); + actor->SetMapper (mapper); +} + +temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) +{ + Mat cloud = _cloud.getMat(); + CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); + + vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkSmartPointer polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + + polydata->SetVerts (vertices); + + vtkSmartPointer points = polydata->GetPoints(); + vtkSmartPointer initcells; + vtkIdType nr_points = cloud.total(); + + points = polydata->GetPoints (); + + if (!points) + { + points = vtkSmartPointer::New (); + if (cloud.depth() == CV_32F) + points->SetDataTypeToFloat(); + else if (cloud.depth() == CV_64F) + points->SetDataTypeToDouble(); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); + + if (cloud.depth() == CV_32F) + { + // Get a pointer to the beginning of the data array + Vec3f *data_beg = vtkpoints_data(points); + Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + + } + else if (cloud.depth() == CV_64F) + { + // Get a pointer to the beginning of the data array + Vec3d *data_beg = vtkpoints_data(points); + Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } + points->SetNumberOfPoints (nr_points); + + // Update cells + vtkSmartPointer cells = vertices->GetData (); + // If no init cells and cells has not been initialized... + if (!cells) + cells = vtkSmartPointer::New (); + + // If we have less values then we need to recreate the array + if (cells->GetNumberOfTuples () < nr_points) + { + cells = vtkSmartPointer::New (); + + // If init cells is given, and there's enough data in it, use it + if (initcells && initcells->GetNumberOfTuples () >= nr_points) + { + cells->DeepCopy (initcells); + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + else + { + // If the number of tuples is still too small, we need to recreate the array + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + vtkIdType *cell = cells->GetPointer (0); + // Fill it with 1s + std::fill_n (cell, nr_points * 2, 1); + cell++; + for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) + *cell = i; + // Save the results in initcells + initcells = vtkSmartPointer::New (); + initcells->DeepCopy (cells); + } + } + else + { + // The assumption here is that the current set of cells has more data than needed + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + + // Set the cells and the vertices + vertices->SetCells (nr_points, cells); + + Color c = vtkcolor(color); + polydata->GetPointData ()->SetScalars (0); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput (polydata); + + bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ()); + + mapper->SetInterpolateScalarsBeforeMapping (interpolation); + mapper->ScalarVisibilityOn (); + + mapper->ImmediateModeRenderingOff (); + + actor->SetNumberOfCloudPoints (int (std::max (1, polydata->GetNumberOfPoints () / 10))); + actor->GetProperty ()->SetInterpolationToFlat (); + actor->GetProperty ()->BackfaceCullingOn (); + actor->SetMapper (mapper); +} \ No newline at end of file diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 0eb0bd082c..bf3f4f95bd 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -874,7 +874,7 @@ void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget removeActorFromRenderer(wam_itr->second.actor); } // Get the actor and set the user matrix - vtkSmartPointer actor; + vtkLODActor *actor; if (actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(widget))) { vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); diff --git a/modules/viz/src/widget.cpp b/modules/viz/src/widget.cpp index e51f86770b..f75827f8ff 100644 --- a/modules/viz/src/widget.cpp +++ b/modules/viz/src/widget.cpp @@ -18,7 +18,7 @@ public: void setColor(const Color& color) { - vtkSmartPointer lod_actor = vtkLODActor::SafeDownCast(actor); + vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor); Color c = vtkcolor(color); lod_actor->GetMapper ()->ScalarVisibilityOff (); lod_actor->GetProperty ()->SetColor (c.val); @@ -32,7 +32,7 @@ public: void setPose(const Affine3f& pose) { - vtkSmartPointer lod_actor = vtkLODActor::SafeDownCast(actor); + vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor); vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); lod_actor->SetUserMatrix (matrix); lod_actor->Modified (); @@ -40,7 +40,7 @@ public: void updatePose(const Affine3f& pose) { - vtkSmartPointer lod_actor = vtkLODActor::SafeDownCast(actor); + vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor); vtkSmartPointer matrix = lod_actor->GetUserMatrix(); if (!matrix) { @@ -58,7 +58,7 @@ public: Affine3f getPose() const { - vtkSmartPointer lod_actor = vtkLODActor::SafeDownCast(actor); + vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor); vtkSmartPointer matrix = lod_actor->GetUserMatrix(); Matx44f matrix_cv = convertToMatx(matrix); return Affine3f(matrix_cv); diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 0ec36770ac..2cbb090729 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -53,7 +53,7 @@ cv::Mat cvcloud_load() { cv::Mat cloud(1, 20000, CV_32FC3); - std::ifstream ifs("d:/cloud_dragon.ply"); + std::ifstream ifs("cloud_dragon.ply"); std::string str; for(size_t i = 0; i < 11; ++i) @@ -99,23 +99,29 @@ TEST(Viz_viz3d, accuracy) temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1)); 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)); // v.showWidget("line", lw); - v.showWidget("plane", pw); +// v.showWidget("plane", pw); // v.showWidget("sphere", sw); // v.showWidget("arrow", aw); // v.showWidget("circle", cw); // v.showWidget("cylinder", cyw); - v.showWidget("cube", cuw); +// v.showWidget("cube", cuw); v.showWidget("coordinateSystem", csw); - v.showWidget("text",tw); +// v.showWidget("text",tw); + v.showWidget("pcw",pcw); + v.showWidget("pcw2",pcw2); temp_viz::LineWidget lw2 = lw; +// v.showPointCloud("cld",cloud, colors); while(!v.wasStopped()) { // Creating new point cloud with id cloud1 cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z)); + cv::Affine3f cloudPosition2(angle_x, angle_y, angle_z, cv::Vec3f(pos_x+0.2, pos_y+0.2, pos_z+0.2)); lw2.setColor(temp_viz::Color(col_blue, col_green, col_red)); lw.setLineWidth(lw.getLineWidth()+pos_x * 10); @@ -129,8 +135,9 @@ TEST(Viz_viz3d, accuracy) cyw.setPose(cloudPosition); lw.setPose(cloudPosition); cuw.setPose(cloudPosition); - - v.showWidget("plane", pw, cloudPosition); + v.showWidget("pcw",pcw, cloudPosition); + v.showWidget("pcw2",pcw2, cloudPosition2); +// v.showWidget("plane", pw, cloudPosition); angle_x += 0.1f; angle_y -= 0.1f; From e76023be965b6f5a970b779cbaf1092a6a8a943f Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 18:22:51 +0200 Subject: [PATCH 3/6] cloud normals widget implementation --- modules/viz/include/opencv2/viz/widgets.hpp | 8 ++ modules/viz/src/simple_widgets.cpp | 133 +++++++++++++++++++- modules/viz/test/test_viz3d.cpp | 31 +++-- 3 files changed, 159 insertions(+), 13 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index eb3b4b719b..dd708491ec 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 ae3536ff5f..700543628d 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 2cbb090729..ad4ceb822d 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; From 658e4c5e971d78af560361d6094128e1e7ff02e9 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 18:56:13 +0200 Subject: [PATCH 4/6] set/get/updateWidgetPose implemented, cloudNormals with color --- modules/viz/include/opencv2/viz/viz3d.hpp | 4 ++ modules/viz/include/opencv2/viz/widgets.hpp | 2 +- modules/viz/src/q/viz3d_impl.hpp | 4 ++ modules/viz/src/simple_widgets.cpp | 3 +- modules/viz/src/viz3d.cpp | 15 +++++ modules/viz/src/viz3d_impl.cpp | 66 +++++++++++++++++++++ modules/viz/test/test_viz3d.cpp | 17 +++--- 7 files changed, 102 insertions(+), 9 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 2810d2ab1f..65065bc7f9 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -48,6 +48,10 @@ namespace temp_viz void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity()); bool removeWidget(const String &id); + + bool setWidgetPose(const String &id, const Affine3f &pose); + bool updateWidgetPose(const String &id, const Affine3f &pose); + Affine3f getWidgetPose(const String &id) const; private: Viz3d(const Viz3d&); Viz3d& operator=(const Viz3d&); diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index dd708491ec..ecd2391414 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -108,7 +108,7 @@ namespace temp_viz class CV_EXPORTS CloudNormalsWidget : public Widget { public: - CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f); + CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f, const Color &color = Color::white()); private: struct ApplyCloudNormals; }; diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 89d8f6b23c..6595416bbb 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -202,6 +202,10 @@ public: void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity()); bool removeWidget(const String &id); + bool setWidgetPose(const String &id, const Affine3f &pose); + bool updateWidgetPose(const String &id, const Affine3f &pose); + Affine3f getWidgetPose(const String &id) const; + void all_data(); private: diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 700543628d..6c6cba83f0 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -616,7 +616,7 @@ struct temp_viz::CloudNormalsWidget::ApplyCloudNormals } }; -temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale) +temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale, const Color &color) { Mat cloud = _cloud.getMat(); Mat normals = _normals.getMat(); @@ -663,4 +663,5 @@ temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _ vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); + setColor(color); } \ No newline at end of file diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index bed07a707a..ed9405e913 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -88,3 +88,18 @@ bool temp_viz::Viz3d::removeWidget(const String &id) { return impl_->removeWidget(id); } + +bool temp_viz::Viz3d::setWidgetPose(const String &id, const Affine3f &pose) +{ + return impl_->setWidgetPose(id, pose); +} + +bool temp_viz::Viz3d::updateWidgetPose(const String &id, const Affine3f &pose) +{ + return impl_->updateWidgetPose(id, pose); +} + +temp_viz::Affine3f temp_viz::Viz3d::getWidgetPose(const String &id) const +{ + return impl_->getWidgetPose(id); +} diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index bf3f4f95bd..c77fda74fb 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -899,3 +899,69 @@ bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id) widget_actor_map_->erase(wam_itr); return true; } + +bool temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose) +{ + WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); + bool exists = wam_itr != widget_actor_map_->end(); + if (!exists) + { + return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); + actor->SetUserMatrix (matrix); + actor->Modified (); + return true; + } + return false; +} + +bool temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose) +{ + WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); + bool exists = wam_itr != widget_actor_map_->end(); + if (!exists) + { + return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = actor->GetUserMatrix(); + if (!matrix) + { + setWidgetPose(id, pose); + return true; + } + Matx44f matrix_cv = convertToMatx(matrix); + + Affine3f updated_pose = pose * Affine3f(matrix_cv); + matrix = convertToVtkMatrix(updated_pose.matrix); + + actor->SetUserMatrix (matrix); + actor->Modified (); + return true; + } + return false; +} + +temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) const +{ + WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id); + bool exists = wam_itr != widget_actor_map_->end(); + if (!exists) + { + return Affine3f(); + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = actor->GetUserMatrix(); + Matx44f matrix_cv = convertToMatx(matrix); + return Affine3f(matrix_cv); + } + return Affine3f(); +} \ No newline at end of file diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index ad4ceb822d..76993d160d 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -52,18 +52,16 @@ cv::Mat cvcloud_load() { - cv::Mat cloud(1, 20000, CV_64FC4); - std::ifstream ifs("cloud_dragon.ply"); + cv::Mat cloud(1, 20000, CV_32FC3); + std::ifstream ifs("d:/cloud_dragon.ply"); std::string str; for(size_t i = 0; i < 11; ++i) std::getline(ifs, str); - 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; - } + cv::Point3f* data = cloud.ptr(); + for(size_t i = 0; i < 20000; ++i) + ifs >> data[i].x >> data[i].y >> data[i].z; return cloud; } @@ -148,6 +146,11 @@ TEST(Viz_viz3d, accuracy) // v.showWidget("pcw2",pcw2, cloudPosition2); // v.showWidget("plane", pw, cloudPosition); + v.setWidgetPose("n",cloudPosition); + v.setWidgetPose("pcw2", cloudPosition); + cnw.setColor(temp_viz::Color(col_blue, col_green, col_red)); + pcw2.setColor(temp_viz::Color(col_blue, col_green, col_red)); + angle_x += 0.1f; angle_y -= 0.1f; angle_z += 0.1f; From cc08149d7cf3079fa62b88dede882cf6fb2e97ac Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 19:05:35 +0200 Subject: [PATCH 5/6] remove redundant statements --- modules/viz/src/simple_widgets.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 6c6cba83f0..76c72e6c31 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -515,9 +515,6 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) // Set the cells and the vertices vertices->SetCells (nr_points, cells); - Color c = vtkcolor(color); - polydata->GetPointData ()->SetScalars (0); - vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput (polydata); @@ -531,8 +528,9 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) actor->SetNumberOfCloudPoints (int (std::max (1, polydata->GetNumberOfPoints () / 10))); actor->GetProperty ()->SetInterpolationToFlat (); actor->GetProperty ()->BackfaceCullingOn (); - actor->GetProperty ()->SetColor(c.val); actor->SetMapper (mapper); + + setColor(color); } /////////////////////////////////////////////////////////////////////////////////////////////// From b50d777985ac682dc1f31db27450d56b09cd0071 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 21:48:19 +0200 Subject: [PATCH 6/6] cloudwidget private implementation for duplicate code, add cv_assert(exists), fix bug point_step bug in CloudNormalsWidget --- modules/viz/include/opencv2/viz/widgets.hpp | 2 + modules/viz/src/simple_widgets.cpp | 250 +++++++------------- modules/viz/src/viz3d_impl.cpp | 22 +- 3 files changed, 99 insertions(+), 175 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index ecd2391414..b5b9b65cd9 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -103,6 +103,8 @@ namespace temp_viz public: CloudWidget(InputArray _cloud, InputArray _colors); CloudWidget(InputArray _cloud, const Color &color = Color::white()); + private: + struct CreateCloudWidget; }; class CV_EXPORTS CloudNormalsWidget : public Widget diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 76c72e6c31..640e85c98f 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -304,97 +304,107 @@ temp_viz::TextWidget::TextWidget(const String &text, const Point2i &pos, int fon /////////////////////////////////////////////////////////////////////////////////////////////// /// point cloud widget implementation -temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors) +struct temp_viz::CloudWidget::CreateCloudWidget { - 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(colors.type() == CV_8UC3 && cloud.size() == colors.size()); - - vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); - vtkSmartPointer polydata = vtkSmartPointer::New (); - vtkSmartPointer vertices = vtkSmartPointer::New (); - - polydata->SetVerts (vertices); - - vtkSmartPointer points = polydata->GetPoints(); - vtkSmartPointer initcells; - vtkIdType nr_points = cloud.total(); - - points = polydata->GetPoints (); - - if (!points) + static inline vtkSmartPointer create(const Mat &cloud, vtkIdType &nr_points) { - points = vtkSmartPointer::New (); + vtkSmartPointer polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + + polydata->SetVerts (vertices); + + vtkSmartPointer points = polydata->GetPoints(); + vtkSmartPointer initcells; + nr_points = cloud.total(); + + points = polydata->GetPoints (); + + if (!points) + { + points = vtkSmartPointer::New (); + if (cloud.depth() == CV_32F) + points->SetDataTypeToFloat(); + else if (cloud.depth() == CV_64F) + points->SetDataTypeToDouble(); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); + if (cloud.depth() == CV_32F) - points->SetDataTypeToFloat(); + { + // Get a pointer to the beginning of the data array + Vec3f *data_beg = vtkpoints_data(points); + Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } else if (cloud.depth() == CV_64F) - points->SetDataTypeToDouble(); - polydata->SetPoints (points); - } - points->SetNumberOfPoints (nr_points); - - if (cloud.depth() == CV_32F) - { - // Get a pointer to the beginning of the data array - Vec3f *data_beg = vtkpoints_data(points); - Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); - nr_points = data_end - data_beg; - - } - else if (cloud.depth() == CV_64F) - { - // Get a pointer to the beginning of the data array - Vec3d *data_beg = vtkpoints_data(points); - Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); - nr_points = data_end - data_beg; - } - points->SetNumberOfPoints (nr_points); - - // Update cells - vtkSmartPointer cells = vertices->GetData (); - // If no init cells and cells has not been initialized... - if (!cells) - cells = vtkSmartPointer::New (); - - // If we have less values then we need to recreate the array - if (cells->GetNumberOfTuples () < nr_points) - { - cells = vtkSmartPointer::New (); - - // If init cells is given, and there's enough data in it, use it - if (initcells && initcells->GetNumberOfTuples () >= nr_points) { - cells->DeepCopy (initcells); - cells->SetNumberOfComponents (2); - cells->SetNumberOfTuples (nr_points); + // Get a pointer to the beginning of the data array + Vec3d *data_beg = vtkpoints_data(points); + Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } + points->SetNumberOfPoints (nr_points); + + // Update cells + vtkSmartPointer cells = vertices->GetData (); + // If no init cells and cells has not been initialized... + if (!cells) + cells = vtkSmartPointer::New (); + + // If we have less values then we need to recreate the array + if (cells->GetNumberOfTuples () < nr_points) + { + cells = vtkSmartPointer::New (); + + // If init cells is given, and there's enough data in it, use it + if (initcells && initcells->GetNumberOfTuples () >= nr_points) + { + cells->DeepCopy (initcells); + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + else + { + // If the number of tuples is still too small, we need to recreate the array + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + vtkIdType *cell = cells->GetPointer (0); + // Fill it with 1s + std::fill_n (cell, nr_points * 2, 1); + cell++; + for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) + *cell = i; + // Save the results in initcells + initcells = vtkSmartPointer::New (); + initcells->DeepCopy (cells); + } } else { - // If the number of tuples is still too small, we need to recreate the array + // The assumption here is that the current set of cells has more data than needed cells->SetNumberOfComponents (2); cells->SetNumberOfTuples (nr_points); - vtkIdType *cell = cells->GetPointer (0); - // Fill it with 1s - std::fill_n (cell, nr_points * 2, 1); - cell++; - for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) - *cell = i; - // Save the results in initcells - initcells = vtkSmartPointer::New (); - initcells->DeepCopy (cells); } + + // Set the cells and the vertices + vertices->SetCells (nr_points, cells); + return polydata; } - else - { - // The assumption here is that the current set of cells has more data than needed - cells->SetNumberOfComponents (2); - cells->SetNumberOfTuples (nr_points); - } +}; + +temp_viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors) +{ + 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(colors.type() == CV_8UC3 && cloud.size() == colors.size()); - // Set the cells and the vertices - vertices->SetCells (nr_points, cells); + vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); + // Filter colors Vec3b* colors_data = new Vec3b[nr_points]; NanFilter::copy(colors, colors_data, cloud); @@ -432,88 +442,8 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); - vtkSmartPointer polydata = vtkSmartPointer::New (); - vtkSmartPointer vertices = vtkSmartPointer::New (); - - polydata->SetVerts (vertices); - - vtkSmartPointer points = polydata->GetPoints(); - vtkSmartPointer initcells; - vtkIdType nr_points = cloud.total(); - - points = polydata->GetPoints (); - - if (!points) - { - points = vtkSmartPointer::New (); - if (cloud.depth() == CV_32F) - points->SetDataTypeToFloat(); - else if (cloud.depth() == CV_64F) - points->SetDataTypeToDouble(); - polydata->SetPoints (points); - } - points->SetNumberOfPoints (nr_points); - - if (cloud.depth() == CV_32F) - { - // Get a pointer to the beginning of the data array - Vec3f *data_beg = vtkpoints_data(points); - Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); - nr_points = data_end - data_beg; - - } - else if (cloud.depth() == CV_64F) - { - // Get a pointer to the beginning of the data array - Vec3d *data_beg = vtkpoints_data(points); - Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); - nr_points = data_end - data_beg; - } - points->SetNumberOfPoints (nr_points); - - // Update cells - vtkSmartPointer cells = vertices->GetData (); - // If no init cells and cells has not been initialized... - if (!cells) - cells = vtkSmartPointer::New (); - - // If we have less values then we need to recreate the array - if (cells->GetNumberOfTuples () < nr_points) - { - cells = vtkSmartPointer::New (); - - // If init cells is given, and there's enough data in it, use it - if (initcells && initcells->GetNumberOfTuples () >= nr_points) - { - cells->DeepCopy (initcells); - cells->SetNumberOfComponents (2); - cells->SetNumberOfTuples (nr_points); - } - else - { - // If the number of tuples is still too small, we need to recreate the array - cells->SetNumberOfComponents (2); - cells->SetNumberOfTuples (nr_points); - vtkIdType *cell = cells->GetPointer (0); - // Fill it with 1s - std::fill_n (cell, nr_points * 2, 1); - cell++; - for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) - *cell = i; - // Save the results in initcells - initcells = vtkSmartPointer::New (); - initcells->DeepCopy (cells); - } - } - else - { - // The assumption here is that the current set of cells has more data than needed - cells->SetNumberOfComponents (2); - cells->SetNumberOfTuples (nr_points); - } - - // Set the cells and the vertices - vertices->SetCells (nr_points, cells); + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); vtkSmartPointer mapper = vtkSmartPointer::New (); mapper->SetInput (polydata); @@ -557,7 +487,7 @@ struct temp_viz::CloudNormalsWidget::ApplyCloudNormals { 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) + 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]; diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index c77fda74fb..11982b5740 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -889,9 +889,7 @@ bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id) { WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); bool exists = wam_itr != widget_actor_map_->end(); - - if (!exists) - return std::cout << "[removeWidget] A widget with id <" << id << "> does not exist!" << std::endl, false; + CV_Assert(exists); if (!removeActorFromRenderer (wam_itr->second.actor)) return false; @@ -904,10 +902,8 @@ bool temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &p { WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); bool exists = wam_itr != widget_actor_map_->end(); - if (!exists) - { - return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; - } + CV_Assert(exists); + vtkLODActor *actor; if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) { @@ -923,10 +919,8 @@ bool temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f { WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); bool exists = wam_itr != widget_actor_map_->end(); - if (!exists) - { - return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; - } + CV_Assert(exists); + vtkLODActor *actor; if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) { @@ -952,10 +946,8 @@ temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) con { WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id); bool exists = wam_itr != widget_actor_map_->end(); - if (!exists) - { - return Affine3f(); - } + CV_Assert(exists); + vtkLODActor *actor; if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) {