diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index fe0851a60..2e89e5f90 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -26,22 +26,15 @@ namespace temp_viz void setBackgroundColor(const Color& color = Color::black()); void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate"); - - void addPointCloud(const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat()); + + void showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose = cv::Affine3f::Identity()); bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud"); - - - bool addPlane (const ModelCoefficients &coefficients, const String &id = "plane"); bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String &id = "plane"); bool removeCoordinateSystem (const String &id = "coordinate"); - - bool updatePointCloud (const Mat& cloud, const Mat& colors, const String& id = "cloud", const Mat& mask = Mat()); - - bool addPolygonMesh (const Mesh3d& mesh, const String &id = "polygon"); bool updatePolygonMesh (const Mesh3d& mesh, const String &id = "polygon"); diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 5e72280a5..17b643c2e 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -96,8 +96,14 @@ public: bool addText3D (const std::string &text, const cv::Point3f &position, const Color& color, double textScale = 1.0, const std::string &id = ""); bool addPointCloudNormals (const cv::Mat &cloud, const cv::Mat& normals, int level = 100, float scale = 0.02f, const std::string &id = "cloud"); - void addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat()); - bool updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id = "cloud", const cv::Mat& mask = cv::Mat()); + + /** \brief If the id exists, updates the point cloud; otherwise, adds a new point cloud to the scene + * \param[in] id a variable to identify the point cloud + * \param[in] cloud cloud input in x,y,z coordinates + * \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()); 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 974013d2f..4b2e3b19e 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -23,9 +23,9 @@ void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const impl_->addCoordinateSystem(scale, t, id); } -void temp_viz::Viz3d::addPointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask) +void temp_viz::Viz3d::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) { - impl_->addPointCloud(cloud, colors, id, mask); + impl_->showPointCloud(id, cloud, colors, pose); } bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals, int level, float scale, const String& id) @@ -33,11 +33,6 @@ bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals return impl_->addPointCloudNormals(cloud, normals, level, scale, id); } -bool temp_viz::Viz3d::updatePointCloud(const Mat& cloud, const Mat& colors, const String& id, const Mat& mask) -{ - return impl_->updatePointCloud(cloud, colors, id, mask); -} - bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String &id) { return impl_->addPolygonMesh(mesh, Mat(), id); diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 223b2bd1f..692e9960d 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -27,207 +27,86 @@ void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name) void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); } void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); } -void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask) -{ - CV_Assert(cloud.type() == CV_32FC3 && colors.type() == CV_8UC3 && colors.size() == cloud.size()); - CV_Assert(mask.empty() || (mask.type() == CV_8U && mask.size() == cloud.size())); +void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) +{ + cv::Mat cloudMat = cloud.getMat(); + cv::Mat colorsMat = colors.getMat(); + CV_Assert(cloudMat.type() == CV_32FC3 && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); + vtkSmartPointer polydata; - - allocVtkPolyData(polydata); - //polydata = vtkSmartPointer::New (); - vtkSmartPointer vertices = vtkSmartPointer::New (); - polydata->SetVerts (vertices); - + vtkSmartPointer vertices; + vtkSmartPointer points; vtkSmartPointer initcells; - vtkIdType nr_points = cloud.size().area(); - vtkSmartPointer points = polydata->GetPoints (); - - if (!points) - { - points = vtkSmartPointer::New (); - points->SetDataTypeToFloat (); - polydata->SetPoints (points); - } - points->SetNumberOfPoints (nr_points); - - // Get a pointer to the beginning of the data array - float *data = (static_cast (points->GetData ()))->GetPointer (0); - - if (mask.empty()) - { - int j = 0; - for(int y = 0; y < cloud.rows; ++y) - { - const cv::Point3f* crow = cloud.ptr(y); - for(int x = 0; x < cloud.cols; ++x) - memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f)); - } - } - else - { - int j = 0; - for(int y = 0; y < cloud.rows; ++y) - { - const cv::Point3f* crow = cloud.ptr(y); - const unsigned char* mrow = mask.ptr(y); - for(int x = 0; x < cloud.cols; ++x) - if (mrow[x]) - memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f)); - } - nr_points = j; - points->SetNumberOfPoints (nr_points); - } - - vtkSmartPointer cells = vertices->GetData (); - updateCells (cells, initcells, nr_points); - - // Set the cells and the vertices - vertices->SetCells (nr_points, cells); - - ///////////////////////////////////////////////////////////////////////////////// - - // use the given geometry handler - polydata->Update (); - - // Get the colors from the handler - bool has_colors = false; - double minmax[2]; - 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]; - - if (mask.empty()) - { - int j = 0; - for(int y = 0; y < colors.rows; ++y) - { - const cv::Vec3b* crow = colors.ptr(y); - for(int x = 0; x < colors.cols; ++x) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); - } - } - else - { - int j = 0; - for(int y = 0; y < colors.rows; ++y) - { - const cv::Vec3b* crow = colors.ptr(y); - const unsigned char* mrow = mask.ptr(y); - for(int x = 0; x < colors.cols; ++x) - if (mrow[x]) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); - } - } - - reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); - - ///////////////////////////////////////// - has_colors = true; - - if (has_colors) - { - polydata->GetPointData ()->SetScalars (scalars); - scalars->GetRange (minmax); - } - - // Create an Actor - vtkSmartPointer actor; - createActorFromVTKDataSet (polydata, actor); - if (has_colors) - 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; -} - - -bool temp_viz::Viz3d::VizImpl::updatePointCloud (const cv::Mat& cloud, const cv::Mat& colors, const std::string& id, const cv::Mat& mask) -{ - // Check to see if this ID entry already exists (has it been already added to the visualizer?) + vtkIdType nr_points; + + // If the cloud already exists, update otherwise create new one CloudActorMap::iterator am_it = cloud_actor_map_->find (id); - if (am_it == cloud_actor_map_->end ()) - return (false); - - // Get the current poly data - vtkSmartPointer polydata = reinterpret_cast(am_it->second.actor->GetMapper ())->GetInput (); - if (!polydata) - return (false); - vtkSmartPointer vertices = polydata->GetVerts (); - vtkSmartPointer points = polydata->GetPoints (); - // Copy the new point array in - vtkIdType nr_points = cloud.size().area(); - points->SetNumberOfPoints (nr_points); - - // Get a pointer to the beginning of the data array - float *data = (static_cast (points->GetData ()))->GetPointer (0); - - if (mask.empty()) + bool isAdd = (am_it == cloud_actor_map_->end()); + if (isAdd) { - int j = 0; - for(int y = 0; y < cloud.rows; ++y) - { - const cv::Point3f* crow = cloud.ptr(y); - for(int x = 0; x < cloud.cols; ++x) - memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f)); - } + // 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 (); + points->SetDataTypeToFloat (); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); } else { - int j = 0; - for(int y = 0; y < cloud.rows; ++y) - { - const cv::Point3f* crow = cloud.ptr(y); - const unsigned char* mrow = mask.ptr(y); - for(int x = 0; x < cloud.cols; ++x) - if (mrow[x]) - memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f)); - } - nr_points = j; - 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 (); + // 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) + { + const cv::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)); + } + } + nr_points = j; + points->SetNumberOfPoints (nr_points); + vtkSmartPointer cells = vertices->GetData (); - updateCells (cells, am_it->second.cells, nr_points); - - + + if (isAdd) + 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); - -#if 1 + // Get the colors from the handler - // vtkSmartPointer scalars; - // color_handler.getColor (scalars); - // double minmax[2]; - // scalars->GetRange (minmax); - - // // Update the data - // polydata->GetPointData ()->SetScalars (scalars); - // polydata->Update (); - - // am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); - // am_it->second.actor->GetMapper ()->SetScalarRange (minmax); - - - //////////////////////////////////////////////////////////////////////////////////////////////////////// - - // Get the colors from the handler - bool has_colors = false; double minmax[2]; vtkSmartPointer scalars = vtkSmartPointer::New (); scalars->SetNumberOfComponents (3); @@ -235,60 +114,57 @@ bool temp_viz::Viz3d::VizImpl::updatePointCloud (const cv::Mat& cloud, const cv: // Get a random color unsigned char* colors_data = new unsigned char[nr_points * 3]; - - if (mask.empty()) + + j = 0; + for(int y = 0; y < colorsMat.rows; ++y) { - int j = 0; - for(int y = 0; y < colors.rows; ++y) - { - const cv::Vec3b* crow = colors.ptr(y); - for(int x = 0; x < colors.cols; ++x) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); - } + const cv::Vec3b* crow = colorsMat.ptr(y); + const cv::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)); + } + + reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); + + // Assign the colors + polydata->GetPointData ()->SetScalars (scalars); + scalars->GetRange (minmax); + + // If this is the new point cloud, a new actor is created + if (isAdd) + { + 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); +// convertToVtkMatrix (pose.matrix, transformation); + + std::cout << transformation->GetElement(0,3) << endl; + + (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; } else { - int j = 0; - for(int y = 0; y < colors.rows; ++y) - { - const cv::Vec3b* crow = colors.ptr(y); - const unsigned char* mrow = mask.ptr(y); - for(int x = 0; x < colors.cols; ++x) - if (mrow[x]) - memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); - } + // Update the mapper + reinterpret_cast(am_it->second.actor->GetMapper ())->SetInput (polydata); } - - reinterpret_cast(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); - - ///////////////////////////////////////// - has_colors = true; - - if (has_colors) - { - polydata->GetPointData ()->SetScalars (scalars); - scalars->GetRange (minmax); - } - -#else - vtkSmartPointer scalars; - polydata->GetPointData ()->SetScalars (scalars); - polydata->Update (); - double minmax[2]; - minmax[0] = std::numeric_limits::min (); - minmax[1] = std::numeric_limits::max (); - am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); - am_it->second.actor->GetMapper ()->SetScalarRange (minmax); -#endif - - - // Update the mapper - reinterpret_cast(am_it->second.actor->GetMapper ())->SetInput (polydata); - return (true); } - - 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); diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 922090c83..925b54c3c 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -78,56 +78,80 @@ TEST(Viz_viz3d, accuracy) cv::Mat cloud = cvcloud_load(); - cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0)); - v.addPointCloud(cloud, colors); - cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0)); - - v.addPointCloudNormals(cloud, normals, 100, 0.02, "n"); - - - temp_viz::ModelCoefficients mc; - mc.values.resize(4); - mc.values[0] = mc.values[1] = mc.values[2] = mc.values[3] = 1; - v.addPlane(mc); - - + + float angle_x = 0.0f; + float angle_y = 0.0f; + float angle_z = 0.0f; + float pos_x = 0.0f; + float pos_y = 0.0f; + float pos_z = 0.0f; temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply"); v.addPolygonMesh(*mesh, "pq"); - - v.spinOnce(1000, true); - - v.removeCoordinateSystem(); - - for(int i = 0; i < mesh->cloud.cols; ++i) - mesh->cloud.ptr()[i] += cv::Point3f(1, 1, 1); - - v.updatePolygonMesh(*mesh, "pq"); - - - for(int i = 0; i < mesh->cloud.cols; ++i) - mesh->cloud.ptr()[i] -= cv::Point3f(2, 2, 2); - v.addPolylineFromPolygonMesh(*mesh); - - - v.addText("===Abd sadfljsadlk", 100, 100, cv::Scalar(255, 0, 0), 15); - for(int i = 0; i < cloud.cols; ++i) - cloud.ptr()[i].x *=2; - - colors.setTo(cv::Scalar(255, 0, 0)); - - v.addSphere(cv::Point3f(0, 0, 0), 0.3, temp_viz::Color::blue()); - - cv::Mat cvpoly(1, 5, CV_32FC3); - cv::Point3f* pdata = cvpoly.ptr(); - pdata[0] = cv::Point3f(0, 0, 0); - pdata[1] = cv::Point3f(0, 1, 1); - pdata[2] = cv::Point3f(3, 1, 2); - pdata[3] = cv::Point3f(0, 2, 4); - pdata[4] = cv::Point3f(7, 2, 3); - v.addPolygon(cvpoly, temp_viz::Color::white()); - - v.updatePointCloud(cloud, colors); - v.spin(); + while(1) + { + // 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); + } + +// cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0)); +// +// v.addPointCloudNormals(cloud, normals, 100, 0.02, "n"); +// +// +// temp_viz::ModelCoefficients mc; +// mc.values.resize(4); +// mc.values[0] = mc.values[1] = mc.values[2] = mc.values[3] = 1; +// v.addPlane(mc); +// +// +// temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("horse.ply"); +// v.addPolygonMesh(*mesh, "pq"); +// +// v.spinOnce(1000, true); +// +// v.removeCoordinateSystem(); +// +// for(int i = 0; i < mesh->cloud.cols; ++i) +// mesh->cloud.ptr()[i] += cv::Point3f(1, 1, 1); +// +// v.updatePolygonMesh(*mesh, "pq"); +// +// +// for(int i = 0; i < mesh->cloud.cols; ++i) +// mesh->cloud.ptr()[i] -= cv::Point3f(2, 2, 2); +// v.addPolylineFromPolygonMesh(*mesh); +// +// +// v.addText("===Abd sadfljsadlk", 100, 100, cv::Scalar(255, 0, 0), 15); +// for(int i = 0; i < cloud.cols; ++i) +// cloud.ptr()[i].x *=2; +// +// colors.setTo(cv::Scalar(255, 0, 0)); +// +// v.addSphere(cv::Point3f(0, 0, 0), 0.3, temp_viz::Color::blue()); +// +// cv::Mat cvpoly(1, 5, CV_32FC3); +// cv::Point3f* pdata = cvpoly.ptr(); +// pdata[0] = cv::Point3f(0, 0, 0); +// pdata[1] = cv::Point3f(0, 1, 1); +// pdata[2] = cv::Point3f(3, 1, 2); +// pdata[3] = cv::Point3f(0, 2, 4); +// pdata[4] = cv::Point3f(7, 2, 3); +// v.addPolygon(cvpoly, temp_viz::Color::white()); +// +// // Updating cloud1 +// v.showPointCloud("cloud1", cloud, colors); +// v.spin(); }