little refactoring

modules/viz/src/precomp.hpp

@@ -56,6 +56,7 @@
 #include <vtkCubeSource.h>
 #include <vtkAxes.h>
 #include <vtkFloatArray.h>
+#include <vtkDoubleArray.h>
 #include <vtkPointData.h>
 #include <vtkPolyData.h>
 #include <vtkPolyDataReader.h>
@@ -153,6 +154,15 @@
 
 
 
+
+
+
+#include <vtkMapper2D.h>
+#include <vtkLeaderActor2D.h>
+#include <vtkAlgorithmOutput.h>
+
+
+
 #if defined __GNUC__ && defined __DEPRECATED_DISABLED__
 #define __DEPRECATED
 #undef __DEPRECATED_DISABLED__

modules/viz/src/viz3d_impl.cpp

-#include <opencv2/core.hpp>
+#include "precomp.hpp"
 #include <q/shapes.h>
-
-#include <vtkCellData.h>
-#include <vtkSmartPointer.h>
-#include <vtkCellArray.h>
-#include <vtkProperty2D.h>
-#include <vtkMapper2D.h>
-#include <vtkLeaderActor2D.h>
-#include <q/shapes.h>
-#include <vtkAlgorithmOutput.h>
-
 #include <q/viz3d_impl.hpp>
 
 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<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)
+    bool exits = (am_it == cloud_actor_map_->end());
+    if (exits)
     {
-	// Add as new cloud
-	allocVtkPolyData(polydata);
-	//polydata = vtkSmartPointer<vtkPolyData>::New ();
-	vertices = vtkSmartPointer<vtkCellArray>::New ();
-	polydata->SetVerts (vertices);
-
-	nr_points = cloudMat.size().area();
-	points = polydata->GetPoints ();
-
-	if (!points)
-	{
-	    points = vtkSmartPointer<vtkPoints>::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<vtkPolyData>::New ();
+        vertices = vtkSmartPointer<vtkCellArray>::New ();
+        polydata->SetVerts (vertices);
+
+        nr_points = cloud.total();
+        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 (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<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 ();
+        // 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<vtkFloatArray*> (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<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)
-		{
-		    // 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<Point3f*>((static_cast<vtkFloatArray*> (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<Point3f>(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<vtkDoubleArray*> (points->GetData ()))->GetPointer (0);
-	
-	// If a point is NaN, ignore it
-	for(int y = 0; y < cloudMat.rows; ++y)
-	{
-	    const Point3d* crow = cloudMat.ptr<Point3d>(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<Point3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
+
+        // If a point is NaN, ignore it
+        for(int y = 0; y < cloud.rows; ++y)
+        {
+            const Point3d* crow = cloud.ptr<Point3d>(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<vtkIdTypeArray> 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<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];
-    
+    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<Vec3b>(y);
-	    const Point3f* cloud_row = cloudMat.ptr<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(Vec3b));
-	}
+        for(int y = 0; y < colors.rows; ++y)
+        {
+            const Vec3b* crow = colors.ptr<Vec3b>(y);
+            const Point3f* cloud_row = cloud.ptr<Point3f>(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<Vec3b>(y);
-	    const Point3d* cloud_row = cloudMat.ptr<Point3d>(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<Vec3b>(y);
+            const Point3d* cloud_row = cloud.ptr<Point3d>(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<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
-    
+
+    reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (reinterpret_cast<unsigned char*>(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<vtkLODActor> 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<float>& 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;
+        vtkSmartPointer<vtkLODActor> 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<float>& 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);
+        // Update the mapper
+        reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
     }
 }
 

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