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

Merge pull request #4 from ozantonkal/implementing_addPointCloud

Browse Source 
showPointCloud initial implementation together with test
This commit is contained in:
Anatoly Baksheev 2013-06-07 02:50:42 -07:00
commit 59acccfa6a
5 changed files with 195 additions and 301 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
modules/viz/include/opencv2/viz/viz3d.hpp
View File

@ -26,22 +26,15 @@ namespace temp_viz
void setBackgroundColor(const Color& color = Color::black());
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");

10
modules/viz/src/q/viz3d_impl.hpp
View File

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

9
modules/viz/src/viz3d.cpp
View File

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

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

@ -27,207 +27,86 @@ 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)
{
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()));
void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose)
{
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;
allocVtkPolyData(polydata);
//polydata = vtkSmartPointer<vtkPolyData>::New ();
vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New ();
polydata->SetVerts (vertices);
vtkSmartPointer<vtkCellArray> vertices;
vtkSmartPointer<vtkPoints> points;
vtkSmartPointer<vtkIdTypeArray> initcells;
vtkIdType nr_points = cloud.size().area();
vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
if (!points)
{
points = vtkSmartPointer<vtkPoints>::New ();
points->SetDataTypeToFloat ();
polydata->SetPoints (points);
}
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, initcells, 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);
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));
}
}
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);
}
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (polydata, actor);
if (has_colors)
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;
}
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?)
vtkIdType nr_points;
// If the cloud already exists, update otherwise create new one
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())
bool isAdd = (am_it == cloud_actor_map_->end());
if (isAdd)
{
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));
}
// 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 ();
points->SetDataTypeToFloat ();
polydata->SetPoints (points);
}
points->SetNumberOfPoints (nr_points);
}
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);
// 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);
// Scan through the data and apply mask where point is NAN
int j = 0;
// If a point is NaN, ignore it
for(int y = 0; y < cloudMat.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)
{
// 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, am_it->second.cells, 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);
#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);
@ -235,60 +114,57 @@ bool temp_viz::Viz3d::VizImpl::updatePointCloud (const cv::Mat& cloud, const cv:
// Get a random color
unsigned char* colors_data = new unsigned char[nr_points * 3];
if (mask.empty())
j = 0;
for(int y = 0; y < colorsMat.rows; ++y)
{
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));
}
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));
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
// Assign the colors
polydata->GetPointData ()->SetScalars (scalars);
scalars->GetRange (minmax);
// If this is the new point cloud, a new actor is created
if (isAdd)
{
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);
// convertToVtkMatrix (pose.matrix, transformation);
std::cout << transformation->GetElement(0,3) << endl;
(*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));
}
// Update the mapper
reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
}
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);

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

@ -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();
}