showWidget takes Affine3f as argument

modules/viz/include/opencv2/viz/viz3d.hpp

@@ -46,7 +46,7 @@ namespace temp_viz
 
         bool wasStopped() const;
         
-        void showWidget(const String &id, const Widget &widget);
+        void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity());
         bool removeWidget(const String &id);
     private:
         Viz3d(const Viz3d&);

modules/viz/src/q/viz3d_impl.hpp

@@ -199,7 +199,7 @@ public:
     void setPosition (int x, int y);
     void setSize (int xw, int yw);
     
-    void showWidget(const String &id, const Widget &widget);
+    void showWidget(const String &id, const Widget &widget, const Affine3f &pose = Affine3f::Identity());
     bool removeWidget(const String &id);
     
     void all_data();

modules/viz/src/viz3d.cpp

@@ -79,9 +79,9 @@ void temp_viz::Viz3d::registerMouseCallback(void (*callback)(const MouseEvent&,
 
 bool temp_viz::Viz3d::wasStopped() const { return impl_->wasStopped(); }
 
-void temp_viz::Viz3d::showWidget(const String &id, const Widget &widget)
+void temp_viz::Viz3d::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
 {
-    impl_->showWidget(id, widget);
+    impl_->showWidget(id, widget, pose);
 }
 
 bool temp_viz::Viz3d::removeWidget(const String &id)

modules/viz/src/viz3d_impl.cpp

@@ -864,7 +864,7 @@ bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& co
     return (true);
 }
 
-void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget)
+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();
@@ -873,8 +873,13 @@ void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget
         // Remove it if it exists and add it again
         removeActorFromRenderer(wam_itr->second.actor);
     }
-    renderer_->AddActor(WidgetAccessor::getActor(widget));
-    (*widget_actor_map_)[id].actor = WidgetAccessor::getActor(widget);
+    // Get the actor and set the user matrix
+    vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(widget));
+    vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
+    actor->SetUserMatrix (matrix);
+    actor->Modified();
+    renderer_->AddActor(actor);
+    (*widget_actor_map_)[id].actor = actor;
 }
 
 bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id)

modules/viz/test/test_viz3d.cpp

@@ -121,13 +121,15 @@ TEST(Viz_viz3d, accuracy)
         pw.setColor(temp_viz::Color(col_blue, col_green, col_red));
         
         sw.setPose(cloudPosition);
-        pw.setPose(cloudPosition);
+//         pw.setPose(cloudPosition);
         aw.setPose(cloudPosition);
         cw.setPose(cloudPosition);
         cyw.setPose(cloudPosition);
         lw.setPose(cloudPosition);
         cuw.setPose(cloudPosition);        
         
+        v.showWidget("plane", pw, cloudPosition);
+        
         angle_x += 0.1f;
         angle_y -= 0.1f;
         angle_z += 0.1f;