diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 046b9dc4d..d7e47074d 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -28,6 +28,7 @@ namespace temp_viz void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate"); void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity()); + void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity()); bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 616da8a01..7afa86a7a 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -104,6 +104,7 @@ public: * \param[in] pose transform to be applied on the point cloud */ void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity()); + void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity()); bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); @@ -493,7 +494,7 @@ struct ApplyAffine const Affine3f& affine_; ApplyAffine(const Affine3f& affine) : affine_(affine) {} - template Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine * p; } + template Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine_ * p; } template Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v) { diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 2047c0980..dfade9926 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -28,6 +28,11 @@ void temp_viz::Viz3d::showPointCloud(const String& id, InputArray cloud, InputAr impl_->showPointCloud(id, cloud, colors, pose); } +void temp_viz::Viz3d::showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose) +{ + impl_->showPointCloud(id, cloud, color, pose); +} + bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals, int level, float scale, const String& id) { return impl_->addPointCloudNormals(cloud, normals, level, scale, id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 1ad067889..f1c425e93 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -114,7 +114,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou vtkSmartPointer cells = vertices->GetData (); if (exist) - updateCells(cells, initcells, nr_points); + updateCells (cells, initcells, nr_points); else updateCells (cells, am_it->second.cells, nr_points); @@ -163,6 +163,127 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou { // Update the mapper reinterpret_cast(am_it->second.actor->GetMapper ())->SetInput (polydata); + am_it->second.actor->GetMapper ()->ScalarVisibilityOn(); + am_it->second.actor->Modified (); + } +} + +void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, 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); + + vtkSmartPointer polydata; + vtkSmartPointer vertices; + vtkSmartPointer points; + vtkSmartPointer initcells; + vtkIdType nr_points = cloud.total(); + + // If the cloud already exists, update otherwise create new one + CloudActorMap::iterator am_it = cloud_actor_map_->find (id); + bool exist = (am_it == cloud_actor_map_->end()); + if (exist) + { + // Add as new cloud + allocVtkPolyData(polydata); + //polydata = vtkSmartPointer::New (); + vertices = vtkSmartPointer::New (); + polydata->SetVerts (vertices); + + 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); + } + else + { + // Update the cloud + // Get the current poly data + polydata = reinterpret_cast(am_it->second.actor->GetMapper ())->GetInput (); + vertices = polydata->GetVerts (); + points = polydata->GetPoints (); + // Update the point data type based on the cloud + if (cloud.depth() == CV_32F) + points->SetDataTypeToFloat (); + else if (cloud.depth() == CV_64F) + points->SetDataTypeToDouble (); + + 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); + 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_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; + } + + points->SetNumberOfPoints (nr_points); + + vtkSmartPointer cells = vertices->GetData (); + + if (exist) + updateCells (cells, initcells, nr_points); + else + updateCells (cells, am_it->second.cells, nr_points); + + // Set the cells and the vertices + vertices->SetCells (nr_points, cells); + + // Get a random color + Color c = vtkcolor(color); + polydata->GetPointData ()->SetScalars (0); + + // If this is the new point cloud, a new actor is created + if (exist) + { + vtkSmartPointer actor; + createActorFromVTKDataSet (polydata, actor, false); + + actor->GetProperty ()->SetColor(c.val); + + // Add it to all renderers + renderer_->AddActor (actor); + + // Save the pointer/ID pair to the global actor map + (*cloud_actor_map_)[id].actor = actor; + (*cloud_actor_map_)[id].cells = initcells; + + 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(); + convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); + + (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; + } + else + { + // Update the mapper + reinterpret_cast(am_it->second.actor->GetMapper ())->SetInput (polydata); + am_it->second.actor->GetProperty ()->SetColor(c.val); + am_it->second.actor->GetMapper ()->ScalarVisibilityOff(); + am_it->second.actor->Modified (); } } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 3214a8179..0af79b7cd 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -89,12 +89,21 @@ TEST(Viz_viz3d, accuracy) temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply"); v.addPolygonMesh(*mesh, "pq"); + int col_blue = 0; + int col_green = 0; + int col_red = 0; + + bool alternate = true; 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)); - v.showPointCloud("cloud1", cloud, colors, cloudPosition); + if (!alternate) + v.showPointCloud("cloud1", cloud, temp_viz::Color(255, 0, 0), cloudPosition); + else + v.showPointCloud("cloud1", cloud, colors, cloudPosition); + alternate = !alternate; angle_x += 0.1f; angle_y -= 0.1f; @@ -102,8 +111,11 @@ TEST(Viz_viz3d, accuracy) pos_x = std::sin(angle_x); pos_y = std::sin(angle_x); pos_z = std::sin(angle_x); + col_blue = int(angle_x * 10) % 256; + col_green = int(angle_x * 20) % 256; + col_red = int(angle_x * 30) % 256; - v.spinOnce(1, true); + v.spinOnce(10, true); } // cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));