From 909c45f1b9138c522aa50a10c3aafdf33001836d Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Mon, 10 Jun 2013 12:32:03 +0400 Subject: [PATCH] little refactoring --- modules/viz/src/precomp.hpp | 10 ++ modules/viz/src/viz3d_impl.cpp | 267 +++++++++++++++----------------- modules/viz/test/test_viz3d.cpp | 28 ++-- 3 files changed, 152 insertions(+), 153 deletions(-) diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp index c3f5956445..9076a3bb47 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,15 @@ + + + +#include +#include +#include + + + #if defined __GNUC__ && defined __DEPRECATED_DISABLED__ #define __DEPRECATED #undef __DEPRECATED_DISABLED__ diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index a2d9a25fbf..14f438cffa 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -1,15 +1,5 @@ -#include +#include "precomp.hpp" #include - -#include -#include -#include -#include -#include -#include -#include -#include - #include void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode) @@ -28,178 +18,175 @@ 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 String& id, InputArray cloud, InputArray colors, const Affine3f& pose) +void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _cloud, InputArray _colors, const Affine3f& pose) { - Mat cloudMat = cloud.getMat(); - Mat colorsMat = colors.getMat(); - CV_Assert((cloudMat.type() == CV_32FC3 || cloudMat.type() == CV_64FC3) && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); - + Mat cloud = _cloud.getMat(); + Mat colors = _colors.getMat(); + CV_Assert((cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3)); + CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size()); + vtkSmartPointer polydata; vtkSmartPointer vertices; vtkSmartPointer points; vtkSmartPointer initcells; vtkIdType nr_points; - + // If the cloud already exists, update otherwise create new one CloudActorMap::iterator am_it = cloud_actor_map_->find (id); - bool isAdd = (am_it == cloud_actor_map_->end()); - if (isAdd) + bool exits = (am_it == cloud_actor_map_->end()); + if (exits) { - // Add as new cloud - allocVtkPolyData(polydata); - //polydata = vtkSmartPointer::New (); - vertices = vtkSmartPointer::New (); - polydata->SetVerts (vertices); - - nr_points = cloudMat.size().area(); - points = polydata->GetPoints (); - - if (!points) - { - points = vtkSmartPointer::New (); - if (cloudMat.type() == CV_32FC3) - points->SetDataTypeToFloat (); - else if (cloudMat.type() == CV_64FC3) - points->SetDataTypeToDouble (); - polydata->SetPoints (points); - } - points->SetNumberOfPoints (nr_points); + // Add as new cloud + allocVtkPolyData(polydata); + //polydata = vtkSmartPointer::New (); + vertices = vtkSmartPointer::New (); + polydata->SetVerts (vertices); + + 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); } 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 (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); + // 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 (); + // Copy the new point array in + nr_points = cloud.total(); + points->SetNumberOfPoints (nr_points); } - + int j = 0; - if (cloudMat.type() == CV_32FC3) + if (cloud.type() == CV_32FC3) { - // 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)); - } - } + // Get a pointer to the beginning of the data array + Point3f *data = reinterpret_cast((static_cast (points->GetData ()))->GetPointer (0)); + + // Scan through the data and apply mask where point is NAN + for(int y = 0; y < cloud.rows; ++y) + { + const Point3f* crow = cloud.ptr(y); + for(int x = 0; x < cloud.cols; ++x) + + //TODO implementa templated copy_if() or copy_non_nans() and use everywhere. + if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) + data[j++] = pose * crow[x]; + } } - else if (cloudMat.type() == CV_64FC3) + else if (cloud.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)); - } - } + // Get a pointer to the beginning of the data array + Point3d *data = reinterpret_cast((static_cast (points->GetData ()))->GetPointer (0)); + + // If a point is NaN, ignore it + for(int y = 0; y < cloud.rows; ++y) + { + const Point3d* crow = cloud.ptr(y); + for(int x = 0; x < cloud.cols; ++x) + if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) + data[j++] = pose * crow[x]; + } } - + nr_points = j; points->SetNumberOfPoints (nr_points); - + vtkSmartPointer cells = vertices->GetData (); - - if (isAdd) - updateCells(cells, initcells, nr_points); + + if (exits) + updateCells(cells, initcells, nr_points); else - updateCells (cells, am_it->second.cells, nr_points); - + updateCells (cells, am_it->second.cells, nr_points); + // Set the cells and the vertices vertices->SetCells (nr_points, cells); - + // Get the colors from the handler - double minmax[2]; + Vec2d minmax; vtkSmartPointer scalars = vtkSmartPointer::New (); scalars->SetNumberOfComponents (3); reinterpret_cast(&(*scalars))->SetNumberOfTuples (nr_points); // Get a random color - unsigned char* colors_data = new unsigned char[nr_points * 3]; - + Vec3b* colors_data = new Vec3b[nr_points]; + j = 0; - if (cloudMat.type() == CV_32FC3) + if (cloud.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)); - } + for(int y = 0; y < colors.rows; ++y) + { + const Vec3b* crow = colors.ptr(y); + const Point3f* cloud_row = cloud.ptr(y); + + for(int x = 0; x < colors.cols; ++x) + if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) + colors_data[j++] = crow[x]; + } } - else if (cloudMat.type() == CV_64FC3) + else if (cloud.type() == CV_64FC3) { - 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)); - } + for(int y = 0; y < colors.rows; ++y) + { + const Vec3b* crow = colors.ptr(y); + const Point3d* cloud_row = cloud.ptr(y); + + for(int x = 0; x < colors.cols; ++x) + if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) + colors_data[j++] = crow[x]; + } } - - reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); - + + reinterpret_cast(&(*scalars))->SetArray (reinterpret_cast(colors_data), 3 * nr_points, 0); + // Assign the colors polydata->GetPointData ()->SetScalars (scalars); - scalars->GetRange (minmax); + scalars->GetRange (minmax.val); // If this is the new point cloud, a new actor is created - if (isAdd) + if (exits) { - vtkSmartPointer actor; - createActorFromVTKDataSet (polydata, actor); - - actor->GetMapper ()->SetScalarRange (minmax); - - // 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::Quaternion& 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; + vtkSmartPointer actor; + createActorFromVTKDataSet (polydata, actor); + + actor->GetMapper ()->SetScalarRange (minmax.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::Quaternion& 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); + // Update the mapper + reinterpret_cast(am_it->second.actor->GetMapper ())->SetInput (polydata); } } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 925b54c3c5..e72ef75989 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -88,20 +88,22 @@ TEST(Viz_viz3d, accuracy) float pos_z = 0.0f; temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply"); v.addPolygonMesh(*mesh, "pq"); - while(1) + + + while(1) //TODO implement and replace with !viz.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); - - angle_x += 0.1; - angle_y -= 0.1; - angle_z += 0.1; - pos_x = std::sin(angle_x); - pos_y = std::sin(angle_x); - pos_z = std::sin(angle_x); - - v.spinOnce(1,true); + // 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); + + angle_x += 0.1f; + angle_y -= 0.1f; + angle_z += 0.1f; + pos_x = std::sin(angle_x); + pos_y = std::sin(angle_x); + pos_z = std::sin(angle_x); + + v.spinOnce(10); } // cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));