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

cloud normals widget implementation

Browse Source
This commit is contained in:
ozantonkal 2013-07-08 18:22:51 +02:00
parent 6e5ae98582
commit e76023be96
3 changed files with 159 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
modules/viz/include/opencv2/viz/widgets.hpp
View File

@ -104,4 +104,12 @@ namespace temp_viz
CloudWidget(InputArray _cloud, InputArray _colors);
CloudWidget(InputArray _cloud, const Color &color = Color::white());
};
class CV_EXPORTS CloudNormalsWidget : public Widget
{
public:
CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f);
private:
struct ApplyCloudNormals;
};
}

133
modules/viz/src/simple_widgets.cpp
View File

@ -524,12 +524,143 @@ temp_viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color)
bool interpolation = (polydata && polydata->GetNumberOfCells () != polydata->GetNumberOfVerts ());
mapper->SetInterpolateScalarsBeforeMapping (interpolation);
mapper->ScalarVisibilityOn ();
mapper->ScalarVisibilityOff ();
mapper->ImmediateModeRenderingOff ();
actor->SetNumberOfCloudPoints (int (std::max<vtkIdType> (1, polydata->GetNumberOfPoints () / 10)));
actor->GetProperty ()->SetInterpolationToFlat ();
actor->GetProperty ()->BackfaceCullingOn ();
actor->GetProperty ()->SetColor(c.val);
actor->SetMapper (mapper);
}
///////////////////////////////////////////////////////////////////////////////////////////////
/// cloud normals widget implementation
struct temp_viz::CloudNormalsWidget::ApplyCloudNormals
{
template<typename _Tp>
struct Impl
{
static vtkSmartPointer<vtkCellArray> applyOrganized(const cv::Mat &cloud, const cv::Mat& normals,
int level, float scale, _Tp *&pts, vtkIdType &nr_normals)
{
vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
nr_normals = (static_cast<vtkIdType> ((cloud.cols - 1)/ point_step) + 1) *
(static_cast<vtkIdType> ((cloud.rows - 1) / point_step) + 1);
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
pts = new _Tp[2 * nr_normals * 3];
int cch = cloud.channels();
vtkIdType cell_count = 0;
for (vtkIdType y = 0; y < cloud.rows; y += point_step)
{
const _Tp *prow = cloud.ptr<_Tp>(y);
const _Tp *nrow = normals.ptr<_Tp>(y);
for (vtkIdType x = 0; x < cloud.cols; x += point_step + cch)
{
pts[2 * cell_count * 3 + 0] = prow[x];
pts[2 * cell_count * 3 + 1] = prow[x+1];
pts[2 * cell_count * 3 + 2] = prow[x+2];
pts[2 * cell_count * 3 + 3] = prow[x] + nrow[x] * scale;
pts[2 * cell_count * 3 + 4] = prow[x+1] + nrow[x+1] * scale;
pts[2 * cell_count * 3 + 5] = prow[x+2] + nrow[x+2] * scale;
lines->InsertNextCell (2);
lines->InsertCellPoint (2 * cell_count);
lines->InsertCellPoint (2 * cell_count + 1);
cell_count++;
}
}
return lines;
}
static vtkSmartPointer<vtkCellArray> applyUnorganized(const cv::Mat &cloud, const cv::Mat& normals,
int level, float scale, _Tp *&pts, vtkIdType &nr_normals)
{
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
nr_normals = (cloud.size().area() - 1) / level + 1 ;
pts = new _Tp[2 * nr_normals * 3];
int cch = cloud.channels();
const _Tp *p = cloud.ptr<_Tp>();
const _Tp *n = normals.ptr<_Tp>();
for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level * cch)
{
pts[2 * j * 3 + 0] = p[i];
pts[2 * j * 3 + 1] = p[i+1];
pts[2 * j * 3 + 2] = p[i+2];
pts[2 * j * 3 + 3] = p[i] + n[i] * scale;
pts[2 * j * 3 + 4] = p[i+1] + n[i+1] * scale;
pts[2 * j * 3 + 5] = p[i+2] + n[i+2] * scale;
lines->InsertNextCell (2);
lines->InsertCellPoint (2 * j);
lines->InsertCellPoint (2 * j + 1);
}
return lines;
}
};
template<typename _Tp>
static inline vtkSmartPointer<vtkCellArray> apply(const cv::Mat &cloud, const cv::Mat& normals,
int level, float scale, _Tp *&pts, vtkIdType &nr_normals)
{
if (cloud.cols > 1 && cloud.rows > 1)
return ApplyCloudNormals::Impl<_Tp>::applyOrganized(cloud, normals, level, scale, pts, nr_normals);
else
return ApplyCloudNormals::Impl<_Tp>::applyUnorganized(cloud, normals, level, scale, pts, nr_normals);
}
};
temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale)
{
Mat cloud = _cloud.getMat();
Mat normals = _normals.getMat();
CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
CV_Assert(cloud.size() == normals.size() && cloud.type() == normals.type());
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
vtkIdType nr_normals = 0;
if (cloud.depth() == CV_32F)
{
points->SetDataTypeToFloat();
vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
data->SetNumberOfComponents (3);
float* pts = 0;
lines = ApplyCloudNormals::apply(cloud, normals, level, scale, pts, nr_normals);
data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
points->SetData (data);
}
else
{
points->SetDataTypeToDouble();
vtkSmartPointer<vtkDoubleArray> data = vtkSmartPointer<vtkDoubleArray>::New ();
data->SetNumberOfComponents (3);
double* pts = 0;
lines = ApplyCloudNormals::apply(cloud, normals, level, scale, pts, nr_normals);
data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
points->SetData (data);
}
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
polyData->SetPoints (points);
polyData->SetLines (lines);
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
mapper->SetInput (polyData);
mapper->SetColorModeToMapScalars();
mapper->SetScalarModeToUsePointData();
vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this));
actor->SetMapper(mapper);
}

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

@ -52,16 +52,18 @@
cv::Mat cvcloud_load()
{
cv::Mat cloud(1, 20000, CV_32FC3);
cv::Mat cloud(1, 20000, CV_64FC4);
std::ifstream ifs("cloud_dragon.ply");
std::string str;
for(size_t i = 0; i < 11; ++i)
std::getline(ifs, str);
cv::Point3f* data = cloud.ptr<cv::Point3f>();
for(size_t i = 0; i < 20000; ++i)
ifs >> data[i].x >> data[i].y >> data[i].z;
cv::Vec4d* data = cloud.ptr<cv::Vec4d>();
for(size_t i = 0; i < 20000; ++i){
ifs >> data[i][0] >> data[i][1] >> data[i][2];
data[i][3] = 1.0;
}
return cloud;
}
@ -100,7 +102,7 @@ TEST(Viz_viz3d, accuracy)
temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity());
temp_viz::TextWidget tw("TEST", cv::Point2i(100,100), 20);
temp_viz::CloudWidget pcw(cloud, colors);
temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(255,255,255));
temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(0,255,255));
// v.showWidget("line", lw);
// v.showWidget("plane", pw);
@ -111,12 +113,18 @@ TEST(Viz_viz3d, accuracy)
// v.showWidget("cube", cuw);
v.showWidget("coordinateSystem", csw);
// v.showWidget("text",tw);
v.showWidget("pcw",pcw);
// v.showWidget("pcw",pcw);
v.showWidget("pcw2",pcw2);
temp_viz::LineWidget lw2 = lw;
// v.showPointCloud("cld",cloud, colors);
cv::Mat normals(cloud.size(), cloud.type(), cv::Scalar(0, 10, 0));
// v.addPointCloudNormals(cloud, normals, 100, 0.02, "n");
temp_viz::CloudNormalsWidget cnw(cloud, normals);
v.showWidget("n", cnw);
while(!v.wasStopped())
{
// Creating new point cloud with id cloud1
@ -135,8 +143,9 @@ TEST(Viz_viz3d, accuracy)
cyw.setPose(cloudPosition);
lw.setPose(cloudPosition);
cuw.setPose(cloudPosition);
v.showWidget("pcw",pcw, cloudPosition);
v.showWidget("pcw2",pcw2, cloudPosition2);
// cnw.setPose(cloudPosition);
// v.showWidget("pcw",pcw, cloudPosition);
// v.showWidget("pcw2",pcw2, cloudPosition2);
// v.showWidget("plane", pw, cloudPosition);
angle_x += 0.1f;
@ -152,9 +161,7 @@ TEST(Viz_viz3d, accuracy)
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;