From 658e4c5e971d78af560361d6094128e1e7ff02e9 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Mon, 8 Jul 2013 18:56:13 +0200 Subject: [PATCH] set/get/updateWidgetPose implemented, cloudNormals with color --- modules/viz/include/opencv2/viz/viz3d.hpp | 4 ++ modules/viz/include/opencv2/viz/widgets.hpp | 2 +- modules/viz/src/q/viz3d_impl.hpp | 4 ++ modules/viz/src/simple_widgets.cpp | 3 +- modules/viz/src/viz3d.cpp | 15 +++++ modules/viz/src/viz3d_impl.cpp | 66 +++++++++++++++++++++ modules/viz/test/test_viz3d.cpp | 17 +++--- 7 files changed, 102 insertions(+), 9 deletions(-) diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 2810d2ab1f..65065bc7f9 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -48,6 +48,10 @@ namespace temp_viz void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity()); bool removeWidget(const String &id); + + bool setWidgetPose(const String &id, const Affine3f &pose); + bool updateWidgetPose(const String &id, const Affine3f &pose); + Affine3f getWidgetPose(const String &id) const; private: Viz3d(const Viz3d&); Viz3d& operator=(const Viz3d&); diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index dd708491ec..ecd2391414 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -108,7 +108,7 @@ namespace temp_viz class CV_EXPORTS CloudNormalsWidget : public Widget { public: - CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f); + CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level = 100, float scale = 0.02f, const Color &color = Color::white()); private: struct ApplyCloudNormals; }; diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index 89d8f6b23c..6595416bbb 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -202,6 +202,10 @@ public: void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity()); bool removeWidget(const String &id); + bool setWidgetPose(const String &id, const Affine3f &pose); + bool updateWidgetPose(const String &id, const Affine3f &pose); + Affine3f getWidgetPose(const String &id) const; + void all_data(); private: diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 700543628d..6c6cba83f0 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -616,7 +616,7 @@ struct temp_viz::CloudNormalsWidget::ApplyCloudNormals } }; -temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale) +temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _normals, int level, float scale, const Color &color) { Mat cloud = _cloud.getMat(); Mat normals = _normals.getMat(); @@ -663,4 +663,5 @@ temp_viz::CloudNormalsWidget::CloudNormalsWidget(InputArray _cloud, InputArray _ vtkLODActor * actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(*this)); actor->SetMapper(mapper); + setColor(color); } \ No newline at end of file diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index bed07a707a..ed9405e913 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -88,3 +88,18 @@ bool temp_viz::Viz3d::removeWidget(const String &id) { return impl_->removeWidget(id); } + +bool temp_viz::Viz3d::setWidgetPose(const String &id, const Affine3f &pose) +{ + return impl_->setWidgetPose(id, pose); +} + +bool temp_viz::Viz3d::updateWidgetPose(const String &id, const Affine3f &pose) +{ + return impl_->updateWidgetPose(id, pose); +} + +temp_viz::Affine3f temp_viz::Viz3d::getWidgetPose(const String &id) const +{ + return impl_->getWidgetPose(id); +} diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index bf3f4f95bd..c77fda74fb 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -899,3 +899,69 @@ bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id) widget_actor_map_->erase(wam_itr); return true; } + +bool 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(); + if (!exists) + { + return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); + actor->SetUserMatrix (matrix); + actor->Modified (); + return true; + } + return false; +} + +bool 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(); + if (!exists) + { + return std::cout << "[setWidgetPose] A widget with id <" << id << "> does not exist!" << std::endl, false; + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = actor->GetUserMatrix(); + if (!matrix) + { + setWidgetPose(id, pose); + return true; + } + Matx44f matrix_cv = convertToMatx(matrix); + + Affine3f updated_pose = pose * Affine3f(matrix_cv); + matrix = convertToVtkMatrix(updated_pose.matrix); + + actor->SetUserMatrix (matrix); + actor->Modified (); + return true; + } + return false; +} + +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(); + if (!exists) + { + return Affine3f(); + } + vtkLODActor *actor; + if ((actor = vtkLODActor::SafeDownCast(wam_itr->second.actor))) + { + vtkSmartPointer matrix = actor->GetUserMatrix(); + Matx44f matrix_cv = convertToMatx(matrix); + return Affine3f(matrix_cv); + } + return Affine3f(); +} \ No newline at end of file diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index ad4ceb822d..76993d160d 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -52,18 +52,16 @@ cv::Mat cvcloud_load() { - cv::Mat cloud(1, 20000, CV_64FC4); - std::ifstream ifs("cloud_dragon.ply"); + cv::Mat cloud(1, 20000, CV_32FC3); + std::ifstream ifs("d:/cloud_dragon.ply"); std::string str; for(size_t i = 0; i < 11; ++i) std::getline(ifs, str); - cv::Vec4d* data = cloud.ptr(); - for(size_t i = 0; i < 20000; ++i){ - ifs >> data[i][0] >> data[i][1] >> data[i][2]; - data[i][3] = 1.0; - } + cv::Point3f* data = cloud.ptr(); + for(size_t i = 0; i < 20000; ++i) + ifs >> data[i].x >> data[i].y >> data[i].z; return cloud; } @@ -148,6 +146,11 @@ TEST(Viz_viz3d, accuracy) // v.showWidget("pcw2",pcw2, cloudPosition2); // v.showWidget("plane", pw, cloudPosition); + 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)); + angle_x += 0.1f; angle_y -= 0.1f; angle_z += 0.1f;