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

Merge pull request #5 from ozantonkal/implementing_addPointCloud

Browse Source 
Implementing add point cloud
This commit is contained in:
Anatoly Baksheev 2013-06-10 00:36:49 -07:00
commit 01e99db675
5 changed files with 74 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
modules/viz/include/opencv2/viz/types.hpp
View File

@ -16,14 +16,17 @@ namespace temp_viz
typedef cv::Vec4d Vec4d; typedef cv::Vec4d Vec4d;
typedef cv::Vec2d Vec2d; typedef cv::Vec2d Vec2d;
typedef cv::Vec2i Vec2i; typedef cv::Vec2i Vec2i;
typedef cv::Vec3b Vec3b;
typedef cv::Matx33d Matx33d; typedef cv::Matx33d Matx33d;
typedef cv::Affine3f Affine3f; typedef cv::Affine3f Affine3f;
typedef cv::Affine3d Affine3d; typedef cv::Affine3d Affine3d;
typedef cv::Point3f Point3f; typedef cv::Point3f Point3f;
typedef cv::Point3d Point3d;
typedef cv::Matx44d Matx44d; typedef cv::Matx44d Matx44d;
typedef cv::Matx44f Matx44f; typedef cv::Matx44f Matx44f;
typedef cv::Size Size; typedef cv::Size Size;
typedef cv::Point Point; typedef cv::Point Point;
typedef cv::InputArray InputArray;

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

@ -27,7 +27,7 @@ namespace temp_viz
void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate"); void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate");
void showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose = cv::Affine3f::Identity()); void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity());
bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud"); bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String &id = "cloud");

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

@ -103,7 +103,7 @@ public:
* \param[in] colors color input in the same order of the points or single uniform color * \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 * \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()); void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity());
bool addPolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon"); 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"); bool updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id = "polygon");

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

@ -23,7 +23,7 @@ void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const
impl_->addCoordinateSystem(scale, t, id); impl_->addCoordinateSystem(scale, t, id);
} }
void temp_viz::Viz3d::showPointCloud(const std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) void temp_viz::Viz3d::showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose)
{ {
impl_->showPointCloud(id, cloud, colors, pose); impl_->showPointCloud(id, cloud, colors, pose);
} }

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

@ -28,11 +28,11 @@ 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 std::string& id, cv::InputArray cloud, cv::InputArray colors, const cv::Affine3f& pose) void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose)
{ {
cv::Mat cloudMat = cloud.getMat(); Mat cloudMat = cloud.getMat();
cv::Mat colorsMat = colors.getMat(); Mat colorsMat = colors.getMat();
CV_Assert(cloudMat.type() == CV_32FC3 && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size()); CV_Assert((cloudMat.type() == CV_32FC3 || cloudMat.type() == CV_64FC3) && colorsMat.type() == CV_8UC3 && cloudMat.size() == colorsMat.size());
vtkSmartPointer<vtkPolyData> polydata; vtkSmartPointer<vtkPolyData> polydata;
vtkSmartPointer<vtkCellArray> vertices; vtkSmartPointer<vtkCellArray> vertices;
@ -57,7 +57,10 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr
if (!points) if (!points)
{ {
points = vtkSmartPointer<vtkPoints>::New (); points = vtkSmartPointer<vtkPoints>::New ();
points->SetDataTypeToFloat (); if (cloudMat.type() == CV_32FC3)
points->SetDataTypeToFloat ();
else if (cloudMat.type() == CV_64FC3)
points->SetDataTypeToDouble ();
polydata->SetPoints (points); polydata->SetPoints (points);
} }
points->SetNumberOfPoints (nr_points); points->SetNumberOfPoints (nr_points);
@ -69,30 +72,54 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr
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
if (cloudMat.type() == CV_32FC3)
points->SetDataTypeToFloat ();
else if (cloudMat.type() == CV_64FC3)
points->SetDataTypeToDouble ();
// Copy the new point array in // Copy the new point array in
nr_points = cloudMat.size().area(); nr_points = cloudMat.size().area();
points->SetNumberOfPoints (nr_points); 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; int j = 0;
if (cloudMat.type() == CV_32FC3)
// If a point is NaN, ignore it
for(int y = 0; y < cloudMat.rows; ++y)
{ {
const cv::Point3f* crow = cloudMat.ptr<cv::Point3f>(y); // Get a pointer to the beginning of the data array
for(int x = 0; x < cloudMat.cols; ++x) float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
if (cvIsNaN(crow[x].x) != 1 && cvIsNaN(crow[x].y) != 1 && cvIsNaN(crow[x].z) != 1)
{ // Scan through the data and apply mask where point is NAN
// Points are transformed based on pose parameter for(int y = 0; y < cloudMat.rows; ++y)
cv::Point3f transformed_point = pose * crow[x]; {
memcpy (&data[j++ * 3], &transformed_point, sizeof(cv::Point3f)); 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));
}
}
} }
else if (cloudMat.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));
}
}
}
nr_points = j; nr_points = j;
points->SetNumberOfPoints (nr_points); points->SetNumberOfPoints (nr_points);
@ -116,13 +143,27 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr
unsigned char* colors_data = new unsigned char[nr_points * 3]; unsigned char* colors_data = new unsigned char[nr_points * 3];
j = 0; j = 0;
for(int y = 0; y < colorsMat.rows; ++y) if (cloudMat.type() == CV_32FC3)
{ {
const cv::Vec3b* crow = colorsMat.ptr<cv::Vec3b>(y); for(int y = 0; y < colorsMat.rows; ++y)
const cv::Point3f* cloud_row = cloudMat.ptr<cv::Point3f>(y); {
for(int x = 0; x < colorsMat.cols; ++x) const Vec3b* crow = colorsMat.ptr<Vec3b>(y);
if (cvIsNaN(cloud_row[x].x) != 1 && cvIsNaN(cloud_row[x].y) != 1 && cvIsNaN(cloud_row[x].z) != 1) const Point3f* cloud_row = cloudMat.ptr<Point3f>(y);
memcpy (&colors_data[j++ * 3], &crow[x], sizeof(cv::Vec3b)); 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));
}
}
else if (cloudMat.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));
}
} }
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0); reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors_data, 3 * nr_points, 0);
@ -152,9 +193,6 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const std::string& id, cv::InputAr
// 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);
// convertToVtkMatrix (pose.matrix, transformation);
std::cout << transformation->GetElement(0,3) << endl;
(*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
} }