generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

little refactoring

Browse Source
This commit is contained in:
Anatoly Baksheev 2013-06-10 12:32:03 +04:00
parent 01e99db675
commit 909c45f1b9
3 changed files with 146 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
modules/viz/src/precomp.hpp
View File

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

225
modules/viz/src/viz3d_impl.cpp
View File

@ -1,15 +1,5 @@
#include <opencv2/core.hpp> #include "precomp.hpp"
#include <q/shapes.h> #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> #include <q/viz3d_impl.hpp>
void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode) void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
@ -28,11 +18,12 @@ 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::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 cloud = _cloud.getMat();
Mat colorsMat = colors.getMat(); Mat colors = _colors.getMat();
CV_Assert((cloudMat.type() == CV_32FC3 || cloudMat.type() == CV_64FC3) && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); CV_Assert((cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3));
CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size());
vtkSmartPointer<vtkPolyData> polydata; vtkSmartPointer<vtkPolyData> polydata;
vtkSmartPointer<vtkCellArray> vertices; vtkSmartPointer<vtkCellArray> vertices;
@ -42,82 +33,76 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud
// If the cloud already exists, update otherwise create new one // If the cloud already exists, update otherwise create new one
CloudActorMap::iterator am_it = cloud_actor_map_->find (id); CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
bool isAdd = (am_it == cloud_actor_map_->end()); bool exits = (am_it == cloud_actor_map_->end());
if (isAdd) if (exits)
{ {
// Add as new cloud // Add as new cloud
allocVtkPolyData(polydata); allocVtkPolyData(polydata);
//polydata = vtkSmartPointer<vtkPolyData>::New (); //polydata = vtkSmartPointer<vtkPolyData>::New ();
vertices = vtkSmartPointer<vtkCellArray>::New (); vertices = vtkSmartPointer<vtkCellArray>::New ();
polydata->SetVerts (vertices); polydata->SetVerts (vertices);
nr_points = cloudMat.size().area(); nr_points = cloud.total();
points = polydata->GetPoints (); points = polydata->GetPoints ();
if (!points) if (!points)
{ {
points = vtkSmartPointer<vtkPoints>::New (); points = vtkSmartPointer<vtkPoints>::New ();
if (cloudMat.type() == CV_32FC3) if (cloud.depth() == CV_32F)
points->SetDataTypeToFloat (); points->SetDataTypeToFloat();
else if (cloudMat.type() == CV_64FC3) else if (cloud.depth() == CV_64F)
points->SetDataTypeToDouble (); points->SetDataTypeToDouble();
polydata->SetPoints (points); polydata->SetPoints (points);
} }
points->SetNumberOfPoints (nr_points); points->SetNumberOfPoints (nr_points);
} }
else else
{ {
// Update the cloud // Update the cloud
// Get the current poly data // Get the current poly data
polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput (); polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
vertices = polydata->GetVerts (); vertices = polydata->GetVerts ();
points = polydata->GetPoints (); points = polydata->GetPoints ();
// Update the point data type based on the cloud // Update the point data type based on the cloud
if (cloudMat.type() == CV_32FC3) if (cloud.depth() == CV_32F)
points->SetDataTypeToFloat (); points->SetDataTypeToFloat ();
else if (cloudMat.type() == CV_64FC3) else if (cloud.depth() == CV_64F)
points->SetDataTypeToDouble (); points->SetDataTypeToDouble ();
// Copy the new point array in // Copy the new point array in
nr_points = cloudMat.size().area(); nr_points = cloud.total();
points->SetNumberOfPoints (nr_points); points->SetNumberOfPoints (nr_points);
} }
int j = 0; int j = 0;
if (cloudMat.type() == CV_32FC3) if (cloud.type() == CV_32FC3)
{ {
// Get a pointer to the beginning of the data array // Get a pointer to the beginning of the data array
float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0); Point3f *data = reinterpret_cast<Point3f*>((static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0));
// Scan through the data and apply mask where point is NAN // Scan through the data and apply mask where point is NAN
for(int y = 0; y < cloudMat.rows; ++y) for(int y = 0; y < cloud.rows; ++y)
{ {
const Point3f* crow = cloudMat.ptr<Point3f>(y); const Point3f* crow = cloud.ptr<Point3f>(y);
for(int x = 0; x < cloudMat.cols; ++x) for(int x = 0; x < cloud.cols; ++x)
if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
{ //TODO implementa templated copy_if() or copy_non_nans() and use everywhere.
// Points are transformed based on pose parameter if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
Point3f transformed_point = pose * crow[x]; data[j++] = pose * crow[x];
memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3f)); }
}
}
} }
else if (cloudMat.type() == CV_64FC3) else if (cloud.type() == CV_64FC3)
{ {
// Get a pointer to the beginning of the data array // Get a pointer to the beginning of the data array
double *data = (static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0); Point3d *data = reinterpret_cast<Point3d*>((static_cast<vtkDoubleArray*> (points->GetData ()))->GetPointer (0));
// If a point is NaN, ignore it // If a point is NaN, ignore it
for(int y = 0; y < cloudMat.rows; ++y) for(int y = 0; y < cloud.rows; ++y)
{ {
const Point3d* crow = cloudMat.ptr<Point3d>(y); const Point3d* crow = cloud.ptr<Point3d>(y);
for(int x = 0; x < cloudMat.cols; ++x) for(int x = 0; x < cloud.cols; ++x)
if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1) if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
{ data[j++] = pose * crow[x];
// Points are transformed based on pose parameter }
Point3d transformed_point = pose * crow[x];
memcpy (&data[j++ * 3], &transformed_point, sizeof(Point3d));
}
}
} }
nr_points = j; nr_points = j;
@ -125,81 +110,83 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud
vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData (); vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
if (isAdd) if (exits)
updateCells(cells, initcells, nr_points); updateCells(cells, initcells, nr_points);
else else
updateCells (cells, am_it->second.cells, nr_points); updateCells (cells, am_it->second.cells, nr_points);
// Set the cells and the vertices // Set the cells and the vertices
vertices->SetCells (nr_points, cells); vertices->SetCells (nr_points, cells);
// Get the colors from the handler // Get the colors from the handler
double minmax[2]; Vec2d minmax;
vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); vtkSmartPointer<vtkDataArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3); scalars->SetNumberOfComponents (3);
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
// Get a random color // Get a random color
unsigned char* colors_data = new unsigned char[nr_points * 3]; Vec3b* colors_data = new Vec3b[nr_points];
j = 0; j = 0;
if (cloudMat.type() == CV_32FC3) if (cloud.type() == CV_32FC3)
{ {
for(int y = 0; y < colorsMat.rows; ++y) for(int y = 0; y < colors.rows; ++y)
{ {
const Vec3b* crow = colorsMat.ptr<Vec3b>(y); const Vec3b* crow = colors.ptr<Vec3b>(y);
const Point3f* cloud_row = cloudMat.ptr<Point3f>(y); const Point3f* cloud_row = cloud.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) for(int x = 0; x < colors.cols; ++x)
memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); 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) for(int y = 0; y < colors.rows; ++y)
{ {
const Vec3b* crow = colorsMat.ptr<Vec3b>(y); const Vec3b* crow = colors.ptr<Vec3b>(y);
const Point3d* cloud_row = cloudMat.ptr<Point3d>(y); const Point3d* cloud_row = cloud.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) for(int x = 0; x < colors.cols; ++x)
memcpy (&colors_data[j++ * 3], &crow[x], sizeof(Vec3b)); 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 // Assign the colors
polydata->GetPointData ()->SetScalars (scalars); polydata->GetPointData ()->SetScalars (scalars);
scalars->GetRange (minmax); scalars->GetRange (minmax.val);
// If this is the new point cloud, a new actor is created // If this is the new point cloud, a new actor is created
if (isAdd) if (exits)
{ {
vtkSmartPointer<vtkLODActor> actor; vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (polydata, actor); createActorFromVTKDataSet (polydata, actor);
actor->GetMapper ()->SetScalarRange (minmax); actor->GetMapper ()->SetScalarRange (minmax.val);
// Add it to all renderers // Add it to all renderers
renderer_->AddActor (actor); renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map // Save the pointer/ID pair to the global actor map
(*cloud_actor_map_)[id].actor = actor; (*cloud_actor_map_)[id].actor = actor;
(*cloud_actor_map_)[id].cells = initcells; (*cloud_actor_map_)[id].cells = initcells;
const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero (); const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity (); const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
// Save the viewpoint transformation matrix to the global actor map // Save the viewpoint transformation matrix to the global actor map
vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New(); vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
(*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
} }
else else
{ {
// Update the mapper // Update the mapper
reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata); reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
} }
} }

24
modules/viz/test/test_viz3d.cpp
View File

@ -88,20 +88,22 @@ TEST(Viz_viz3d, accuracy)
float pos_z = 0.0f; float pos_z = 0.0f;
temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply"); temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply");
v.addPolygonMesh(*mesh, "pq"); v.addPolygonMesh(*mesh, "pq");
while(1)
while(1) //TODO implement and replace with !viz.wasStopped()
{ {
// Creating new point cloud with id cloud1 // Creating new point cloud with id cloud1
cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z)); cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z));
v.showPointCloud("cloud1", cloud, colors, cloudPosition); v.showPointCloud("cloud1", cloud, colors, cloudPosition);
angle_x += 0.1; angle_x += 0.1f;
angle_y -= 0.1; angle_y -= 0.1f;
angle_z += 0.1; angle_z += 0.1f;
pos_x = std::sin(angle_x); pos_x = std::sin(angle_x);
pos_y = std::sin(angle_x); pos_y = std::sin(angle_x);
pos_z = std::sin(angle_x); pos_z = std::sin(angle_x);
v.spinOnce(1,true); v.spinOnce(10);
} }
// cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0)); // cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));