From e58b0b3507601f9c0de7dbf1cab5c4b88d5d84d8 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Fri, 7 Jun 2013 12:14:40 +0200 Subject: [PATCH 1/2] removed cv keywords and redundant lines from showPointCloud --- modules/viz/include/opencv2/viz/types.hpp | 3 +++ modules/viz/include/opencv2/viz/viz3d.hpp | 2 +- modules/viz/src/q/viz3d_impl.hpp | 2 +- modules/viz/src/viz3d.cpp | 2 +- modules/viz/src/viz3d_impl.cpp | 21 +++++++++------------ 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index 6085da2f24..fd504a784b 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -16,14 +16,17 @@ namespace temp_viz typedef cv::Vec4d Vec4d; typedef cv::Vec2d Vec2d; typedef cv::Vec2i Vec2i; + typedef cv::Vec3b Vec3b; typedef cv::Matx33d Matx33d; typedef cv::Affine3f Affine3f; typedef cv::Affine3d Affine3d; typedef cv::Point3f Point3f; + typedef cv::Point3d Point3d; typedef cv::Matx44d Matx44d; typedef cv::Matx44f Matx44f; typedef cv::Size Size; typedef cv::Point Point; + typedef cv::InputArray InputArray; diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 2e89e5f908..3a861eeff7 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -27,7 +27,7 @@ namespace temp_viz void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate"); - void showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose = cv::Affine3f::Identity()); + void showPointCloud(const String& id, InputArray cloud, InputArray colors, 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 17b643c2e0..1696661418 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -103,7 +103,7 @@ public: * \param[in] colors color input in the same order of the points or single uniform color * \param[in] pose transform to be applied on the point cloud */ - void showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose = cv::Affine3f::Identity()); + void showPointCloud(const String& id, InputArray cloud, InputArray colors, 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 4b2e3b19e4..e06c85ca03 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -23,7 +23,7 @@ void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const impl_->addCoordinateSystem(scale, t, id); } -void temp_viz::Viz3d::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) +void temp_viz::Viz3d::showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose) { impl_->showPointCloud(id, cloud, colors, pose); } diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 692e9960da..615f035e87 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -28,10 +28,10 @@ void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); } -void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) +void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose) { - cv::Mat cloudMat = cloud.getMat(); - cv::Mat colorsMat = colors.getMat(); + Mat cloudMat = cloud.getMat(); + Mat colorsMat = colors.getMat(); CV_Assert(cloudMat.type() == CV_32FC3 && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); vtkSmartPointer polydata; @@ -84,13 +84,13 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr // If a point is NaN, ignore it for(int y = 0; y < cloudMat.rows; ++y) { - const cv::Point3f* crow = cloudMat.ptr(y); + const Point3f* crow = cloudMat.ptr(y); for(int x = 0; x < cloudMat.cols; ++x) if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) { // Points are transformed based on pose parameter - cv::Point3f transformed_point = pose * crow[x]; - memcpy (&data[j++ * 3], &transformed_point, sizeof(cv::Point3f)); + Point3f transformed_point = pose * crow[x]; + memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3f)); } } nr_points = j; @@ -118,11 +118,11 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr j = 0; for(int y = 0; y < colorsMat.rows; ++y) { - const cv::Vec3b* crow = colorsMat.ptr(y); - const cv::Point3f* cloud_row = cloudMat.ptr(y); + const Vec3b* crow = colorsMat.ptr(y); + const Point3f* cloud_row = cloudMat.ptr(y); for(int x = 0; x < colorsMat.cols; ++x) if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); + memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); } reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); @@ -152,9 +152,6 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr // Save the viewpoint transformation matrix to the global actor map vtkSmartPointer transformation = vtkSmartPointer::New(); convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); -// convertToVtkMatrix (pose.matrix, transformation); - - std::cout << transformation->GetElement(0,3) << endl; (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; } From aecff5237b62f04036acc1652b3ea0eecba5a689 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Fri, 7 Jun 2013 16:07:23 +0200 Subject: [PATCH 2/2] CV_64FC3 support --- modules/viz/src/viz3d_impl.cpp | 89 +++++++++++++++++++++++++--------- 1 file changed, 65 insertions(+), 24 deletions(-) diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 615f035e87..a2d9a25fbf 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -32,7 +32,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud { Mat cloudMat = cloud.getMat(); Mat colorsMat = colors.getMat(); - CV_Assert(cloudMat.type() == CV_32FC3 && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); + CV_Assert((cloudMat.type() == CV_32FC3 || cloudMat.type() == CV_64FC3) && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); vtkSmartPointer polydata; vtkSmartPointer vertices; @@ -57,7 +57,10 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud if (!points) { points = vtkSmartPointer::New (); - points->SetDataTypeToFloat (); + if (cloudMat.type() == CV_32FC3) + points->SetDataTypeToFloat (); + else if (cloudMat.type() == CV_64FC3) + points->SetDataTypeToDouble (); polydata->SetPoints (points); } points->SetNumberOfPoints (nr_points); @@ -69,30 +72,54 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud 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 (cloudMat.type() == CV_32FC3) + points->SetDataTypeToFloat (); + else if (cloudMat.type() == CV_64FC3) + points->SetDataTypeToDouble (); // Copy the new point array in nr_points = cloudMat.size().area(); points->SetNumberOfPoints (nr_points); } - - // Get a pointer to the beginning of the data array - float *data = (static_cast (points->GetData ()))->GetPointer (0); - - // Scan through the data and apply mask where point is NAN int j = 0; - - // If a point is NaN, ignore it - for(int y = 0; y < cloudMat.rows; ++y) + if (cloudMat.type() == CV_32FC3) { - const Point3f* crow = cloudMat.ptr(y); - for(int x = 0; x < cloudMat.cols; ++x) - if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) - { - // Points are transformed based on pose parameter - Point3f transformed_point = pose * crow[x]; - memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3f)); - } + // Get a pointer to the beginning of the data array + float *data = (static_cast (points->GetData ()))->GetPointer (0); + + // Scan through the data and apply mask where point is NAN + for(int y = 0; y < cloudMat.rows; ++y) + { + const Point3f* crow = cloudMat.ptr(y); + for(int x = 0; x < cloudMat.cols; ++x) + if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) + { + // Points are transformed based on pose parameter + Point3f transformed_point = pose * crow[x]; + memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3f)); + } + } } + else if (cloudMat.type() == CV_64FC3) + { + // Get a pointer to the beginning of the data array + double *data = (static_cast (points->GetData ()))->GetPointer (0); + + // If a point is NaN, ignore it + for(int y = 0; y < cloudMat.rows; ++y) + { + const Point3d* crow = cloudMat.ptr(y); + for(int x = 0; x < cloudMat.cols; ++x) + if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) + { + // Points are transformed based on pose parameter + Point3d transformed_point = pose * crow[x]; + memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3d)); + } + } + } + nr_points = j; points->SetNumberOfPoints (nr_points); @@ -116,13 +143,27 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud unsigned char* colors_data = new unsigned char[nr_points * 3]; j = 0; - for(int y = 0; y < colorsMat.rows; ++y) + if (cloudMat.type() == CV_32FC3) + { + for(int y = 0; y < colorsMat.rows; ++y) + { + const Vec3b* crow = colorsMat.ptr(y); + const Point3f* cloud_row = cloudMat.ptr(y); + for(int x = 0; x < colorsMat.cols; ++x) + if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) + memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); + } + } + else if (cloudMat.type() == CV_64FC3) { - const Vec3b* crow = colorsMat.ptr(y); - const Point3f* cloud_row = cloudMat.ptr(y); - for(int x = 0; x < colorsMat.cols; ++x) - if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); + for(int y = 0; y < colorsMat.rows; ++y) + { + const Vec3b* crow = colorsMat.ptr(y); + const Point3d* cloud_row = cloudMat.ptr(y); + for(int x = 0; x < colorsMat.cols; ++x) + if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) + memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); + } } reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);