From 3e41f0647e2a8ab0d077906132efd0d4cbf306ab Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Sat, 13 Jul 2013 17:45:46 +0400 Subject: [PATCH] removed q subfolder --- modules/viz/include/opencv2/viz/mesh_load.hpp | 10 - modules/viz/include/opencv2/viz/types.hpp | 5 +- modules/viz/src/common.cpp | 2 +- modules/viz/src/{q => }/common.h | 0 modules/viz/src/interactor_style.cpp | 2 +- modules/viz/src/{q => }/interactor_style.h | 2 +- modules/viz/src/mesh_load.cpp | 4 +- modules/viz/src/precomp.hpp | 2 +- modules/viz/src/viz3d.cpp | 2 +- modules/viz/src/viz3d_impl.cpp | 559 ----------------- modules/viz/src/{q => }/viz3d_impl.hpp | 24 +- modules/viz/src/viz_main.cpp | 561 +++++++++++++++++- modules/viz/src/{q => }/viz_types.h | 0 modules/viz/test/test_viz3d.cpp | 60 +- 14 files changed, 612 insertions(+), 621 deletions(-) delete mode 100644 modules/viz/include/opencv2/viz/mesh_load.hpp rename modules/viz/src/{q => }/common.h (100%) rename modules/viz/src/{q => }/interactor_style.h (99%) delete mode 100644 modules/viz/src/viz3d_impl.cpp rename modules/viz/src/{q => }/viz3d_impl.hpp (96%) rename modules/viz/src/{q => }/viz_types.h (100%) diff --git a/modules/viz/include/opencv2/viz/mesh_load.hpp b/modules/viz/include/opencv2/viz/mesh_load.hpp deleted file mode 100644 index 737679c14..000000000 --- a/modules/viz/include/opencv2/viz/mesh_load.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace temp_viz -{ - CV_EXPORTS Mesh3d::Ptr mesh_load(const String& file); -} diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index d64debc81..dc67219f9 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -67,10 +67,13 @@ namespace temp_viz class CV_EXPORTS Mesh3d { public: - typedef cv::Ptr Ptr; + typedef Ptr Ptr; Mat cloud, colors; std::vector polygons; + + static Mesh3d::Ptr mesh_load(const String& file); + }; ///////////////////////////////////////////////////////////////////////////// diff --git a/modules/viz/src/common.cpp b/modules/viz/src/common.cpp index 78678a069..5cb57c86f 100644 --- a/modules/viz/src/common.cpp +++ b/modules/viz/src/common.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/modules/viz/src/q/common.h b/modules/viz/src/common.h similarity index 100% rename from modules/viz/src/q/common.h rename to modules/viz/src/common.h diff --git a/modules/viz/src/interactor_style.cpp b/modules/viz/src/interactor_style.cpp index 6e7bc7490..16b2e43ba 100644 --- a/modules/viz/src/interactor_style.cpp +++ b/modules/viz/src/interactor_style.cpp @@ -1,6 +1,6 @@ #include "precomp.hpp" -#include +#include "interactor_style.h" //#include diff --git a/modules/viz/src/q/interactor_style.h b/modules/viz/src/interactor_style.h similarity index 99% rename from modules/viz/src/q/interactor_style.h rename to modules/viz/src/interactor_style.h index 267b2b619..87095d593 100644 --- a/modules/viz/src/q/interactor_style.h +++ b/modules/viz/src/interactor_style.h @@ -1,6 +1,6 @@ #pragma once -#include +#include "viz_types.h" #include namespace temp_viz diff --git a/modules/viz/src/mesh_load.cpp b/modules/viz/src/mesh_load.cpp index 8b0a678b7..c729f2a6a 100644 --- a/modules/viz/src/mesh_load.cpp +++ b/modules/viz/src/mesh_load.cpp @@ -1,5 +1,3 @@ -#include - #include "precomp.hpp" #include @@ -8,7 +6,7 @@ #include #include -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(); diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp index 67c9ccd9c..11e0cdcdd 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -152,7 +152,7 @@ #endif -#include +#include "viz3d_impl.hpp" #include #include #include "opencv2/viz/widget_accessor.hpp" diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 4b9357391..80ce52aff 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -1,5 +1,5 @@ #include -#include +#include "viz3d_impl.hpp" temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_name)) diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp deleted file mode 100644 index d04348e3e..000000000 --- a/modules/viz/src/viz3d_impl.cpp +++ /dev/null @@ -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 fields; - - - // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields); - // if (rgb_idx == -1) - // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields); - - vtkSmartPointer colors_array; -#if 1 - if (!mesh.colors.empty()) - { - colors_array = vtkSmartPointer::New (); - colors_array->SetNumberOfComponents (3); - colors_array->SetName ("Colors"); - - const unsigned char* data = mesh.colors.ptr(); - - //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 (&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 points = vtkSmartPointer::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 (points->GetData ())->GetPointer (0); - - - std::vector 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(); - const cv::Point3f *cdata = mesh.cloud.ptr(); - cv::Point3f* out = reinterpret_cast(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 (mesh.polygons[i].vertices.size ())) - max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); - - vtkSmartPointer actor; - - if (mesh.polygons.size () > 1) - { - // Create polys from polyMesh.polygons - vtkSmartPointer cell_array = vtkSmartPointer::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 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 polygon = vtkSmartPointer::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 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(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData (); - - const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero (); - const Eigen::Quaternion& sensor_orientation = Eigen::Quaternionf::Identity (); - - // Save the viewpoint transformation matrix to the global actor map - vtkSmartPointer transformation = vtkSmartPointer::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 polydata = static_cast(am_it->second.actor->GetMapper ())->GetInput (); - if (!polydata) - return (false); - vtkSmartPointer cells = polydata->GetStrips (); - if (!cells) - return (false); - vtkSmartPointer 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 (points->GetData ()))->GetPointer (0); - - int ptr = 0; - std::vector 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(); - const cv::Point3f *cdata = mesh.cloud.ptr(); - cv::Point3f* out = reinterpret_cast(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(); - for(int i = 0; i < mesh.colors.cols; ++i) - colors_array->InsertNextTupleValue(&data[i*3]); - } - else - { - const unsigned char* color = mesh.colors.ptr(); - const unsigned char* mdata = mask.ptr(); - - 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 (mesh.polygons[i].vertices.size ())) - max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); - - // Update the cells - cells = vtkSmartPointer::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 leader = vtkSmartPointer::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 leader = vtkSmartPointer::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 points = vtkSmartPointer::New (); - vtkSmartPointer polygon = vtkSmartPointer::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()[i]; - points->SetPoint (i, p.x, p.y, p.z); - polygon->GetPointIds ()->SetId (i, i); - } - - vtkSmartPointer 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 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 all_data = vtkSmartPointer::New (); - - // Add old data - all_data->AddInput (reinterpret_cast ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ()); - - // Add new data - vtkSmartPointer surface_filter = vtkSmartPointer::New (); - surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data)); - vtkSmartPointer poly_data = surface_filter->GetOutput (); - all_data->AddInput (poly_data); - - // Create an Actor - vtkSmartPointer 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 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 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 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 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 matrix = actor->GetUserMatrix(); - Matx44f matrix_cv = convertToMatx(matrix); - return Affine3f(matrix_cv); -} diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/viz3d_impl.hpp similarity index 96% rename from modules/viz/src/q/viz3d_impl.hpp rename to modules/viz/src/viz3d_impl.hpp index c2b79b429..88593c24d 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/viz3d_impl.hpp @@ -2,17 +2,15 @@ #include #include -#include -#include -#include +#include "interactor_style.h" +#include "viz_types.h" +#include "common.h" #include #include #include -namespace temp_viz -{ -struct Viz3d::VizImpl +struct temp_viz::Viz3d::VizImpl { public: typedef cv::Ptr 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 &polydata); }; + + +namespace temp_viz +{ + //void getTransformationMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternionf& orientation, Eigen::Matrix4f &transformation); //void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer &vtk_matrix); diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index 5dcc7c41c..b3b502203 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -1,7 +1,7 @@ #include "precomp.hpp" #include -#include +#include "viz3d_impl.hpp" #include #ifndef __APPLE__ @@ -1058,3 +1058,562 @@ void temp_viz::convertToEigenMatrix (const vtkSmartPointer &vtk_ma for (int k = 0; k < 4; k++) m (i,k) = static_cast (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 fields; + + + // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields); + // if (rgb_idx == -1) + // rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields); + + vtkSmartPointer colors_array; +#if 1 + if (!mesh.colors.empty()) + { + colors_array = vtkSmartPointer::New (); + colors_array->SetNumberOfComponents (3); + colors_array->SetName ("Colors"); + + const unsigned char* data = mesh.colors.ptr(); + + //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 (&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 points = vtkSmartPointer::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 (points->GetData ())->GetPointer (0); + + + std::vector 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(); + const cv::Point3f *cdata = mesh.cloud.ptr(); + cv::Point3f* out = reinterpret_cast(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 (mesh.polygons[i].vertices.size ())) + max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); + + vtkSmartPointer actor; + + if (mesh.polygons.size () > 1) + { + // Create polys from polyMesh.polygons + vtkSmartPointer cell_array = vtkSmartPointer::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 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 polygon = vtkSmartPointer::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 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(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData (); + + const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero (); + const Eigen::Quaternion& sensor_orientation = Eigen::Quaternionf::Identity (); + + // Save the viewpoint transformation matrix to the global actor map + vtkSmartPointer transformation = vtkSmartPointer::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 polydata = static_cast(am_it->second.actor->GetMapper ())->GetInput (); + if (!polydata) + return (false); + vtkSmartPointer cells = polydata->GetStrips (); + if (!cells) + return (false); + vtkSmartPointer 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 (points->GetData ()))->GetPointer (0); + + int ptr = 0; + std::vector 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(); + const cv::Point3f *cdata = mesh.cloud.ptr(); + cv::Point3f* out = reinterpret_cast(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(); + for(int i = 0; i < mesh.colors.cols; ++i) + colors_array->InsertNextTupleValue(&data[i*3]); + } + else + { + const unsigned char* color = mesh.colors.ptr(); + const unsigned char* mdata = mask.ptr(); + + 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 (mesh.polygons[i].vertices.size ())) + max_size_of_polygon = static_cast (mesh.polygons[i].vertices.size ()); + + // Update the cells + cells = vtkSmartPointer::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 leader = vtkSmartPointer::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 leader = vtkSmartPointer::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 points = vtkSmartPointer::New (); + vtkSmartPointer polygon = vtkSmartPointer::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()[i]; + points->SetPoint (i, p.x, p.y, p.z); + polygon->GetPointIds ()->SetId (i, i); + } + + vtkSmartPointer 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 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 all_data = vtkSmartPointer::New (); + + // Add old data + all_data->AddInput (reinterpret_cast ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ()); + + // Add new data + vtkSmartPointer surface_filter = vtkSmartPointer::New (); + surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data)); + vtkSmartPointer poly_data = surface_filter->GetOutput (); + all_data->AddInput (poly_data); + + // Create an Actor + vtkSmartPointer 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 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 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 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 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 matrix = actor->GetUserMatrix(); + Matx44f matrix_cv = convertToMatx(matrix); + return Affine3f(matrix_cv); +} diff --git a/modules/viz/src/q/viz_types.h b/modules/viz/src/viz_types.h similarity index 100% rename from modules/viz/src/q/viz_types.h rename to modules/viz/src/viz_types.h diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 090501eb5..884b0b576 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -48,7 +48,6 @@ #include #include -#include 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(); // pw = v.getWidget("n").cast(); @@ -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::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::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;