diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 157997c50..5df61eb74 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -202,6 +202,18 @@ namespace cv struct CreateCloudWidget; }; + class CV_EXPORTS CloudCollectionWidget : public Widget3D + { + public: + CloudCollectionWidget(); + + void addCloud(InputArray cloud, InputArray colors, const Affine3f &pose = Affine3f::Identity()); + void addCloud(InputArray cloud, const Color &color = Color::white(), const Affine3f &pose = Affine3f::Identity()); + + private: + struct CreateCloudWidget; + }; + class CV_EXPORTS CloudNormalsWidget : public Widget3D { public: @@ -239,6 +251,7 @@ namespace cv template<> CV_EXPORTS CameraPositionWidget Widget::cast(); template<> CV_EXPORTS TrajectoryWidget Widget::cast(); template<> CV_EXPORTS CloudWidget Widget::cast(); + template<> CV_EXPORTS CloudCollectionWidget Widget::cast(); template<> CV_EXPORTS CloudNormalsWidget Widget::cast(); template<> CV_EXPORTS MeshWidget Widget::cast(); diff --git a/modules/viz/src/cloud_widgets.cpp b/modules/viz/src/cloud_widgets.cpp index 9eec79074..2d980df10 100644 --- a/modules/viz/src/cloud_widgets.cpp +++ b/modules/viz/src/cloud_widgets.cpp @@ -154,7 +154,6 @@ cv::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); - vtkIdType nr_points; vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); @@ -184,6 +183,255 @@ template<> cv::viz::CloudWidget cv::viz::Widget::cast() return static_cast(widget); } +/////////////////////////////////////////////////////////////////////////////////////////////// +/// Cloud Collection Widget implementation + +struct cv::viz::CloudCollectionWidget::CreateCloudWidget +{ + static inline vtkSmartPointer create(const Mat &cloud, vtkIdType &nr_points) + { + vtkSmartPointer polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + + polydata->SetVerts (vertices); + + vtkSmartPointer points = polydata->GetPoints(); + vtkSmartPointer initcells; + nr_points = cloud.total(); + + 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); + return polydata; + } +}; + +cv::viz::CloudCollectionWidget::CloudCollectionWidget() +{ + // Just create the actor + vtkSmartPointer actor = vtkSmartPointer::New(); + WidgetAccessor::setProp(*this, actor); +} + +void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, InputArray _colors, const Affine3f &pose) +{ + 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()); + + if (cloud.isContinuous() && colors.isContinuous()) + { + cloud.reshape(cloud.channels(), 1); + colors.reshape(colors.channels(), 1); + } + + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); + + // Filter colors + Vec3b* colors_data = new Vec3b[nr_points]; + NanFilter::copyColor(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); + + // Transform the poly data based on the pose + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(convertToVtkMatrix(pose.matrix)); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(polydata->GetProducerPort()); + transform_filter->Update(); + + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + vtkDataSetMapper *mapper = vtkDataSetMapper::SafeDownCast(actor->GetMapper()); + if (!mapper) + { + // This is the first cloud + vtkSmartPointer mapper_new = vtkSmartPointer::New (); + mapper_new->SetInputConnection (transform_filter->GetOutputPort()); + + Vec3d minmax(scalars->GetRange()); + mapper_new->SetScalarRange(minmax.val); + mapper_new->SetScalarModeToUsePointData (); + + bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ()); + + mapper_new->SetInterpolateScalarsBeforeMapping(interpolation); + mapper_new->ScalarVisibilityOn(); + mapper_new->ImmediateModeRenderingOff(); + + actor->SetNumberOfCloudPoints (int (std::max(1, polydata->GetNumberOfPoints () / 10))); + actor->GetProperty()->SetInterpolationToFlat(); + actor->GetProperty()->BackfaceCullingOn(); + actor->SetMapper(mapper_new); + return ; + } + + vtkPolyData *data = vtkPolyData::SafeDownCast(mapper->GetInput()); + CV_Assert(data); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + appendFilter->AddInputConnection(mapper->GetInput()->GetProducerPort()); + appendFilter->AddInputConnection(transform_filter->GetOutputPort()); + mapper->SetInputConnection(appendFilter->GetOutputPort()); + + // Update the number of cloud points + vtkIdType old_cloud_points = actor->GetNumberOfCloudPoints(); + actor->SetNumberOfCloudPoints (int (std::max(1, old_cloud_points+polydata->GetNumberOfPoints () / 10))); +} + +void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, const Color &color, const Affine3f &pose) +{ + Mat cloud = _cloud.getMat(); + CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); + + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); + + vtkSmartPointer scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + scalars->SetNumberOfTuples (nr_points); + scalars->FillComponent(0, color[2]); + scalars->FillComponent(1, color[1]); + scalars->FillComponent(2, color[0]); + + // Assign the colors + polydata->GetPointData ()->SetScalars (scalars); + + // Transform the poly data based on the pose + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(convertToVtkMatrix(pose.matrix)); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(polydata->GetProducerPort()); + transform_filter->Update(); + + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + vtkDataSetMapper *mapper = vtkDataSetMapper::SafeDownCast(actor->GetMapper()); + if (!mapper) + { + // This is the first cloud + vtkSmartPointer mapper_new = vtkSmartPointer::New (); + mapper_new->SetInputConnection (transform_filter->GetOutputPort()); + + Vec3d minmax(scalars->GetRange()); + mapper_new->SetScalarRange(minmax.val); + mapper_new->SetScalarModeToUsePointData (); + + bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ()); + + mapper_new->SetInterpolateScalarsBeforeMapping(interpolation); + mapper_new->ScalarVisibilityOn(); + mapper_new->ImmediateModeRenderingOff(); + + actor->SetNumberOfCloudPoints (int (std::max(1, polydata->GetNumberOfPoints () / 10))); + actor->GetProperty()->SetInterpolationToFlat(); + actor->GetProperty()->BackfaceCullingOn(); + actor->SetMapper(mapper_new); + return ; + } + + vtkPolyData *data = vtkPolyData::SafeDownCast(mapper->GetInput()); + CV_Assert(data); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + appendFilter->AddInputConnection(mapper->GetInput()->GetProducerPort()); + appendFilter->AddInputConnection(transform_filter->GetOutputPort()); + mapper->SetInputConnection(appendFilter->GetOutputPort()); + + // Update the number of cloud points + vtkIdType old_cloud_points = actor->GetNumberOfCloudPoints(); + actor->SetNumberOfCloudPoints (int (std::max(1, old_cloud_points+polydata->GetNumberOfPoints () / 10))); +} + +template<> cv::viz::CloudCollectionWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} + /////////////////////////////////////////////////////////////////////////////////////////////// /// Cloud Normals Widget implementation diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp index aec8ca4f2..569d934ce 100644 --- a/modules/viz/src/shape_widgets.cpp +++ b/modules/viz/src/shape_widgets.cpp @@ -1204,10 +1204,10 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, i // Set the color for polyData vtkSmartPointer colors = vtkSmartPointer::New(); colors->SetNumberOfComponents(3); - - // TODO Make this more efficient - for (int i = 0; i < nr_points; ++i) - colors->InsertNextTuple3(color[2], color[1], color[0]); + colors->SetNumberOfTuples(nr_points); + colors->FillComponent(0, color[2]); + colors->FillComponent(1, color[1]); + colors->FillComponent(2, color[0]); polyData->GetPointData()->SetScalars(colors); appendFilter->AddInputConnection(polyData->GetProducerPort());