set/get/updateWidgetPose implemented, cloudNormals with color

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&);

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;
     };

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:

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

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);
+}

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<vtkMatrix4x4> 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<vtkMatrix4x4> 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<vtkMatrix4x4> matrix = actor->GetUserMatrix();
+        Matx44f matrix_cv = convertToMatx(matrix);
+        return Affine3f(matrix_cv);
+    }
+    return Affine3f();
+}
\ No newline at end of file

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<cv::Vec4d>();
-    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<cv::Point3f>();
+    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;