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

Merge pull request #9 from ozantonkal/implementing_addPointCloud

Browse Source 
Implementing add point cloud
This commit is contained in:
Anatoly Baksheev 2013-06-12 13:23:08 -07:00
commit 7cfa3cbd8a
5 changed files with 144 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

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

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

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