From 01086323b068a399e39e09a0ed5df5bab94b6bd7 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 12 Jun 2013 09:29:04 +0200 Subject: [PATCH 1/4] mistype fix --- modules/viz/src/q/viz3d_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 616da8a01..bf67701cd 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -493,7 +493,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) { From 46cf22cdfff9d0f067a14a35558493b29e8124b6 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 12 Jun 2013 10:55:07 +0200 Subject: [PATCH 2/4] showPointCloud with single color --- modules/viz/include/opencv2/viz/viz3d.hpp | 1 + modules/viz/src/q/viz3d_impl.hpp | 1 + modules/viz/src/viz3d.cpp | 5 +++++ modules/viz/src/viz3d_impl.cpp | 9 ++++++++- 4 files changed, 15 insertions(+), 1 deletion(-) 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 bf67701cd..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"); 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..2ee07ffe7 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); @@ -166,6 +166,13 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou } } +void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, const Color& color, const Affine3f& pose) +{ + // Generate an array of colors from single color + Mat colors(_cloud.size(), CV_8UC3, color); + showPointCloud(id, _cloud, colors, pose); +} + bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level, float scale, const std::string &id) { CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3); From 0e90c0fff26e7b5afc58c56549bf5fb1a36bfcde Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 12 Jun 2013 10:56:46 +0200 Subject: [PATCH 3/4] updated test for single color --- modules/viz/test/test_viz3d.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 3214a8179..5c9141f98 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -89,12 +89,15 @@ 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; 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); + v.showPointCloud("cloud1", cloud, temp_viz::Color(col_blue, col_green, col_red), cloudPosition); angle_x += 0.1f; angle_y -= 0.1f; @@ -102,6 +105,9 @@ 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); } From f91b1a1771980b629210f84c3735dca8658d8383 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Wed, 12 Jun 2013 17:38:39 +0200 Subject: [PATCH 4/4] showPointCloud with single color without duplicating color --- modules/viz/src/viz3d_impl.cpp | 120 +++++++++++++++++++++++++++++++- modules/viz/test/test_viz3d.cpp | 10 ++- 2 files changed, 125 insertions(+), 5 deletions(-) diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 2ee07ffe7..f1c425e93 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -163,14 +163,128 @@ 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) { - // Generate an array of colors from single color - Mat colors(_cloud.size(), CV_8UC3, color); - showPointCloud(id, _cloud, colors, 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 (); + } } bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level, float scale, const std::string &id) diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 5c9141f98..0af79b7cd 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -92,12 +92,18 @@ TEST(Viz_viz3d, accuracy) 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, temp_viz::Color(col_blue, col_green, col_red), 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; @@ -109,7 +115,7 @@ TEST(Viz_viz3d, accuracy) 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));