Merge pull request #14 from ozantonkal/implementing_widgets

Implementing widgets

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

@@ -81,7 +81,7 @@ namespace temp_viz
     class CV_EXPORTS CubeWidget : public Widget
     {
     public:
-        CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color = Color::white());
+        CubeWidget(const Point3f& pt_min, const Point3f& pt_max, bool wire_frame = true, const Color &color = Color::white());
     };
     
     class CV_EXPORTS CoordinateSystemWidget : public Widget
@@ -97,4 +97,21 @@ namespace temp_viz
         
         // TODO Overload setColor method, and hide setPose, updatePose, getPose methods
     };
+    
+    class CV_EXPORTS CloudWidget : public Widget
+    {
+    public:
+        CloudWidget(InputArray _cloud, InputArray _colors);
+        CloudWidget(InputArray _cloud, const Color &color = Color::white());
+    private:
+        struct CreateCloudWidget;
+    };
+    
+    class CV_EXPORTS CloudNormalsWidget : public Widget
+    {
+    public:
+        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/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

@@ -874,7 +874,7 @@ void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget
         removeActorFromRenderer(wam_itr->second.actor);
     }
     // Get the actor and set the user matrix
-    vtkSmartPointer<vtkLODActor> actor;
+    vtkLODActor *actor;
     if (actor = vtkLODActor::SafeDownCast(WidgetAccessor::getActor(widget)))
     {
         vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
@@ -889,9 +889,7 @@ bool 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();
-    
-    if (!exists)
-        return std::cout << "[removeWidget] A widget with id <" << id << "> does not exist!" << std::endl, false;
+    CV_Assert(exists);
 
     if (!removeActorFromRenderer (wam_itr->second.actor))
         return false;
@@ -899,3 +897,63 @@ 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();
+    CV_Assert(exists);
+    
+    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();
+    CV_Assert(exists);
+    
+    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();
+    CV_Assert(exists);
+    
+    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/src/widget.cpp

@@ -18,7 +18,7 @@ public:
 
     void setColor(const Color& color)
     {
-        vtkSmartPointer<vtkLODActor> lod_actor = vtkLODActor::SafeDownCast(actor);
+        vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor);
         Color c = vtkcolor(color);
         lod_actor->GetMapper ()->ScalarVisibilityOff ();
         lod_actor->GetProperty ()->SetColor (c.val);
@@ -32,7 +32,7 @@ public:
 
     void setPose(const Affine3f& pose)
     {
-        vtkSmartPointer<vtkLODActor> lod_actor = vtkLODActor::SafeDownCast(actor);
+        vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor);
         vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
         lod_actor->SetUserMatrix (matrix);
         lod_actor->Modified ();
@@ -40,7 +40,7 @@ public:
 
     void updatePose(const Affine3f& pose)
     {
-        vtkSmartPointer<vtkLODActor> lod_actor = vtkLODActor::SafeDownCast(actor);
+        vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor);
         vtkSmartPointer<vtkMatrix4x4> matrix = lod_actor->GetUserMatrix();
         if (!matrix)
         {
@@ -58,7 +58,7 @@ public:
 
     Affine3f getPose() const
     {
-        vtkSmartPointer<vtkLODActor> lod_actor = vtkLODActor::SafeDownCast(actor);
+        vtkLODActor *lod_actor = vtkLODActor::SafeDownCast(actor);
         vtkSmartPointer<vtkMatrix4x4> matrix = lod_actor->GetUserMatrix();
         Matx44f matrix_cv = convertToMatx(matrix);
         return Affine3f(matrix_cv);

modules/viz/test/test_viz3d.cpp

@@ -96,26 +96,38 @@ TEST(Viz_viz3d, accuracy)
     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::Color(0,0,255));
+    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::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("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("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);
     
     while(!v.wasStopped())
     {
         // Creating new point cloud with id cloud1
         cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z));
+        cv::Affine3f cloudPosition2(angle_x, angle_y, angle_z, cv::Vec3f(pos_x+0.2, pos_y+0.2, pos_z+0.2));
 
         lw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
         lw.setLineWidth(lw.getLineWidth()+pos_x * 10);
@@ -128,9 +140,16 @@ TEST(Viz_viz3d, accuracy)
         cw.setPose(cloudPosition);
         cyw.setPose(cloudPosition);
         lw.setPose(cloudPosition);
-        cuw.setPose(cloudPosition);        
+        cuw.setPose(cloudPosition);
+//         cnw.setPose(cloudPosition);
+//         v.showWidget("pcw",pcw, cloudPosition);
+//         v.showWidget("pcw2",pcw2, cloudPosition2);
+//         v.showWidget("plane", pw, cloudPosition);
         
-        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;
@@ -145,9 +164,7 @@ TEST(Viz_viz3d, accuracy)
         v.spinOnce(1, true);
     }
    
-//     cv::Mat normals(cloud.size(), CV_32FC3, cv::Scalar(0, 10, 0));
-// 
-//     v.addPointCloudNormals(cloud, normals, 100, 0.02, "n");
+
 // 
 // 
 //     temp_viz::ModelCoefficients mc;