From c4f4727ee5f52f3542d2db4627fce7f0d9669926 Mon Sep 17 00:00:00 2001
From: ozantonkal <ozantonkal@gmail.com>
Date: Mon, 8 Jul 2013 10:53:00 +0300
Subject: [PATCH] showWidget takes Affine3f as argument

---
 modules/viz/include/opencv2/viz/viz3d.hpp |  2 +-
 modules/viz/src/q/viz3d_impl.hpp          |  2 +-
 modules/viz/src/viz3d.cpp                 |  4 ++--
 modules/viz/src/viz3d_impl.cpp            | 11 ++++++++---
 modules/viz/test/test_viz3d.cpp           |  4 +++-
 5 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp
index b4b697b90..2810d2ab1 100644
--- a/modules/viz/include/opencv2/viz/viz3d.hpp
+++ b/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&);
diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp
index f4a85e218..89d8f6b23 100644
--- a/modules/viz/src/q/viz3d_impl.hpp
+++ b/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();
diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp
index e3ebf85bf..bed07a707 100644
--- a/modules/viz/src/viz3d.cpp
+++ b/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)
diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp
index 4d51964c7..07a25abc7 100644
--- a/modules/viz/src/viz3d_impl.cpp
+++ b/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)
diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp
index d9d6837c0..e574449cf 100644
--- a/modules/viz/test/test_viz3d.cpp
+++ b/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;