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

removed q subfolder

Browse Source
This commit is contained in:
Anatoly Baksheev 2013-07-13 17:45:46 +04:00
parent f480eca668
commit 3e41f0647e
14 changed files with 612 additions and 621 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
modules/viz/include/opencv2/viz/mesh_load.hpp
View File

@ -1,10 +0,0 @@
#pragma once
#include <opencv2/core.hpp>
#include <opencv2/viz/types.hpp>
#include <vector>
namespace temp_viz
{
CV_EXPORTS Mesh3d::Ptr mesh_load(const String& file);
}

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

@ -67,10 +67,13 @@ namespace temp_viz
class CV_EXPORTS Mesh3d
{
public:
typedef cv::Ptr<Mesh3d> Ptr;
typedef Ptr<Mesh3d> Ptr;
Mat cloud, colors;
std::vector<Vertices> polygons;
static Mesh3d::Ptr mesh_load(const String& file);
};
/////////////////////////////////////////////////////////////////////////////

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

@ -1,4 +1,4 @@
#include <q/common.h>
#include <common.h>
#include <cstdlib>
#include <opencv2/viz/types.hpp>

0
modules/viz/src/q/common.h → modules/viz/src/common.h
View File

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

@ -1,6 +1,6 @@
#include "precomp.hpp"
#include <q/interactor_style.h>
#include "interactor_style.h"
//#include <q/visualization/vtk/vtkVertexBufferObjectMapper.h>

2
modules/viz/src/q/interactor_style.h → modules/viz/src/interactor_style.h
View File

@ -1,6 +1,6 @@
#pragma once
#include <q/viz_types.h>
#include "viz_types.h"
#include <opencv2/viz/events.hpp>
namespace temp_viz

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

@ -1,5 +1,3 @@
#include <opencv2/viz/mesh_load.hpp>
#include "precomp.hpp"
#include <vtkPLYReader.h>
@ -8,7 +6,7 @@
#include <vtkPointData.h>
#include <vtkCellArray.h>
temp_viz::Mesh3d::Ptr temp_viz::mesh_load(const String& file)
temp_viz::Mesh3d::Ptr temp_viz::Mesh3d::mesh_load(const String& file)
{
Mesh3d::Ptr mesh = new Mesh3d();

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

@ -152,7 +152,7 @@
#endif
#include <q/viz3d_impl.hpp>
#include "viz3d_impl.hpp"
#include <opencv2/core.hpp>
#include <opencv2/viz.hpp>
#include "opencv2/viz/widget_accessor.hpp"

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

@ -1,5 +1,5 @@
#include <opencv2/viz/viz3d.hpp>
#include <q/viz3d_impl.hpp>
#include "viz3d_impl.hpp"
temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_name))

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

@ -1,559 +0,0 @@
#include "precomp.hpp"
void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
{
if (window_)
window_->SetFullScreen (mode);
}
void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
{
if (window_)
window_->SetWindowName (name.c_str ());
}
void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
bool temp_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 = 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);
}
bool temp_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 ();
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color, bool display_length, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
// Create an Actor
vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
leader->GetPositionCoordinate()->SetCoordinateSystemToWorld ();
leader->GetPositionCoordinate()->SetValue (p1.x, p1.y, p1.z);
leader->GetPosition2Coordinate()->SetCoordinateSystemToWorld ();
leader->GetPosition2Coordinate()->SetValue (p2.x, p2.y, p2.z);
leader->SetArrowStyleToFilled();
leader->SetArrowPlacementToPoint2 ();
if (display_length)
leader->AutoLabelOn ();
else
leader->AutoLabelOff ();
Color c = vtkcolor(color);
leader->GetProperty ()->SetColor (c.val);
renderer_->AddActor (leader);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = leader;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color_line, const Color& color_text, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
// Create an Actor
vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
leader->GetPositionCoordinate ()->SetValue (p1.x, p1.y, p1.z);
leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
leader->GetPosition2Coordinate ()->SetValue (p2.x, p2.y, p2.z);
leader->SetArrowStyleToFilled ();
leader->AutoLabelOn ();
Color ct = vtkcolor(color_text);
leader->GetLabelTextProperty()->SetColor(ct.val);
Color cl = vtkcolor(color_line);
leader->GetProperty ()->SetColor (cl.val);
renderer_->AddActor (leader);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = leader;
return (true);
}
bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
{
CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
int total = cloud.size().area();
points->SetNumberOfPoints (total);
polygon->GetPointIds ()->SetNumberOfIds (total);
for (int i = 0; i < total; ++i)
{
cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
points->SetPoint (i, p.x, p.y, p.z);
polygon->GetPointIds ()->SetId (i, i);
}
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 ();
//////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> data = poly_grid;
Color c = vtkcolor(color);
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
// Add old data
all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
// Add new data
vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
all_data->AddInput (poly_data);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (all_data->GetOutput (), actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->BackfaceCullingOff ();
removeActorFromRenderer (am_it->second);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
else
{
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->BackfaceCullingOff ();
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
return (true);
}
void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
if (exists)
{
// Remove it if it exists and add it again
removeActorFromRenderer(wam_itr->second.actor);
}
// Get the actor and set the user matrix
vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(widget));
if (actor)
{
// If the actor is 3D, apply pose
vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified();
}
// If the actor is a vtkFollower, then it should always face the camera
vtkFollower *follower = vtkFollower::SafeDownCast(actor);
if (follower)
{
follower->SetCamera(renderer_->GetActiveCamera());
}
renderer_->AddActor(WidgetAccessor::getProp(widget));
(*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
}
void temp_viz::Viz3d::VizImpl::removeWidget(const String &id)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
CV_Assert(removeActorFromRenderer (wam_itr->second.actor));
widget_actor_map_->erase(wam_itr);
}
temp_viz::Widget temp_viz::Viz3d::VizImpl::getWidget(const String &id) const
{
WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
Widget widget;
WidgetAccessor::setProp(widget, wam_itr->second.actor);
return widget;
}
void temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
}
void temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
if (!matrix)
{
setWidgetPose(id, pose);
return ;
}
Matx44f matrix_cv = convertToMatx(matrix);
Affine3f updated_pose = pose * Affine3f(matrix_cv);
matrix = convertToVtkMatrix(updated_pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
}
temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) const
{
WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
Matx44f matrix_cv = convertToMatx(matrix);
return Affine3f(matrix_cv);
}

24
modules/viz/src/q/viz3d_impl.hpp → modules/viz/src/viz3d_impl.hpp
View File

@ -2,17 +2,15 @@
#include <opencv2/core.hpp>
#include <opencv2/viz/events.hpp>
#include <q/interactor_style.h>
#include <q/viz_types.h>
#include <q/common.h>
#include "interactor_style.h"
#include "viz_types.h"
#include "common.h"
#include <opencv2/viz/types.hpp>
#include <opencv2/core/affine.hpp>
#include <opencv2/viz/viz3d.hpp>
namespace temp_viz
{
struct Viz3d::VizImpl
struct temp_viz::Viz3d::VizImpl
{
public:
typedef cv::Ptr<VizImpl> Ptr;
@ -23,16 +21,7 @@ public:
void setFullScreen (bool mode);
void setWindowName (const String &name);
/** \brief Register a callback function for keyboard input
* \param[in] callback function that will be registered as a callback for a keyboard event
* \param[in] cookie for passing user data to callback
*/
void registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie = 0);
/** \brief Register a callback function for mouse events
* \param[in] ccallback function that will be registered as a callback for a mouse event
* \param[in] cookie for passing user data to callback
*/
void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0);
void spin ();
@ -289,6 +278,11 @@ private:
void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
};
namespace temp_viz
{
//void getTransformationMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternionf& orientation, Eigen::Matrix4f &transformation);
//void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);

561
modules/viz/src/viz_main.cpp
View File

@ -1,7 +1,7 @@
#include "precomp.hpp"
#include <opencv2/calib3d.hpp>
#include <q/viz3d_impl.hpp>
#include "viz3d_impl.hpp"
#include <vtkRenderWindowInteractor.h>
#ifndef __APPLE__
@ -1058,3 +1058,562 @@ void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_ma
for (int k = 0; k < 4; k++)
m (i,k) = static_cast<float> (vtk_matrix->GetElement (i, k));
}
void temp_viz::Viz3d::VizImpl::setFullScreen (bool mode)
{
if (window_)
window_->SetFullScreen (mode);
}
void temp_viz::Viz3d::VizImpl::setWindowName (const std::string &name)
{
if (window_)
window_->SetWindowName (name.c_str ());
}
void temp_viz::Viz3d::VizImpl::setPosition (int x, int y) { window_->SetPosition (x, y); }
void temp_viz::Viz3d::VizImpl::setSize (int xw, int yw) { window_->SetSize (xw, yw); }
bool temp_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 = 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);
}
bool temp_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 ();
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color, bool display_length, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
return std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
// Create an Actor
vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
leader->GetPositionCoordinate()->SetCoordinateSystemToWorld ();
leader->GetPositionCoordinate()->SetValue (p1.x, p1.y, p1.z);
leader->GetPosition2Coordinate()->SetCoordinateSystemToWorld ();
leader->GetPosition2Coordinate()->SetValue (p2.x, p2.y, p2.z);
leader->SetArrowStyleToFilled();
leader->SetArrowPlacementToPoint2 ();
if (display_length)
leader->AutoLabelOn ();
else
leader->AutoLabelOff ();
Color c = vtkcolor(color);
leader->GetProperty ()->SetColor (c.val);
renderer_->AddActor (leader);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = leader;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool temp_viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color_line, const Color& color_text, const std::string &id)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
return (false);
}
// Create an Actor
vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
leader->GetPositionCoordinate ()->SetValue (p1.x, p1.y, p1.z);
leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
leader->GetPosition2Coordinate ()->SetValue (p2.x, p2.y, p2.z);
leader->SetArrowStyleToFilled ();
leader->AutoLabelOn ();
Color ct = vtkcolor(color_text);
leader->GetLabelTextProperty()->SetColor(ct.val);
Color cl = vtkcolor(color_line);
leader->GetProperty ()->SetColor (cl.val);
renderer_->AddActor (leader);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = leader;
return (true);
}
bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
{
CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
int total = cloud.size().area();
points->SetNumberOfPoints (total);
polygon->GetPointIds ()->SetNumberOfIds (total);
for (int i = 0; i < total; ++i)
{
cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
points->SetPoint (i, p.x, p.y, p.z);
polygon->GetPointIds ()->SetId (i, i);
}
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 ();
//////////////////////////////////////////////////////
vtkSmartPointer<vtkDataSet> data = poly_grid;
Color c = vtkcolor(color);
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
// Add old data
all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
// Add new data
vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
all_data->AddInput (poly_data);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (all_data->GetOutput (), actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->BackfaceCullingOff ();
removeActorFromRenderer (am_it->second);
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
else
{
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetColor (c.val);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->BackfaceCullingOff ();
renderer_->AddActor (actor);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
}
return (true);
}
void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
if (exists)
{
// Remove it if it exists and add it again
removeActorFromRenderer(wam_itr->second.actor);
}
// Get the actor and set the user matrix
vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(widget));
if (actor)
{
// If the actor is 3D, apply pose
vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified();
}
// If the actor is a vtkFollower, then it should always face the camera
vtkFollower *follower = vtkFollower::SafeDownCast(actor);
if (follower)
{
follower->SetCamera(renderer_->GetActiveCamera());
}
renderer_->AddActor(WidgetAccessor::getProp(widget));
(*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
}
void temp_viz::Viz3d::VizImpl::removeWidget(const String &id)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
CV_Assert(removeActorFromRenderer (wam_itr->second.actor));
widget_actor_map_->erase(wam_itr);
}
temp_viz::Widget temp_viz::Viz3d::VizImpl::getWidget(const String &id) const
{
WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
Widget widget;
WidgetAccessor::setProp(widget, wam_itr->second.actor);
return widget;
}
void temp_viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
}
void temp_viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose)
{
WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
if (!matrix)
{
setWidgetPose(id, pose);
return ;
}
Matx44f matrix_cv = convertToMatx(matrix);
Affine3f updated_pose = pose * Affine3f(matrix_cv);
matrix = convertToVtkMatrix(updated_pose.matrix);
actor->SetUserMatrix (matrix);
actor->Modified ();
}
temp_viz::Affine3f temp_viz::Viz3d::VizImpl::getWidgetPose(const String &id) const
{
WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
bool exists = wam_itr != widget_actor_map_->end();
CV_Assert(exists);
vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
CV_Assert(actor);
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
Matx44f matrix_cv = convertToMatx(matrix);
return Affine3f(matrix_cv);
}

0
modules/viz/src/q/viz_types.h → modules/viz/src/viz_types.h
View File

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

@ -48,7 +48,6 @@
#include <string>
#include <opencv2/viz.hpp>
#include <opencv2/viz/mesh_load.hpp>
cv::Mat cvcloud_load()
{
@ -83,45 +82,50 @@ TEST(Viz_viz3d, accuracy)
float pos_x = 0.0f;
float pos_y = 0.0f;
float pos_z = 0.0f;
// temp_viz::Mesh3d::Ptr mesh = temp_viz::mesh_load("d:/horse.ply");
// v.addPolygonMesh(*mesh, "pq");
temp_viz::Mesh3d::Ptr mesh = temp_viz::Mesh3d::mesh_load("d:/horse.ply");
v.addPolygonMesh(*mesh, "pq");
int col_blue = 0;
int col_green = 0;
int col_red = 0;
temp_viz::LineWidget lw(cv::Point3f(0.0,0.0,0.0), cv::Point3f(4.0,4.0,4.0), temp_viz::Color(0,255,0));
temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0), 5.0);
temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0), 50.0);
temp_viz::SphereWidget sw(cv::Point3f(0,0,0), 0.5);
temp_viz::ArrowWidget aw(cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0));
temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 0.5, 0.01, temp_viz::Color(0,255,0));
temp_viz::CylinderWidget cyw(cv::Point3f(0,0,0), cv::Point3f(-1,-1,-1), 0.5, 30, temp_viz::Color(0,255,0));
temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1));
temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity());
temp_viz::CoordinateSystemWidget csw;
temp_viz::TextWidget tw("TEST", cv::Point2i(100,100), 20);
temp_viz::CloudWidget pcw(cloud, colors);
temp_viz::CloudWidget pcw2(cloud, temp_viz::Color(0,255,255));
// v.showWidget("line", lw);
// v.showWidget("plane", pw);
// v.showWidget("sphere", sw);
// v.showWidget("arrow", aw);
// v.showWidget("circle", cw);
// v.showWidget("cylinder", cyw);
// v.showWidget("cube", cuw);
v.showWidget("coordinateSystem", csw);
// v.showWidget("text",tw);
// v.showWidget("pcw",pcw);
// v.showWidget("pcw2",pcw2);
v.showWidget("sphere", sw);
v.spin();
//v.showWidget("arrow", aw);
//v.showWidget("circle", cw);
//v.showWidget("cylinder", cyw);
//v.showWidget("cube", cuw);
//v.showWidget("coordinateSystem", csw);
//v.showWidget("coordinateSystem2", temp_viz::CoordinateSystemWidget(2.0), cv::Affine3f(0, 0, 0, cv::Vec3f(2, 0, 0)));
//v.showWidget("text",tw);
//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);
//temp_viz::CloudNormalsWidget cnw(cloud, normals);
//v.showWidget("n", cnw);
// lw = v.getWidget("n").cast<temp_viz::LineWidget>();
// pw = v.getWidget("n").cast<temp_viz::PlaneWidget>();
@ -135,16 +139,18 @@ TEST(Viz_viz3d, accuracy)
data[3] = cv::Vec4d(3.0,4.0,1.0,1.0);
points = points.reshape(0, 2);
temp_viz::PolyLineWidget plw(points);
// v.showWidget("polyline",plw);
temp_viz::PolyLineWidget plw(points, temp_viz::Color::green());
v.showWidget("polyline",plw);
// lw = v.getWidget("polyline").cast<temp_viz::LineWidget>();
temp_viz::GridWidget gw(temp_viz::Vec2i(10,10), temp_viz::Vec2d(0.1,0.1));
v.showWidget("grid", gw);
v.spin();
//temp_viz::GridWidget gw(temp_viz::Vec2i(100,100), temp_viz::Vec2d(1,1));
//v.showWidget("grid", gw);
lw = v.getWidget("grid").cast<temp_viz::LineWidget>();
temp_viz::Text3DWidget t3w("OpenCV", cv::Point3f(0.0, 2.0, 0.0), 1.0, temp_viz::Color(255,255,0));
v.showWidget("txt3d", t3w);
//temp_viz::Text3DWidget t3w("OpenCV", cv::Point3f(0.0, 2.0, 0.0), 1.0, temp_viz::Color(255,255,0));
//v.showWidget("txt3d", t3w);
// float grid_x_angle = 0.0;
while(!v.wasStopped())
@ -156,7 +162,7 @@ TEST(Viz_viz3d, accuracy)
lw.setColor(temp_viz::Color(col_blue, col_green, col_red));
// lw.setLineWidth(pos_x * 10);
plw.setColor(temp_viz::Color(col_blue, col_green, col_red));
//plw.setColor(temp_viz::Color(col_blue, col_green, col_red));
sw.setPose(cloudPosition);
// pw.setPose(cloudPosition);
@ -172,10 +178,10 @@ TEST(Viz_viz3d, accuracy)
// v.setWidgetPose("n",cloudPosition);
// v.setWidgetPose("pcw2", cloudPosition);
cnw.setColor(temp_viz::Color(col_blue, col_green, col_red));
pcw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
//cnw.setColor(temp_viz::Color(col_blue, col_green, col_red));
//pcw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
gw.updatePose(temp_viz::Affine3f(0.0, 0.1, 0.0, cv::Vec3f(0.0,0.0,0.0)));
//gw.updatePose(temp_viz::Affine3f(0.0, 0.1, 0.0, cv::Vec3f(0.0,0.0,0.0)));
angle_x += 0.1f;
angle_y -= 0.1f;