Merge pull request #9 from ozantonkal/implementing_addPointCloud

Implementing add point cloud

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

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");
@@ -493,7 +494,7 @@ struct ApplyAffine
     const Affine3f& affine_;
     ApplyAffine(const Affine3f& affine) : affine_(affine) {}
 
-    template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine * p; }
+    template<typename _Tp> Point3_<_Tp> operator()(const Point3_<_Tp>& p) { return affine_ * p; }
 
     template<typename _Tp> Vec<_Tp, 3> operator()(const Vec<_Tp, 3>& v)
     {

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

modules/viz/src/viz3d_impl.cpp

@@ -114,7 +114,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
     vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
 
     if (exist)
-        updateCells(cells, initcells, nr_points);
+        updateCells (cells, initcells, nr_points);
     else
         updateCells (cells, am_it->second.cells, nr_points);
 
@@ -163,6 +163,127 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
     {
         // Update the mapper
         reinterpret_cast<vtkPolyDataMapper*>(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)
+{
+    Mat cloud = _cloud.getMat();
+    CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
+        
+    vtkSmartPointer<vtkPolyData> polydata;
+    vtkSmartPointer<vtkCellArray> vertices;
+    vtkSmartPointer<vtkPoints> points;
+    vtkSmartPointer<vtkIdTypeArray> 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<vtkPolyData>::New ();
+        vertices = vtkSmartPointer<vtkCellArray>::New ();
+        polydata->SetVerts (vertices);
+
+        points = polydata->GetPoints ();
+
+        if (!points)
+        {
+            points = vtkSmartPointer<vtkPoints>::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<vtkPolyDataMapper*>(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<float>(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<double>(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<vtkIdTypeArray> 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<vtkLODActor> 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<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
+        convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
+
+        (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
+    }
+    else
+    {
+        // Update the mapper
+        reinterpret_cast<vtkPolyDataMapper*>(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 ();
     }
 }
 

modules/viz/test/test_viz3d.cpp

@@ -89,12 +89,21 @@ 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;
+    
+    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, colors, 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;
@@ -102,8 +111,11 @@ 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);
+        v.spinOnce(10, true);
     }
    
 //     cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));