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

mesh widget implementation

Browse Source
This commit is contained in:
ozantonkal 2013-07-15 12:02:20 +02:00
parent 6ca1822f36
commit b066d1982f
11 changed files with 649 additions and 479 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,22 +32,15 @@ namespace cv
static Color gray();
};
struct CV_EXPORTS Vertices
{
std::vector<unsigned int> vertices;
};
class CV_EXPORTS Mesh3d
{
public:
typedef Ptr<Mesh3d> Ptr;
Mat cloud, colors;
std::vector<Vertices> polygons;
static Mesh3d::Ptr mesh_load(const String& file);
typedef cv::Ptr<Mesh3d> Ptr;
Mat cloud, colors, polygons;
static cv::viz::Mesh3d::Ptr loadMesh(const String& file);
private:
struct loadMeshImpl;
};
class CV_EXPORTS KeyboardEvent
@ -100,8 +93,4 @@ namespace cv
};
} /* namespace viz */
} /* namespace cv */
} /* namespace cv */

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

@ -159,6 +159,15 @@ namespace cv
private:
struct ApplyCloudNormals;
};
class CV_EXPORTS MeshWidget : public Widget3D
{
public:
MeshWidget(const Mesh3d &mesh);
private:
struct CopyImpl;
};
template<> CV_EXPORTS Widget2D Widget::cast<Widget2D>();
template<> CV_EXPORTS Widget3D Widget::cast<Widget3D>();
@ -178,7 +187,4 @@ namespace cv
template<> CV_EXPORTS CloudNormalsWidget Widget::cast<CloudNormalsWidget>();
} /* namespace viz */
} /* namespace cv */
} /* namespace cv */

153
modules/viz/src/cloud_widgets.cpp
View File

@ -1,5 +1,12 @@
#include "precomp.hpp"
namespace cv
{
namespace viz
{
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////
/// Point Cloud Widget implementation
@ -311,3 +318,149 @@ template<> cv::viz::CloudNormalsWidget cv::viz::Widget::cast<cv::viz::CloudNorma
Widget3D widget = this->cast<Widget3D>();
return static_cast<CloudNormalsWidget&>(widget);
}
///////////////////////////////////////////////////////////////////////////////////////////////
/// Mesh Widget implementation
struct cv::viz::MeshWidget::CopyImpl
{
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<_Msk>::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::MeshWidget::MeshWidget(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.type() == CV_8UC3 && 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();
int * look_up = new int[nr_points];
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 = CopyImpl::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 = CopyImpl::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 = 0;
colors_data = new Vec3b[nr_points];
NanFilter::copy(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);
poly_grid->Update ();
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 ();
mapper->SetInput (data);
mapper->ImmediateModeRenderingOff ();
vtkIdType numberOfCloudPoints = nr_points * 0.1;
actor->SetNumberOfCloudPoints (int (numberOfCloudPoints > 1 ? numberOfCloudPoints : 1));
actor->SetMapper (mapper);
WidgetAccessor::setProp(*this, actor);
}

79
modules/viz/src/mesh_load.cpp
View File

@ -1,79 +0,0 @@
#include "precomp.hpp"
#include <vtkPLYReader.h>
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
cv::viz::Mesh3d::Ptr cv::viz::Mesh3d::mesh_load(const String& file)
{
Mesh3d::Ptr mesh = new Mesh3d();
vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New();
reader->SetFileName(file.c_str());
reader->Update();
vtkSmartPointer<vtkPolyData> poly_data = reader->GetOutput ();
typedef unsigned int uint32_t;
mesh->polygons.clear();
vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
mesh->cloud.create(1, nr_points, CV_32FC3);
double point_xyz[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
mesh_points->GetPoint (i, &point_xyz[0]);
mesh->cloud.ptr<cv::Point3f>()[i] = cv::Point3d(point_xyz[0], point_xyz[1], point_xyz[2]);;
}
// Then the color information, if any
vtkUnsignedCharArray* poly_colors = NULL;
if (poly_data->GetPointData() != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
// some applications do not save the name of scalars (including PCL's native vtk_io)
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
// TODO: currently only handles rgb values with 3 components
if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
{
mesh->colors.create(1, nr_points, CV_8UC3);
unsigned char point_color[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
poly_colors->GetTupleValue (i, &point_color[0]);
//RGB or BGR?????
mesh->colors.ptr<cv::Vec3b>()[i] = cv::Vec3b(point_color[0], point_color[1], point_color[2]);
}
}
else
mesh->colors.release();
// Now handle the polygons
mesh->polygons.resize (nr_polygons);
vtkIdType* cell_points;
vtkIdType nr_cell_points;
vtkCellArray * mesh_polygons = poly_data->GetPolys ();
mesh_polygons->InitTraversal ();
int id_poly = 0;
while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
{
mesh->polygons[id_poly].vertices.resize (nr_cell_points);
for (int i = 0; i < nr_cell_points; ++i)
mesh->polygons[id_poly].vertices[i] = static_cast<int> (cell_points[i]);
++id_poly;
}
return mesh;
}

24
modules/viz/src/precomp.hpp
View File

@ -156,27 +156,3 @@
#include <opencv2/core.hpp>
#include <opencv2/viz.hpp>
#include "opencv2/viz/widget_accessor.hpp"
namespace cv
{
namespace viz
{
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
template<> static inline Vec3f* vtkpoints_data<float>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_FLOAT);
vtkDataArray *data = points->GetData();
float *pointer = static_cast<vtkFloatArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3f*>(pointer);
}
template<> static inline Vec3d* vtkpoints_data<double>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_DOUBLE);
vtkDataArray *data = points->GetData();
double *pointer = static_cast<vtkDoubleArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3d*>(pointer);
}
}
}

10
modules/viz/src/shape_widgets.cpp
View File

@ -1,5 +1,13 @@
#include "precomp.hpp"
namespace cv
{
namespace viz
{
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////
/// line widget implementation
cv::viz::LineWidget::LineWidget(const Point3f &pt1, const Point3f &pt2, const Color &color)
@ -524,4 +532,4 @@ cv::String cv::viz::TextWidget::getText() const
vtkTextActor *actor = vtkTextActor::SafeDownCast(WidgetAccessor::getProp(*this));
CV_Assert(actor);
return actor->GetInput();
}
}

84
modules/viz/src/types.cpp
View File

@ -59,3 +59,87 @@ cv::viz::MouseEvent::MouseEvent (const Type& _type, const MouseButton& _button,
if (shift)
key_state |= KeyboardEvent::Shift;
}
////////////////////////////////////////////////////////////////////
/// cv::viz::Mesh3d
struct cv::viz::Mesh3d::loadMeshImpl
{
static cv::viz::Mesh3d::Ptr loadMesh(const String &file)
{
Mesh3d::Ptr mesh = new Mesh3d();
vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New();
reader->SetFileName(file.c_str());
reader->Update();
vtkSmartPointer<vtkPolyData> poly_data = reader->GetOutput ();
typedef unsigned int uint32_t;
vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
mesh->cloud.create(1, nr_points, CV_32FC3);
double point_xyz[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
mesh_points->GetPoint (i, &point_xyz[0]);
mesh->cloud.ptr<cv::Point3f>()[i] = cv::Point3d(point_xyz[0], point_xyz[1], point_xyz[2]);;
}
// Then the color information, if any
vtkUnsignedCharArray* poly_colors = NULL;
if (poly_data->GetPointData() != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
// some applications do not save the name of scalars (including PCL's native vtk_io)
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
// TODO: currently only handles rgb values with 3 components
if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
{
mesh->colors.create(1, nr_points, CV_8UC3);
unsigned char point_color[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
poly_colors->GetTupleValue (i, &point_color[0]);
//RGB or BGR?????
mesh->colors.ptr<cv::Vec3b>()[i] = cv::Vec3b(point_color[0], point_color[1], point_color[2]);
}
}
else
mesh->colors.release();
// Now handle the polygons
vtkIdType* cell_points;
vtkIdType nr_cell_points;
vtkCellArray * mesh_polygons = poly_data->GetPolys ();
mesh_polygons->InitTraversal ();
int id_poly = 0;
mesh->polygons.create(1, mesh_polygons->GetSize(), CV_32SC1);
int * polygons = mesh->polygons.ptr<int>();
while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
{
*polygons++ = nr_cell_points;
for (int i = 0; i < nr_cell_points; ++i)
*polygons++ = static_cast<int> (cell_points[i]);
}
return mesh;
}
};
cv::viz::Mesh3d::Ptr cv::viz::Mesh3d::loadMesh(const String& file)
{
return loadMeshImpl::loadMesh(file);
}

24
modules/viz/src/viz.cpp
View File

@ -37,3 +37,27 @@ cv::Matx44f cv::viz::convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& vtk_matr
m(i, k) = vtk_matrix->GetElement (i, k);
return m;
}
namespace cv
{
namespace viz
{
template<typename _Tp> Vec<_Tp, 3>* vtkpoints_data(vtkSmartPointer<vtkPoints>& points);
template<> Vec3f* vtkpoints_data<float>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_FLOAT);
vtkDataArray *data = points->GetData();
float *pointer = static_cast<vtkFloatArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3f*>(pointer);
}
template<> Vec3d* vtkpoints_data<double>(vtkSmartPointer<vtkPoints>& points)
{
CV_Assert(points->GetDataType() == VTK_DOUBLE);
vtkDataArray *data = points->GetData();
double *pointer = static_cast<vtkDoubleArray*>(data)->GetPointer(0);
return reinterpret_cast<Vec3d*>(pointer);
}
}
}

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

@ -849,53 +849,53 @@ bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename,
bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
{
CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addPolylineFromPolygonMesh] A shape with id <"<< id << "> already exists! Please choose a different id and retry.\n" << std::endl, false;
vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
poly_points->SetNumberOfPoints (mesh.cloud.size().area());
const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
for (int i = 0; i < mesh.cloud.cols; ++i)
poly_points->InsertPoint (i, cdata[i].x, cdata[i].y,cdata[i].z);
// Create a cell array to store the lines in and add the lines to it
vtkSmartPointer <vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
vtkSmartPointer <vtkPolyData> polyData;
allocVtkPolyData (polyData);
for (size_t i = 0; i < mesh.polygons.size (); i++)
{
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
polyLine->GetPointIds()->SetNumberOfIds(mesh.polygons[i].vertices.size());
for(unsigned int k = 0; k < mesh.polygons[i].vertices.size(); k++)
{
polyLine->GetPointIds()->SetId(k,mesh. polygons[i].vertices[k]);
}
cells->InsertNextCell (polyLine);
}
// Add the points to the dataset
polyData->SetPoints (poly_points);
// Add the lines to the dataset
polyData->SetLines (cells);
// Setup actor and mapper
vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
mapper->SetInput (polyData);
vtkSmartPointer <vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
actor->SetMapper (mapper);
renderer_->AddActor(actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
// CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
//
// ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
// if (am_it != shape_actor_map_->end ())
// return std::cout << "[addPolylineFromPolygonMesh] A shape with id <"<< id << "> already exists! Please choose a different id and retry.\n" << std::endl, false;
//
// vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
// poly_points->SetNumberOfPoints (mesh.cloud.size().area());
//
// const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
// for (int i = 0; i < mesh.cloud.cols; ++i)
// poly_points->InsertPoint (i, cdata[i].x, cdata[i].y,cdata[i].z);
//
//
// // Create a cell array to store the lines in and add the lines to it
// vtkSmartPointer <vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
// vtkSmartPointer <vtkPolyData> polyData;
// allocVtkPolyData (polyData);
//
// for (size_t i = 0; i < mesh.polygons.size (); i++)
// {
// vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
// polyLine->GetPointIds()->SetNumberOfIds(mesh.polygons[i].vertices.size());
// for(unsigned int k = 0; k < mesh.polygons[i].vertices.size(); k++)
// {
// polyLine->GetPointIds()->SetId(k,mesh. polygons[i].vertices[k]);
// }
//
// cells->InsertNextCell (polyLine);
// }
//
// // Add the points to the dataset
// polyData->SetPoints (poly_points);
//
// // Add the lines to the dataset
// polyData->SetLines (cells);
//
// // Setup actor and mapper
// vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
// mapper->SetInput (polyData);
//
// vtkSmartPointer <vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
// actor->SetMapper (mapper);
// renderer_->AddActor(actor);
//
// // Save the pointer/ID pair to the global actor map
// (*shape_actor_map_)[id] = actor;
return (true);
}
@ -1019,304 +1019,305 @@ void cv::viz::Viz3d::VizImpl::setWindowSize (int xw, int yw) { window_->SetSize
bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
{
CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
return std::cout << "[addPolygonMesh] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
// int rgb_idx = -1;
// std::vector<sensor_msgs::PointField> fields;
// rgb_idx = cv::viz::getFieldIndex (*cloud, "rgb", fields);
// if (rgb_idx == -1)
// rgb_idx = cv::viz::getFieldIndex (*cloud, "rgba", fields);
vtkSmartPointer<vtkUnsignedCharArray> colors_array;
#if 1
if (!mesh.colors.empty())
{
colors_array = vtkSmartPointer<vtkUnsignedCharArray>::New ();
colors_array->SetNumberOfComponents (3);
colors_array->SetName ("Colors");
const unsigned char* data = mesh.colors.ptr<unsigned char>();
//TODO check mask
CV_Assert(mask.empty()); //because not implemented;
for(int i = 0; i < mesh.colors.cols; ++i)
colors_array->InsertNextTupleValue(&data[i*3]);
// cv::viz::RGB rgb_data;
// for (size_t i = 0; i < cloud->size (); ++i)
// {
// if (!isFinite (cloud->points[i]))
// continue;
// memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (cv::viz::RGB));
// unsigned char color[3];
// color[0] = rgb_data.r;
// color[1] = rgb_data.g;
// color[2] = rgb_data.b;
// colors->InsertNextTupleValue (color);
// }
}
#endif
// Create points from polyMesh.cloud
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkIdType nr_points = mesh.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);
std::vector<int> lookup;
// If the dataset is dense (no NaNs)
if (mask.empty())
{
cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
mesh.cloud.copyTo(hdr);
}
else
{
lookup.resize (nr_points);
const unsigned char *mdata = mask.ptr<unsigned char>();
const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
int j = 0; // true point index
for (int i = 0; i < nr_points; ++i)
if(mdata[i])
{
lookup[i] = j;
out[j++] = cdata[i];
}
nr_points = j;
points->SetNumberOfPoints (nr_points);
}
// Get the maximum size of a polygon
int max_size_of_polygon = -1;
for (size_t i = 0; i < mesh.polygons.size (); ++i)
if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
vtkSmartPointer<vtkLODActor> actor;
if (mesh.polygons.size () > 1)
{
// Create polys from polyMesh.polygons
vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
vtkIdType *cell = cell_array->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
int idx = 0;
if (lookup.size () > 0)
{
for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
{
size_t n_points = mesh.polygons[i].vertices.size ();
*cell++ = n_points;
//cell_array->InsertNextCell (n_points);
for (size_t j = 0; j < n_points; j++, ++idx)
*cell++ = lookup[mesh.polygons[i].vertices[j]];
//cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
}
}
else
{
for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
{
size_t n_points = mesh.polygons[i].vertices.size ();
*cell++ = n_points;
//cell_array->InsertNextCell (n_points);
for (size_t j = 0; j < n_points; j++, ++idx)
*cell++ = mesh.polygons[i].vertices[j];
//cell_array->InsertCellPoint (vertices[i].vertices[j]);
}
}
vtkSmartPointer<vtkPolyData> polydata;
allocVtkPolyData (polydata);
cell_array->GetData ()->SetNumberOfValues (idx);
cell_array->Squeeze ();
polydata->SetStrips (cell_array);
polydata->SetPoints (points);
if (colors_array)
polydata->GetPointData ()->SetScalars (colors_array);
createActorFromVTKDataSet (polydata, actor, false);
}
else
{
vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
size_t n_points = mesh.polygons[0].vertices.size ();
polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
if (lookup.size () > 0)
{
for (size_t j = 0; j < n_points - 1; ++j)
polygon->GetPointIds ()->SetId (j, lookup[mesh.polygons[0].vertices[j]]);
}
else
{
for (size_t j = 0; j < n_points - 1; ++j)
polygon->GetPointIds ()->SetId (j, mesh.polygons[0].vertices[j]);
}
vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
allocVtkUnstructuredGrid (poly_grid);
poly_grid->Allocate (1, 1);
poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
poly_grid->SetPoints (points);
poly_grid->Update ();
if (colors_array)
poly_grid->GetPointData ()->SetScalars (colors_array);
createActorFromVTKDataSet (poly_grid, actor, false);
}
renderer_->AddActor (actor);
actor->GetProperty ()->SetRepresentationToSurface ();
// Backface culling renders the visualization slower, but guarantees that we see all triangles
actor->GetProperty ()->BackfaceCullingOff ();
actor->GetProperty ()->SetInterpolationToFlat ();
actor->GetProperty ()->EdgeVisibilityOff ();
actor->GetProperty ()->ShadingOff ();
// Save the pointer/ID pair to the global actor map
(*cloud_actor_map_)[id].actor = actor;
//if (vertices.size () > 1)
// (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
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;
return (true);
// CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
// CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
// CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
//
// if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
// return std::cout << "[addPolygonMesh] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
//
// // int rgb_idx = -1;
// // std::vector<sensor_msgs::PointField> fields;
//
//
// // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields);
// // if (rgb_idx == -1)
// // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields);
//
// vtkSmartPointer<vtkUnsignedCharArray> colors_array;
// #if 1
// if (!mesh.colors.empty())
// {
// colors_array = vtkSmartPointer<vtkUnsignedCharArray>::New ();
// colors_array->SetNumberOfComponents (3);
// colors_array->SetName ("Colors");
//
// const unsigned char* data = mesh.colors.ptr<unsigned char>();
//
// //TODO check mask
// CV_Assert(mask.empty()); //because not implemented;
//
// for(int i = 0; i < mesh.colors.cols; ++i)
// colors_array->InsertNextTupleValue(&data[i*3]);
//
// // temp_viz::RGB rgb_data;
// // for (size_t i = 0; i < cloud->size (); ++i)
// // {
// // if (!isFinite (cloud->points[i]))
// // continue;
// // memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (temp_viz::RGB));
// // unsigned char color[3];
// // color[0] = rgb_data.r;
// // color[1] = rgb_data.g;
// // color[2] = rgb_data.b;
// // colors->InsertNextTupleValue (color);
// // }
// }
// #endif
//
// // Create points from polyMesh.cloud
// vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
// vtkIdType nr_points = mesh.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);
//
//
// std::vector<int> lookup;
// // If the dataset is dense (no NaNs)
// if (mask.empty())
// {
// cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
// mesh.cloud.copyTo(hdr);
// }
// else
// {
// lookup.resize (nr_points);
//
// const unsigned char *mdata = mask.ptr<unsigned char>();
// const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
// cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
//
// int j = 0; // true point index
// for (int i = 0; i < nr_points; ++i)
// if(mdata[i])
// {
// lookup[i] = j;
// out[j++] = cdata[i];
// }
// nr_points = j;
// points->SetNumberOfPoints (nr_points);
// }
//
// // Get the maximum size of a polygon
// int max_size_of_polygon = -1;
// for (size_t i = 0; i < mesh.polygons.size (); ++i)
// if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
// max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
//
// vtkSmartPointer<vtkLODActor> actor;
//
// if (mesh.polygons.size () > 1)
// {
// // Create polys from polyMesh.polygons
// vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
// vtkIdType *cell = cell_array->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
// int idx = 0;
// if (lookup.size () > 0)
// {
// for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
// {
// size_t n_points = mesh.polygons[i].vertices.size ();
// *cell++ = n_points;
// //cell_array->InsertNextCell (n_points);
// for (size_t j = 0; j < n_points; j++, ++idx)
// *cell++ = lookup[mesh.polygons[i].vertices[j]];
// //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
// }
// }
// else
// {
// for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
// {
// size_t n_points = mesh.polygons[i].vertices.size ();
// *cell++ = n_points;
// //cell_array->InsertNextCell (n_points);
// for (size_t j = 0; j < n_points; j++, ++idx)
// *cell++ = mesh.polygons[i].vertices[j];
// //cell_array->InsertCellPoint (vertices[i].vertices[j]);
// }
// }
// vtkSmartPointer<vtkPolyData> polydata;
// allocVtkPolyData (polydata);
// cell_array->GetData ()->SetNumberOfValues (idx);
// cell_array->Squeeze ();
// polydata->SetStrips (cell_array);
// polydata->SetPoints (points);
//
// if (colors_array)
// polydata->GetPointData ()->SetScalars (colors_array);
//
// createActorFromVTKDataSet (polydata, actor, false);
// }
// else
// {
// vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
// size_t n_points = mesh.polygons[0].vertices.size ();
// polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
//
// if (lookup.size () > 0)
// {
// for (size_t j = 0; j < n_points - 1; ++j)
// polygon->GetPointIds ()->SetId (j, lookup[mesh.polygons[0].vertices[j]]);
// }
// else
// {
// for (size_t j = 0; j < n_points - 1; ++j)
// polygon->GetPointIds ()->SetId (j, mesh.polygons[0].vertices[j]);
// }
// vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
// allocVtkUnstructuredGrid (poly_grid);
// poly_grid->Allocate (1, 1);
// poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
// poly_grid->SetPoints (points);
// poly_grid->Update ();
// if (colors_array)
// poly_grid->GetPointData ()->SetScalars (colors_array);
//
// createActorFromVTKDataSet (poly_grid, actor, false);
// }
// renderer_->AddActor (actor);
// actor->GetProperty ()->SetRepresentationToSurface ();
// // Backface culling renders the visualization slower, but guarantees that we see all triangles
// actor->GetProperty ()->BackfaceCullingOff ();
// actor->GetProperty ()->SetInterpolationToFlat ();
// actor->GetProperty ()->EdgeVisibilityOff ();
// actor->GetProperty ()->ShadingOff ();
//
// // Save the pointer/ID pair to the global actor map
// (*cloud_actor_map_)[id].actor = actor;
// //if (vertices.size () > 1)
// // (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
//
// 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;
//
// return (true);
return true;
}
bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id)
{
CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
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 = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
if (!polydata)
return (false);
vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
if (!cells)
return (false);
vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
// Copy the new point array in
vtkIdType nr_points = mesh.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);
int ptr = 0;
std::vector<int> lookup;
// If the dataset is dense (no NaNs)
if (mask.empty())
{
cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
mesh.cloud.copyTo(hdr);
}
else
{
lookup.resize (nr_points);
const unsigned char *mdata = mask.ptr<unsigned char>();
const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
int j = 0; // true point index
for (int i = 0; i < nr_points; ++i)
if(mdata[i])
{
lookup[i] = j;
out[j++] = cdata[i];
}
nr_points = j;
points->SetNumberOfPoints (nr_points);;
}
// Update colors
vtkUnsignedCharArray* colors_array = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
if (!mesh.colors.empty() && colors_array)
{
if (mask.empty())
{
const unsigned char* data = mesh.colors.ptr<unsigned char>();
for(int i = 0; i < mesh.colors.cols; ++i)
colors_array->InsertNextTupleValue(&data[i*3]);
}
else
{
const unsigned char* color = mesh.colors.ptr<unsigned char>();
const unsigned char* mdata = mask.ptr<unsigned char>();
int j = 0;
for(int i = 0; i < mesh.colors.cols; ++i)
if (mdata[i])
colors_array->SetTupleValue (j++, &color[i*3]);
}
}
// Get the maximum size of a polygon
int max_size_of_polygon = -1;
for (size_t i = 0; i < mesh.polygons.size (); ++i)
if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
// Update the cells
cells = vtkSmartPointer<vtkCellArray>::New ();
vtkIdType *cell = cells->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
int idx = 0;
if (lookup.size () > 0)
{
for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
{
size_t n_points = mesh.polygons[i].vertices.size ();
*cell++ = n_points;
for (size_t j = 0; j < n_points; j++, cell++, ++idx)
*cell = lookup[mesh.polygons[i].vertices[j]];
}
}
else
{
for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
{
size_t n_points = mesh.polygons[i].vertices.size ();
*cell++ = n_points;
for (size_t j = 0; j < n_points; j++, cell++, ++idx)
*cell = mesh.polygons[i].vertices[j];
}
}
cells->GetData ()->SetNumberOfValues (idx);
cells->Squeeze ();
// Set the the vertices
polydata->SetStrips (cells);
polydata->Update ();
// CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
// CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
// CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
//
// // Check to see if this ID entry already exists (has it been already added to the visualizer?)
// 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 = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
// if (!polydata)
// return (false);
// vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
// if (!cells)
// return (false);
// vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
// // Copy the new point array in
// vtkIdType nr_points = mesh.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);
//
// int ptr = 0;
// std::vector<int> lookup;
// // If the dataset is dense (no NaNs)
// if (mask.empty())
// {
// cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
// mesh.cloud.copyTo(hdr);
//
// }
// else
// {
// lookup.resize (nr_points);
//
// const unsigned char *mdata = mask.ptr<unsigned char>();
// const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
// cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
//
// int j = 0; // true point index
// for (int i = 0; i < nr_points; ++i)
// if(mdata[i])
// {
// lookup[i] = j;
// out[j++] = cdata[i];
// }
// nr_points = j;
// points->SetNumberOfPoints (nr_points);;
// }
//
// // Update colors
// vtkUnsignedCharArray* colors_array = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
//
// if (!mesh.colors.empty() && colors_array)
// {
// if (mask.empty())
// {
// const unsigned char* data = mesh.colors.ptr<unsigned char>();
// for(int i = 0; i < mesh.colors.cols; ++i)
// colors_array->InsertNextTupleValue(&data[i*3]);
// }
// else
// {
// const unsigned char* color = mesh.colors.ptr<unsigned char>();
// const unsigned char* mdata = mask.ptr<unsigned char>();
//
// int j = 0;
// for(int i = 0; i < mesh.colors.cols; ++i)
// if (mdata[i])
// colors_array->SetTupleValue (j++, &color[i*3]);
//
// }
// }
//
// // Get the maximum size of a polygon
// int max_size_of_polygon = -1;
// for (size_t i = 0; i < mesh.polygons.size (); ++i)
// if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
// max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
//
// // Update the cells
// cells = vtkSmartPointer<vtkCellArray>::New ();
// vtkIdType *cell = cells->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
// int idx = 0;
// if (lookup.size () > 0)
// {
// for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
// {
// size_t n_points = mesh.polygons[i].vertices.size ();
// *cell++ = n_points;
// for (size_t j = 0; j < n_points; j++, cell++, ++idx)
// *cell = lookup[mesh.polygons[i].vertices[j]];
// }
// }
// else
// {
// for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
// {
// size_t n_points = mesh.polygons[i].vertices.size ();
// *cell++ = n_points;
// for (size_t j = 0; j < n_points; j++, cell++, ++idx)
// *cell = mesh.polygons[i].vertices[j];
// }
// }
// cells->GetData ()->SetNumberOfValues (idx);
// cells->Squeeze ();
// // Set the the vertices
// polydata->SetStrips (cells);
// polydata->Update ();
return (true);
}

1
modules/viz/src/widget.cpp
View File

@ -1,3 +1,4 @@
#include "precomp.hpp"
///////////////////////////////////////////////////////////////////////////////////////////////

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

@ -99,18 +99,18 @@ TEST(Viz_viz3d, accuracy)
viz::CloudWidget pcw(cloud, colors);
viz::CloudWidget pcw2(cloud, viz::Color::magenta());
viz.showWidget("line", lw);
viz.showWidget("plane", pw);
viz.showWidget("sphere", sw);
viz.showWidget("arrow", aw);
viz.showWidget("circle", cw);
viz.showWidget("cylinder", cyw);
viz.showWidget("cube", cuw);
viz.showWidget("coordinateSystem", csw);
viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
viz.showWidget("text",tw);
viz.showWidget("pcw",pcw);
viz.showWidget("pcw2",pcw2);
viz.showWidget("line", lw);
viz.showWidget("plane", pw);
viz.showWidget("sphere", sw);
viz.showWidget("arrow", aw);
viz.showWidget("circle", cw);
viz.showWidget("cylinder", cyw);
viz.showWidget("cube", cuw);
viz.showWidget("coordinateSystem", csw);
viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
viz.showWidget("text",tw);
viz.showWidget("pcw",pcw);
viz.showWidget("pcw2",pcw2);
// viz::LineWidget lw2 = lw;
// v.showPointCloud("cld",cloud, colors);
@ -128,16 +128,23 @@ TEST(Viz_viz3d, accuracy)
viz.showWidget("polyline", plw);
// lw = v.getWidget("polyline").cast<viz::LineWidget>();
viz::Mesh3d::Ptr mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
viz::MeshWidget mw(*mesh);
viz.showWidget("mesh", mw);
viz.spin();
//viz::GridWidget gw(viz::Vec2i(100,100), viz::Vec2d(1,1));
//v.showWidget("grid", gw);
lw = viz.getWidget("grid").cast<cv::viz::LineWidget>();
// lw = viz.getWidget("grid").cast<cv::viz::LineWidget>();
//viz::Text3DWidget t3w("OpenCV", cv::Point3f(0.0, 2.0, 0.0), 1.0, viz::Color(255,255,0));
//v.showWidget("txt3d", t3w);
// float grid_x_angle = 0.0;
while(!viz.wasStopped())
{
// Creating new point cloud with id cloud1