659 lines
25 KiB
C++
659 lines
25 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
//
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
// If you do not agree to this license, do not download, install,
|
|
// copy or use the software.
|
|
//
|
|
//
|
|
// License Agreement
|
|
// For Open Source Computer Vision Library
|
|
//
|
|
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
|
|
// Third party copyrights are property of their respective owners.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
// are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistribution's of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
//
|
|
// * Redistribution's in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
//
|
|
// * The name of the copyright holders may not be used to endorse or promote products
|
|
// derived from this software without specific prior written permission.
|
|
//
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
//
|
|
// Authors:
|
|
// * Ozan Tonkal, ozantonkal@gmail.com
|
|
// * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com
|
|
//
|
|
// OpenCV Viz module is complete rewrite of
|
|
// PCL visualization module (www.pointclouds.org)
|
|
//
|
|
//M*/
|
|
|
|
#include "precomp.hpp"
|
|
|
|
namespace cv
|
|
{
|
|
namespace viz
|
|
{
|
|
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
|
|
}
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
/// Point Cloud Widget implementation
|
|
|
|
cv::viz::WCloud::WCloud(InputArray _cloud, InputArray _colors)
|
|
{
|
|
Mat cloud = _cloud.getMat();
|
|
Mat colors = _colors.getMat();
|
|
|
|
if (cloud.isContinuous() && colors.isContinuous())
|
|
{
|
|
cloud = cloud.reshape(cloud.channels(), 1);
|
|
colors = colors.reshape(colors.channels(), 1);
|
|
}
|
|
|
|
vtkSmartPointer<vtkCloudColorMatSource> cloud_source = vtkSmartPointer<vtkCloudColorMatSource>::New();
|
|
cloud_source->SetCloud(cloud);
|
|
cloud_source->SetColors(colors, cloud);
|
|
|
|
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
|
|
mapper->SetInputConnection(cloud_source->GetOutputPort());
|
|
mapper->SetScalarModeToUsePointData();
|
|
mapper->ImmediateModeRenderingOff();
|
|
mapper->SetScalarRange(0, 255);
|
|
mapper->ScalarVisibilityOn();
|
|
|
|
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
|
|
actor->GetProperty()->SetInterpolationToFlat();
|
|
actor->GetProperty()->BackfaceCullingOn();
|
|
actor->SetMapper(mapper);
|
|
|
|
WidgetAccessor::setProp(*this, actor);
|
|
}
|
|
|
|
cv::viz::WCloud::WCloud(InputArray _cloud, const Color &color)
|
|
{
|
|
Mat cloud = _cloud.getMat();
|
|
|
|
vtkSmartPointer<vtkCloudMatSource> cloud_source = vtkSmartPointer<vtkCloudMatSource>::New();
|
|
cloud_source->SetCloud(cloud);
|
|
|
|
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
|
|
mapper->SetInputConnection(cloud_source->GetOutputPort());
|
|
mapper->ImmediateModeRenderingOff();
|
|
mapper->ScalarVisibilityOff();
|
|
|
|
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
|
|
actor->GetProperty()->SetInterpolationToFlat();
|
|
actor->GetProperty()->BackfaceCullingOn();
|
|
actor->SetMapper(mapper);
|
|
|
|
WidgetAccessor::setProp(*this, actor);
|
|
setColor(color);
|
|
}
|
|
|
|
template<> cv::viz::WCloud cv::viz::Widget::cast<cv::viz::WCloud>()
|
|
{
|
|
Widget3D widget = this->cast<Widget3D>();
|
|
return static_cast<WCloud&>(widget);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
/// Cloud Collection Widget implementation
|
|
|
|
namespace cv { namespace viz { namespace
|
|
{
|
|
struct CloudCollectionUtils
|
|
{
|
|
static inline vtkSmartPointer<vtkPolyData> create(const Mat &cloud, vtkIdType &nr_points)
|
|
{
|
|
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
|
|
vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
|
|
|
|
polydata->SetVerts(vertices);
|
|
|
|
vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
|
|
vtkSmartPointer<vtkIdTypeArray> initcells;
|
|
nr_points = cloud.total();
|
|
|
|
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);
|
|
|
|
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);
|
|
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);
|
|
nr_points = data_end - data_beg;
|
|
}
|
|
points->SetNumberOfPoints(nr_points);
|
|
|
|
// Update cells
|
|
vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData();
|
|
// If no init cells and cells has not been initialized...
|
|
if (!cells)
|
|
cells = vtkSmartPointer<vtkIdTypeArray>::New();
|
|
|
|
// If we have less values then we need to recreate the array
|
|
if (cells->GetNumberOfTuples() < nr_points)
|
|
{
|
|
cells = vtkSmartPointer<vtkIdTypeArray>::New();
|
|
|
|
// If init cells is given, and there's enough data in it, use it
|
|
if (initcells && initcells->GetNumberOfTuples() >= nr_points)
|
|
{
|
|
cells->DeepCopy(initcells);
|
|
cells->SetNumberOfComponents(2);
|
|
cells->SetNumberOfTuples(nr_points);
|
|
}
|
|
else
|
|
{
|
|
// If the number of tuples is still too small, we need to recreate the array
|
|
cells->SetNumberOfComponents(2);
|
|
cells->SetNumberOfTuples(nr_points);
|
|
vtkIdType *cell = cells->GetPointer(0);
|
|
// Fill it with 1s
|
|
std::fill(cell, cell + nr_points * 2, 1);
|
|
cell++;
|
|
for (vtkIdType i = 0; i < nr_points; ++i, cell += 2)
|
|
*cell = i;
|
|
// Save the results in initcells
|
|
initcells = vtkSmartPointer<vtkIdTypeArray>::New();
|
|
initcells->DeepCopy(cells);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// The assumption here is that the current set of cells has more data than needed
|
|
cells->SetNumberOfComponents(2);
|
|
cells->SetNumberOfTuples(nr_points);
|
|
}
|
|
|
|
// Set the cells and the vertices
|
|
vertices->SetCells(nr_points, cells);
|
|
return polydata;
|
|
}
|
|
|
|
static void createMapper(vtkSmartPointer<vtkLODActor> actor, vtkSmartPointer<vtkPolyData> poly_data)
|
|
{
|
|
vtkDataSetMapper *mapper = vtkDataSetMapper::SafeDownCast(actor->GetMapper());
|
|
if (!mapper)
|
|
{
|
|
// This is the first cloud
|
|
vtkSmartPointer<vtkDataSetMapper> mapper_new = vtkSmartPointer<vtkDataSetMapper>::New();
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
mapper_new->SetInputConnection(poly_data->GetProducerPort());
|
|
#else
|
|
mapper_new->SetInputData(poly_data);
|
|
#endif
|
|
|
|
mapper_new->SetScalarRange(0, 255);
|
|
mapper_new->SetScalarModeToUsePointData();
|
|
|
|
bool interpolation = (poly_data && poly_data->GetNumberOfCells() != poly_data->GetNumberOfVerts());
|
|
|
|
mapper_new->SetInterpolateScalarsBeforeMapping(interpolation);
|
|
mapper_new->ScalarVisibilityOn();
|
|
mapper_new->ImmediateModeRenderingOff();
|
|
|
|
actor->SetNumberOfCloudPoints(int(std::max<vtkIdType>(1, poly_data->GetNumberOfPoints() / 10)));
|
|
actor->GetProperty()->SetInterpolationToFlat();
|
|
actor->GetProperty()->BackfaceCullingOn();
|
|
actor->SetMapper(mapper_new);
|
|
return ;
|
|
}
|
|
|
|
vtkPolyData *data = vtkPolyData::SafeDownCast(mapper->GetInput());
|
|
CV_Assert("Cloud Widget without data" && data);
|
|
|
|
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
appendFilter->AddInputConnection(mapper->GetInput()->GetProducerPort());
|
|
appendFilter->AddInputConnection(poly_data->GetProducerPort());
|
|
#else
|
|
appendFilter->AddInputData(data);
|
|
appendFilter->AddInputData(poly_data);
|
|
#endif
|
|
mapper->SetInputConnection(appendFilter->GetOutputPort());
|
|
|
|
// Update the number of cloud points
|
|
vtkIdType old_cloud_points = actor->GetNumberOfCloudPoints();
|
|
actor->SetNumberOfCloudPoints(int(std::max<vtkIdType>(1, old_cloud_points+poly_data->GetNumberOfPoints() / 10)));
|
|
}
|
|
};
|
|
}}}
|
|
|
|
cv::viz::WCloudCollection::WCloudCollection()
|
|
{
|
|
// Just create the actor
|
|
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
|
|
WidgetAccessor::setProp(*this, actor);
|
|
}
|
|
|
|
void cv::viz::WCloudCollection::addCloud(InputArray _cloud, InputArray _colors, const Affine3f &pose)
|
|
{
|
|
Mat cloud = _cloud.getMat();
|
|
Mat colors = _colors.getMat();
|
|
CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4);
|
|
CV_Assert(colors.depth() == CV_8U && cloud.size() == colors.size());
|
|
|
|
if (cloud.isContinuous() && colors.isContinuous())
|
|
{
|
|
cloud = cloud.reshape(cloud.channels(), 1);
|
|
colors = colors.reshape(colors.channels(), 1);
|
|
}
|
|
|
|
vtkIdType nr_points;
|
|
vtkSmartPointer<vtkPolyData> polydata = CloudCollectionUtils::create(cloud, nr_points);
|
|
|
|
// Filter colors
|
|
Vec3b* colors_data = new Vec3b[nr_points];
|
|
NanFilter::copyColor(colors, colors_data, cloud);
|
|
|
|
vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
|
|
scalars->SetNumberOfComponents(3);
|
|
scalars->SetNumberOfTuples(nr_points);
|
|
scalars->SetArray(colors_data->val, 3 * nr_points, 0);
|
|
|
|
// Assign the colors
|
|
polydata->GetPointData()->SetScalars(scalars);
|
|
|
|
// Transform the poly data based on the pose
|
|
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
|
|
transform->PreMultiply();
|
|
transform->SetMatrix(convertToVtkMatrix(pose.matrix));
|
|
|
|
vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
|
|
transform_filter->SetTransform(transform);
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
transform_filter->SetInputConnection(polydata->GetProducerPort());
|
|
#else
|
|
transform_filter->SetInputData(polydata);
|
|
#endif
|
|
transform_filter->Update();
|
|
|
|
vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this));
|
|
CV_Assert("Incompatible widget type." && actor);
|
|
|
|
CloudCollectionUtils::createMapper(actor, transform_filter->GetOutput());
|
|
|
|
}
|
|
|
|
void cv::viz::WCloudCollection::addCloud(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);
|
|
|
|
vtkIdType nr_points;
|
|
vtkSmartPointer<vtkPolyData> polydata = CloudCollectionUtils::create(cloud, nr_points);
|
|
|
|
vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
|
|
scalars->SetNumberOfComponents(3);
|
|
scalars->SetNumberOfTuples(nr_points);
|
|
scalars->FillComponent(0, color[2]);
|
|
scalars->FillComponent(1, color[1]);
|
|
scalars->FillComponent(2, color[0]);
|
|
|
|
// Assign the colors
|
|
polydata->GetPointData()->SetScalars(scalars);
|
|
|
|
// Transform the poly data based on the pose
|
|
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
|
|
transform->PreMultiply();
|
|
transform->SetMatrix(convertToVtkMatrix(pose.matrix));
|
|
|
|
vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
|
|
transform_filter->SetTransform(transform);
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
transform_filter->SetInputConnection(polydata->GetProducerPort());
|
|
#else
|
|
transform_filter->SetInputData(polydata);
|
|
#endif
|
|
transform_filter->Update();
|
|
|
|
vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this));
|
|
CV_Assert("Incompatible widget type." && actor);
|
|
|
|
CloudCollectionUtils::createMapper(actor, transform_filter->GetOutput());
|
|
}
|
|
|
|
template<> cv::viz::WCloudCollection cv::viz::Widget::cast<cv::viz::WCloudCollection>()
|
|
{
|
|
Widget3D widget = this->cast<Widget3D>();
|
|
return static_cast<WCloudCollection&>(widget);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
/// Cloud Normals Widget implementation
|
|
|
|
namespace cv { namespace viz { namespace
|
|
{
|
|
struct CloudNormalsUtils
|
|
{
|
|
template<typename _Tp>
|
|
struct Impl
|
|
{
|
|
static vtkSmartPointer<vtkCellArray> applyOrganized(const Mat &cloud, const Mat& normals, double level, float scale, _Tp *&pts, vtkIdType &nr_normals)
|
|
{
|
|
vtkIdType point_step = static_cast<vtkIdType>(std::sqrt(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 Mat &cloud, const 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 Mat &cloud, const Mat& normals, int level, float scale, _Tp *&pts, vtkIdType &nr_normals)
|
|
{
|
|
if (cloud.cols > 1 && cloud.rows > 1)
|
|
return CloudNormalsUtils::Impl<_Tp>::applyOrganized(cloud, normals, level, scale, pts, nr_normals);
|
|
else
|
|
return CloudNormalsUtils::Impl<_Tp>::applyUnorganized(cloud, normals, level, scale, pts, nr_normals);
|
|
}
|
|
};
|
|
|
|
}}}
|
|
|
|
cv::viz::WCloudNormals::WCloudNormals(InputArray _cloud, InputArray _normals, int level, float scale, const Color &color)
|
|
{
|
|
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 = CloudNormalsUtils::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 = CloudNormalsUtils::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();
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
mapper->SetInput(polyData);
|
|
#else
|
|
mapper->SetInputData(polyData);
|
|
#endif
|
|
mapper->SetColorModeToMapScalars();
|
|
mapper->SetScalarModeToUsePointData();
|
|
|
|
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
|
|
actor->SetMapper(mapper);
|
|
WidgetAccessor::setProp(*this, actor);
|
|
setColor(color);
|
|
}
|
|
|
|
template<> cv::viz::WCloudNormals cv::viz::Widget::cast<cv::viz::WCloudNormals>()
|
|
{
|
|
Widget3D widget = this->cast<Widget3D>();
|
|
return static_cast<WCloudNormals&>(widget);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
/// Mesh Widget implementation
|
|
|
|
namespace cv { namespace viz { namespace
|
|
{
|
|
struct MeshUtils
|
|
{
|
|
template<typename _Tp>
|
|
static Vec<_Tp, 3> * copy(const Mat &source, Vec<_Tp, 3> *output, int *look_up, const Mat &nan_mask)
|
|
{
|
|
CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
|
|
CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
|
|
CV_DbgAssert(DataDepth<_Tp>::value == nan_mask.depth());
|
|
|
|
int s_chs = source.channels();
|
|
int m_chs = nan_mask.channels();
|
|
|
|
int index = 0;
|
|
const _Tp* srow = source.ptr<_Tp>(0);
|
|
const _Tp* mrow = nan_mask.ptr<_Tp>(0);
|
|
|
|
for (int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
|
|
{
|
|
if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
|
|
{
|
|
look_up[x] = index;
|
|
*output++ = Vec<_Tp, 3>(srow);
|
|
++index;
|
|
}
|
|
}
|
|
return output;
|
|
}
|
|
};
|
|
}}}
|
|
|
|
cv::viz::WMesh::WMesh(const Mesh3d &mesh)
|
|
{
|
|
CV_Assert(mesh.cloud.rows == 1 && (mesh.cloud.type() == CV_32FC3 || mesh.cloud.type() == CV_64FC3 || mesh.cloud.type() == CV_32FC4 || mesh.cloud.type() == CV_64FC4));
|
|
CV_Assert(mesh.colors.empty() || (mesh.colors.depth() == CV_8U && mesh.cloud.size() == mesh.colors.size()));
|
|
CV_Assert(!mesh.polygons.empty() && mesh.polygons.type() == CV_32SC1);
|
|
|
|
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
|
|
vtkIdType nr_points = mesh.cloud.total();
|
|
Mat look_up_mat(1, nr_points, CV_32SC1);
|
|
int * look_up = look_up_mat.ptr<int>();
|
|
points->SetNumberOfPoints(nr_points);
|
|
|
|
// Copy data from cloud to vtkPoints
|
|
if (mesh.cloud.depth() == CV_32F)
|
|
{
|
|
points->SetDataTypeToFloat();
|
|
Vec3f *data_beg = vtkpoints_data<float>(points);
|
|
Vec3f *data_end = MeshUtils::copy(mesh.cloud, data_beg, look_up, mesh.cloud);
|
|
nr_points = data_end - data_beg;
|
|
}
|
|
else
|
|
{
|
|
points->SetDataTypeToDouble();
|
|
Vec3d *data_beg = vtkpoints_data<double>(points);
|
|
Vec3d *data_end = MeshUtils::copy(mesh.cloud, data_beg, look_up, mesh.cloud);
|
|
nr_points = data_end - data_beg;
|
|
}
|
|
|
|
vtkSmartPointer<vtkUnsignedCharArray> scalars;
|
|
|
|
if (!mesh.colors.empty())
|
|
{
|
|
Vec3b *colors_data = new Vec3b[nr_points];
|
|
NanFilter::copyColor(mesh.colors, colors_data, mesh.cloud);
|
|
|
|
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
|
|
scalars->SetNumberOfComponents(3);
|
|
scalars->SetNumberOfTuples(nr_points);
|
|
scalars->SetArray(colors_data->val, 3 * nr_points, 0);
|
|
}
|
|
|
|
points->SetNumberOfPoints(nr_points);
|
|
|
|
vtkSmartPointer<vtkPointSet> data;
|
|
|
|
if (mesh.polygons.size().area() > 1)
|
|
{
|
|
vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New();
|
|
const int * polygons = mesh.polygons.ptr<int>();
|
|
|
|
int idx = 0;
|
|
int poly_size = mesh.polygons.total();
|
|
for (int i = 0; i < poly_size; ++idx)
|
|
{
|
|
int n_points = polygons[i++];
|
|
|
|
cell_array->InsertNextCell(n_points);
|
|
for (int j = 0; j < n_points; ++j, ++idx)
|
|
cell_array->InsertCellPoint(look_up[polygons[i++]]);
|
|
}
|
|
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
|
|
cell_array->GetData()->SetNumberOfValues(idx);
|
|
cell_array->Squeeze();
|
|
polydata->SetStrips(cell_array);
|
|
polydata->SetPoints(points);
|
|
|
|
if (scalars)
|
|
polydata->GetPointData()->SetScalars(scalars);
|
|
|
|
data = polydata;
|
|
}
|
|
else
|
|
{
|
|
// Only one polygon
|
|
vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New();
|
|
const int * polygons = mesh.polygons.ptr<int>();
|
|
int n_points = polygons[0];
|
|
|
|
polygon->GetPointIds()->SetNumberOfIds(n_points);
|
|
|
|
for (int j = 1; j < n_points+1; ++j)
|
|
polygon->GetPointIds()->SetId(j, look_up[polygons[j]]);
|
|
|
|
vtkSmartPointer<vtkUnstructuredGrid> poly_grid = vtkSmartPointer<vtkUnstructuredGrid>::New();
|
|
poly_grid->Allocate(1, 1);
|
|
poly_grid->InsertNextCell(polygon->GetCellType(), polygon->GetPointIds());
|
|
poly_grid->SetPoints(points);
|
|
|
|
if (scalars)
|
|
poly_grid->GetPointData()->SetScalars(scalars);
|
|
|
|
data = poly_grid;
|
|
}
|
|
|
|
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
|
|
|
|
actor->GetProperty()->SetRepresentationToSurface();
|
|
actor->GetProperty()->BackfaceCullingOff(); // Backface culling is off for higher efficiency
|
|
actor->GetProperty()->SetInterpolationToFlat();
|
|
actor->GetProperty()->EdgeVisibilityOff();
|
|
actor->GetProperty()->ShadingOff();
|
|
|
|
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
|
|
#if VTK_MAJOR_VERSION <= 5
|
|
mapper->SetInput(data);
|
|
#else
|
|
mapper->SetInputData(data);
|
|
#endif
|
|
mapper->ImmediateModeRenderingOff();
|
|
|
|
vtkIdType numberOfCloudPoints = nr_points * 0.1;
|
|
actor->SetNumberOfCloudPoints(int(numberOfCloudPoints > 1 ? numberOfCloudPoints : 1));
|
|
actor->SetMapper(mapper);
|
|
|
|
WidgetAccessor::setProp(*this, actor);
|
|
}
|
|
|
|
template<> CV_EXPORTS cv::viz::WMesh cv::viz::Widget::cast<cv::viz::WMesh>()
|
|
{
|
|
Widget3D widget = this->cast<Widget3D>();
|
|
return static_cast<WMesh&>(widget);
|
|
}
|