diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp
index c3f595644..9076a3bb4 100644
--- a/modules/viz/src/precomp.hpp
+++ b/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__
diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp
index a2d9a25fb..14f438cff 100644
--- a/modules/viz/src/viz3d_impl.cpp
+++ b/modules/viz/src/viz3d_impl.cpp
@@ -1,15 +1,5 @@
-#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);
+ // 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 ();
+ nr_points = cloud.total();
+ 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);
+ 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);
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (polydata, actor);
- // Add it to all renderers
- renderer_->AddActor (actor);
+ actor->GetMapper ()->SetScalarRange (minmax.val);
- // Save the pointer/ID pair to the global actor map
- (*cloud_actor_map_)[id].actor = actor;
- (*cloud_actor_map_)[id].cells = initcells;
+ // Add it to all renderers
+ renderer_->AddActor (actor);
- const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
- const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
+ // Save the pointer/ID pair to the global actor map
+ (*cloud_actor_map_)[id].actor = actor;
+ (*cloud_actor_map_)[id].cells = initcells;
- // 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;
+ 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);
}
}
diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp
index 925b54c3c..e72ef7598 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));