showPointCloud initial implementation together with test

modules/viz/include/opencv2/viz/viz3d.hpp

@@ -27,21 +27,14 @@ namespace temp_viz
 
         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");
 

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");

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);

modules/viz/src/viz3d_impl.cpp

@@ -27,21 +27,32 @@ 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)
+
+void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose)
 {
-    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()));
+    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<vtkPolyData> polydata;
+    vtkSmartPointer<vtkCellArray> vertices;
+    vtkSmartPointer<vtkPoints> points;
+    vtkSmartPointer<vtkIdTypeArray> 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)
+    {
+	// Add as new cloud
 	allocVtkPolyData(polydata);
 	//polydata = vtkSmartPointer<vtkPolyData>::New ();
-    vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
+	vertices = vtkSmartPointer<vtkCellArray>::New ();
 	polydata->SetVerts (vertices);
 
-    vtkSmartPointer<vtkIdTypeArray> initcells;
-    vtkIdType nr_points = cloud.size().area();
-    vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
+	nr_points = cloudMat.size().area();
+	points = polydata->GetPoints ();
 
 	if (!points)
 	{
@@ -50,48 +61,52 @@ void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat
 	    polydata->SetPoints (points);
 	}
 	points->SetNumberOfPoints (nr_points);
+    }
+    else
+    {
+	// Update the cloud
+	// Get the current poly data
+	polydata = reinterpret_cast<vtkPolyDataMapper*>(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<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
     
-    if (mask.empty())
-    {
+    // Scan through the data and apply mask where point is NAN
     int j = 0;
-        for(int y = 0; y < cloud.rows; ++y)
-        {
-            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
-            for(int x = 0; x < cloud.cols; ++x)
-                memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
-        }
-    }
-    else
+     
+    // If a point is NaN, ignore it
+    for(int y = 0; y < cloudMat.rows; ++y)
     {
-        int j = 0;
-        for(int y = 0; y < cloud.rows; ++y)
+	const cv::Point3f* crow = cloudMat.ptr<cv::Point3f>(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)
 	    {
-            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(y);
-            const unsigned char* mrow = mask.ptr<unsigned char>(y);
-            for(int x = 0; x < cloud.cols; ++x)
-                if (mrow[x])
-                    memcpy (&data[j++ * 3], &crow[x], sizeof(cv::Point3f));
+		// 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<vtkIdTypeArray> cells = vertices->GetData ();
-    updateCells (cells, initcells, 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);
     
-    /////////////////////////////////////////////////////////////////////////////////
-
-    // use the given geometry handler
-    polydata->Update ();
-
     // Get the colors from the handler
-    bool has_colors = false;
     double minmax[2];
     vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
     scalars->SetNumberOfComponents (3);
@@ -100,44 +115,28 @@ void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat
     // 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)
+    j = 0;
+    for(int y = 0; y < colorsMat.rows; ++y)
     {
-            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
-            for(int x = 0; x < colors.cols; ++x)
+	const cv::Vec3b* crow = colorsMat.ptr<cv::Vec3b>(y);
+	const cv::Point3f* cloud_row = cloudMat.ptr<cv::Point3f>(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));
     }
-    }
-    else
-    {
-        int j = 0;
-        for(int y = 0; y < colors.rows; ++y)
-        {
-            const cv::Vec3b* crow = colors.ptr<cv::Vec3b>(y);
-            const unsigned char* mrow = mask.ptr<unsigned char>(y);
-            for(int x = 0; x < colors.cols; ++x)
-                if (mrow[x])
-                    memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
-        }
-    }
     
     reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
     
-    /////////////////////////////////////////
-    has_colors = true;
-
-    if (has_colors)
-    {
+    // Assign the colors
     polydata->GetPointData ()->SetScalars (scalars);
     scalars->GetRange (minmax);
-    }
 
-    // Create an Actor
+    // If this is the new point cloud, a new actor is created
+    if (isAdd)
+    {
 	vtkSmartPointer<vtkLODActor> actor;
 	createActorFromVTKDataSet (polydata, actor);
-    if (has_colors)
+	
 	actor->GetMapper ()->SetScalarRange (minmax);
 
 	// Add it to all renderers
@@ -153,142 +152,19 @@ void temp_viz::Viz3d::VizImpl::addPointCloud(const cv::Mat& cloud, const cv::Mat
 	// Save the viewpoint transformation matrix to the global actor map
 	vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::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?)
-    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-    if (am_it == cloud_actor_map_->end ())
-        return (false);
-
-    // Get the current poly data
-    vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
-    if (!polydata)
-        return (false);
-    vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
-    vtkSmartPointer<vtkPoints> 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<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
-
-    if (mask.empty())
-    {
-        int j = 0;
-        for(int y = 0; y < cloud.rows; ++y)
-        {
-            const cv::Point3f* crow = cloud.ptr<cv::Point3f>(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<cv::Point3f>(y);
-            const unsigned char* mrow = mask.ptr<unsigned char>(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<vtkIdTypeArray> cells = vertices->GetData ();
-    updateCells (cells, am_it->second.cells, nr_points);
+// 	convertToVtkMatrix (pose.matrix, transformation);
 	
+	std::cout << transformation->GetElement(0,3) << endl;
 	
-    // Set the cells and the vertices
-    vertices->SetCells (nr_points, cells);
-
-#if 1
-    // Get the colors from the handler
-    //  vtkSmartPointer<vtkDataArray> 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<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-    scalars->SetNumberOfComponents (3);
-    reinterpret_cast<vtkUnsignedCharArray*>(&(*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<cv::Vec3b>(y);
-            for(int x = 0; x < colors.cols; ++x)
-                memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
-        }
+	(*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<cv::Vec3b>(y);
-            const unsigned char* mrow = mask.ptr<unsigned char>(y);
-            for(int x = 0; x < colors.cols; ++x)
-                if (mrow[x])
-                    memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b));
-        }
-    }
-
-    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
-
-    /////////////////////////////////////////
-    has_colors = true;
-
-    if (has_colors)
-    {
-        polydata->GetPointData ()->SetScalars (scalars);
-        scalars->GetRange (minmax);
-    }
-
-#else
-    vtkSmartPointer<vtkDataArray> scalars;
-    polydata->GetPointData ()->SetScalars (scalars);
-    polydata->Update ();
-    double minmax[2];
-    minmax[0] = std::numeric_limits<double>::min ();
-    minmax[1] = std::numeric_limits<double>::max ();
-    am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
-    am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
-#endif
-
-
 	// Update the mapper
 	reinterpret_cast<vtkPolyDataMapper*>(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);

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<cv::Point3f>()[i] += cv::Point3f(1, 1, 1);
-
-    v.updatePolygonMesh(*mesh, "pq");
-
-
-    for(int i = 0; i < mesh->cloud.cols; ++i)
-        mesh->cloud.ptr<cv::Point3f>()[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<cv::Point3f>()[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<cv::Point3f>();
-    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<cv::Point3f>()[i] += cv::Point3f(1, 1, 1);
+// 
+//     v.updatePolygonMesh(*mesh, "pq");
+// 
+// 
+//     for(int i = 0; i < mesh->cloud.cols; ++i)
+//         mesh->cloud.ptr<cv::Point3f>()[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<cv::Point3f>()[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<cv::Point3f>();
+//     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();
 }