Merge pull request #17 from ozantonkal/implementing_widgets

Implementing widgets

modules/viz/include/opencv2/viz.hpp

@@ -64,7 +64,7 @@ namespace cv
         CV_EXPORTS Affine3f makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& axis_y, const Vec3f& axis_z, const Vec3f& origin = Vec3f::all(0));
 
         //! constructs camera pose from position, focal_point and up_vector (see gluLookAt() for more infromation
-        CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& up_vector);
+        CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir);
 
 
         //! checks float value for Nan

modules/viz/include/opencv2/viz/widgets.hpp

 #pragma once
 
 #include <opencv2/viz/types.hpp>
+#include <common.h>
 
 
 namespace cv
@@ -70,6 +71,8 @@ namespace cv
         public:
             PlaneWidget(const Vec4f& coefs, double size = 1.0, const Color &color = Color::white());
             PlaneWidget(const Vec4f& coefs, const Point3f& pt, double size = 1.0, const Color &color = Color::white());
+        private:
+            struct SetSizeImpl;
         };
 
         class CV_EXPORTS SphereWidget : public Widget3D
@@ -81,7 +84,7 @@ namespace cv
         class CV_EXPORTS ArrowWidget : public Widget3D
         {
         public:
-            ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color = Color::white());
+            ArrowWidget(const Point3f& pt1, const Point3f& pt2, double thickness = 0.03, const Color &color = Color::white());
         };
 
         class CV_EXPORTS CircleWidget : public Widget3D
@@ -120,7 +123,12 @@ namespace cv
         class CV_EXPORTS GridWidget : public Widget3D
         {
         public:
-            GridWidget(Vec2i dimensions, Vec2d spacing, const Color &color = Color::white());
+            GridWidget(const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white());
+            GridWidget(const Vec4f &coeffs, const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white());
+            
+        private:
+            struct GridImpl;
+        
         };
 
         class CV_EXPORTS Text3DWidget : public Widget3D
@@ -140,6 +148,48 @@ namespace cv
             void setText(const String &text);
             String getText() const;
         };
+        
+        class CV_EXPORTS ImageOverlayWidget : public Widget2D
+        {
+        public:
+            ImageOverlayWidget(const Mat &image, const Rect &rect);
+            
+            void setImage(const Mat &image);
+        };
+        
+        class CV_EXPORTS Image3DWidget : public Widget3D
+        {
+        public:
+            Image3DWidget(const Mat &image, const Size &size);
+            Image3DWidget(const Vec3f &position, const Vec3f &normal, const Vec3f &up_vector, const Mat &image, const Size &size);
+            
+            void setImage(const Mat &image);
+        };
+        
+        class CV_EXPORTS CameraPositionWidget : public Widget3D
+        {
+        public:
+            CameraPositionWidget(double scale = 1.0);
+            CameraPositionWidget(const Matx33f &K, double scale = 1.0, const Color &color = Color::white());
+            CameraPositionWidget(const Vec2f &fov, double scale = 1.0, const Color &color = Color::white());
+            CameraPositionWidget(const Matx33f &K, const Mat &img, double scale = 1.0, const Color &color = Color::white());
+            
+        };
+        
+        class CV_EXPORTS TrajectoryWidget : public Widget3D
+        {
+        public:
+            enum {DISPLAY_FRAMES = 1, DISPLAY_PATH = 2};
+            
+            TrajectoryWidget(const std::vector<Affine3f> &path, int display_mode = TrajectoryWidget::DISPLAY_PATH, const Color &color = Color::white(), double scale = 1.0);
+            TrajectoryWidget(const std::vector<Affine3f> &path, float line_length, double init_sphere_radius,
+                             double sphere_radius, const Color &line_color = Color::white(), const Color &sphere_color = Color::white());
+            TrajectoryWidget(const std::vector<Affine3f> &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
+            TrajectoryWidget(const std::vector<Affine3f> &path, const Vec2f &fov, double scale = 1.0, const Color &color = Color::white()); // Camera frustums
+            
+        private:
+            struct ApplyPath;
+        };
 
         class CV_EXPORTS CloudWidget : public Widget3D
         {
@@ -151,6 +201,18 @@ namespace cv
             struct CreateCloudWidget;
         };
 
+        class CV_EXPORTS CloudCollectionWidget : public Widget3D
+        {
+        public:
+            CloudCollectionWidget();
+            
+            void addCloud(InputArray cloud, InputArray colors, const Affine3f &pose = Affine3f::Identity());
+            void addCloud(InputArray cloud, const Color &color = Color::white(), const Affine3f &pose = Affine3f::Identity());
+            
+        private:
+            struct CreateCloudWidget;
+        };
+        
         class CV_EXPORTS CloudNormalsWidget : public Widget3D
         {
         public:
@@ -183,7 +245,12 @@ namespace cv
         template<> CV_EXPORTS GridWidget Widget::cast<GridWidget>();
         template<> CV_EXPORTS Text3DWidget Widget::cast<Text3DWidget>();
         template<> CV_EXPORTS TextWidget Widget::cast<TextWidget>();
+        template<> CV_EXPORTS ImageOverlayWidget Widget::cast<ImageOverlayWidget>();
+        template<> CV_EXPORTS Image3DWidget Widget::cast<Image3DWidget>();
+        template<> CV_EXPORTS CameraPositionWidget Widget::cast<CameraPositionWidget>();
+        template<> CV_EXPORTS TrajectoryWidget Widget::cast<TrajectoryWidget>();
         template<> CV_EXPORTS CloudWidget Widget::cast<CloudWidget>();
+        template<> CV_EXPORTS CloudCollectionWidget Widget::cast<CloudCollectionWidget>();
         template<> CV_EXPORTS CloudNormalsWidget Widget::cast<CloudNormalsWidget>();
         template<> CV_EXPORTS MeshWidget Widget::cast<MeshWidget>();
 

modules/viz/src/precomp.hpp

@@ -43,6 +43,7 @@
 #include <vtkPlaneSource.h>
 #include <vtkSphereSource.h>
 #include <vtkArrowSource.h>
+#include <vtkOutlineSource.h>
 #include <vtkIdentityTransform.h>
 #include <vtkTransform.h>
 #include <vtkTransformPolyDataFilter.h>
@@ -107,6 +108,7 @@
 #include <vtkImageCanvasSource2D.h>
 #include <vtkImageBlend.h>
 #include <vtkImageStencilData.h>
+#include <vtkImageActor.h>
 
 #include <vtkRenderWindowInteractor.h>
 #include <vtkChartXY.h>
@@ -131,7 +133,9 @@
 #include <vtkImageReader2Factory.h>
 #include <vtkImageReader2.h>
 #include <vtkImageData.h>
-
+#include <vtkExtractEdges.h>
+#include <vtkFrustumSource.h>
+#include <vtkTextureMapToPlane.h>
 
 #include <vtkPolyDataNormals.h>
 #include <vtkMapper.h>

modules/viz/src/types.cpp

@@ -111,7 +111,7 @@ struct cv::viz::Mesh3d::loadMeshImpl
                 Vec3b point_color;
                 poly_colors->GetTupleValue (i, point_color.val);
 
-                //RGB or BGR? should we swap channels????
+                std::swap(point_color[0], point_color[2]); // RGB -> BGR
                 mesh_colors[i] = point_color;
             }
         }

modules/viz/src/viz.cpp

@@ -19,6 +19,32 @@ cv::Affine3f cv::viz::makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& ax
     return Affine3f(R, origin);
 }
 
+cv::Affine3f cv::viz::makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir)
+{
+    // Compute the transformation matrix for drawing the camera frame in a scene
+    Vec3f u,v,n;
+    n = normalize(focal_point - position);
+    u = normalize(y_dir.cross(n));
+    v = n.cross(u);
+    
+    Matx44f pose_mat;
+    pose_mat.zeros();
+    pose_mat(0,0) = u[0];
+    pose_mat(0,1) = u[1];
+    pose_mat(0,2) = u[2];
+    pose_mat(1,0) = v[0];
+    pose_mat(1,1) = v[1];
+    pose_mat(1,2) = v[2];
+    pose_mat(2,0) = n[0];
+    pose_mat(2,1) = n[1];
+    pose_mat(2,2) = n[2];
+    pose_mat(3,0) = position[0];
+    pose_mat(3,1) = position[1];
+    pose_mat(3,2) = position[2];
+    pose_mat(3,3) = 1.0f;
+    pose_mat = pose_mat.t();
+    return pose_mat;
+}
 
 vtkSmartPointer<vtkMatrix4x4> cv::viz::convertToVtkMatrix (const cv::Matx44f &m)
 {

modules/viz/src/viz3d_impl.cpp

@@ -847,7 +847,7 @@ bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename,
     return (true);
 }
 
-bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& /*mesh*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
 // 
@@ -1017,7 +1017,7 @@ void cv::viz::Viz3d::VizImpl::setWindowName (const std::string &name)
 void cv::viz::Viz3d::VizImpl::setWindowPosition (int x, int y) { window_->SetPosition (x, y); }
 void cv::viz::Viz3d::VizImpl::setWindowSize (int xw, int yw) { window_->SetSize (xw, yw); }
 
-bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& /*mesh*/, const Mat& /*mask*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
 //     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
@@ -1205,7 +1205,7 @@ return true;
 }
 
 
-bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id)
+bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& /*mesh*/, const cv::Mat& /*mask*/, const std::string &/*id*/)
 {
 //     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
 //     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));

modules/viz/src/viz3d_impl.hpp

@@ -327,6 +327,31 @@ namespace cv
                     }
                     return output;
                 }
+                
+                static _Out* copyColor(const Mat& source, _Out* output, const Mat& nan_mask)
+                {
+                    CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size());
+                    CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4);
+                    CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth());
+
+                    int s_chs = source.channels();
+                    int m_chs = nan_mask.channels();
+
+                    for(int y = 0; y < source.rows; ++y)
+                    {
+                        const _Tp* srow = source.ptr<_Tp>(y);
+                        const _Msk* mrow = nan_mask.ptr<_Msk>(y);
+
+                        for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs)
+                            if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2]))
+                            {
+                                *output = _Out(srow);
+                                std::swap((*output)[0], (*output)[2]); // BGR -> RGB
+                                ++output;
+                            }
+                    }
+                    return output;
+                }
             };
 
             template<typename _Tp>
@@ -339,6 +364,17 @@ namespace cv
 
                 return table[nan_mask.depth() - 5](source, output, nan_mask);
             }
+            
+            template<typename _Tp>
+            static inline Vec<_Tp, 3>* copyColor(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask)
+            {
+                CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F);
+
+                typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&);
+                const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copyColor, &NanFilter::Impl<_Tp, double>::copyColor };
+
+                return table[nan_mask.depth() - 5](source, output, nan_mask);
+            }
         };
 
         struct ApplyAffine
@@ -374,6 +410,63 @@ namespace cv
 
         inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); }
         template<typename _Tp> inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); }
+        
+        struct ConvertToVtkImage
+        {
+            struct Impl
+            {
+                static void copyImageMultiChannel(const Mat &image, vtkSmartPointer<vtkImageData> output)
+                {
+                    int i_chs = image.channels();
+            
+                    for (int i = 0; i < image.rows; ++i)
+                    {
+                        const unsigned char * irows = image.ptr<unsigned char>(i);
+                        for (int j = 0; j < image.cols; ++j, irows += i_chs)
+                        {
+                            unsigned char * vrows = static_cast<unsigned char *>(output->GetScalarPointer(j,i,0));
+                            memcpy(vrows, irows, i_chs);
+                            std::swap(vrows[0], vrows[2]); // BGR -> RGB
+                        }
+                    }
+                    output->Modified();
+                }
+                
+                static void copyImageSingleChannel(const Mat &image, vtkSmartPointer<vtkImageData> output)
+                {
+                    for (int i = 0; i < image.rows; ++i)
+                    {
+                        const unsigned char * irows = image.ptr<unsigned char>(i);
+                        for (int j = 0; j < image.cols; ++j, ++irows)
+                        {
+                            unsigned char * vrows = static_cast<unsigned char *>(output->GetScalarPointer(j,i,0));
+                            *vrows = *irows;
+                        }
+                    }
+                    output->Modified();
+                }
+            };
+            
+            static void convert(const Mat &image, vtkSmartPointer<vtkImageData> output)
+            {
+                // Create the vtk image
+                output->SetDimensions(image.cols, image.rows, 1);
+                output->SetNumberOfScalarComponents(image.channels());
+                output->SetScalarTypeToUnsignedChar();
+                output->AllocateScalars();
+                
+                int i_chs = image.channels();
+                if (i_chs > 1)
+                {
+                    // Multi channel images are handled differently because of BGR <-> RGB
+                    Impl::copyImageMultiChannel(image, output);
+                }
+                else
+                {
+                    Impl::copyImageSingleChannel(image, output);
+                }
+            }
+        };
     }
 
 }

modules/viz/src/widget.cpp

@@ -142,10 +142,6 @@ void cv::viz::Widget3D::setColor(const Color &color)
     actor->GetMapper ()->ScalarVisibilityOff ();
     actor->GetProperty ()->SetColor (c.val);
     actor->GetProperty ()->SetEdgeColor (c.val);
-    actor->GetProperty ()->SetAmbient (0.8);
-    actor->GetProperty ()->SetDiffuse (0.8);
-    actor->GetProperty ()->SetSpecular (0.8);
-    actor->GetProperty ()->SetLighting (0);
     actor->Modified ();
 }
 

modules/viz/test/test_viz3d.cpp

@@ -43,6 +43,7 @@
 #include <opencv2/viz.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
 
 #include <fstream>
 #include <string>
@@ -88,9 +89,11 @@ TEST(Viz_viz3d, accuracy)
     viz::Color color = viz::Color::black();
 
     viz::LineWidget lw(Point3f(0, 0, 0), Point3f(4.f, 4.f,4.f), viz::Color::green());
-    viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0), 5.0);
-    viz::SphereWidget sw(Point3f(0, 0, 0), 0.5);
-    viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), viz::Color::red());
+    viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0));
+    viz::PlaneWidget pw2(Vec4f(0.0,1.0,2.0,3.0), 2.0, viz::Color::red());
+    viz::PlaneWidget pw3(Vec4f(0.0,1.0,2.0,3.0), 3.0, viz::Color::blue());
+    viz::SphereWidget sw(Point3f(0, 0, 0), 0.2);
+    viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), 0.01, viz::Color::red());
     viz::CircleWidget cw(Point3f(0, 0, 0), 0.5, 0.01, viz::Color::green());
     viz::CylinderWidget cyw(Point3f(0, 0, 0), Point3f(-1, -1, -1), 0.5, 30, viz::Color::green());
     viz::CubeWidget cuw(Point3f(-2, -2, -2), Point3f(-1, -1, -1));
@@ -99,18 +102,20 @@ TEST(Viz_viz3d, accuracy)
     viz::CloudWidget pcw(cloud, colors);
     viz::CloudWidget pcw2(cloud, viz::Color::magenta());
     
-    viz.showWidget("line", lw);
+//     viz.showWidget("line", lw);
     viz.showWidget("plane", pw);
-    viz.showWidget("sphere", sw);
-    viz.showWidget("arrow", aw);
-    viz.showWidget("circle", cw);
-    viz.showWidget("cylinder", cyw);
-    viz.showWidget("cube", cuw);
+    viz.showWidget("plane2", pw2);
+    viz.showWidget("plane3", pw3);
+//     viz.showWidget("sphere", sw);
+//     viz.showWidget("arrow", aw);
+//     viz.showWidget("circle", cw);
+//     viz.showWidget("cylinder", cyw);
+//     viz.showWidget("cube", cuw);
     viz.showWidget("coordinateSystem", csw);
-    viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
-    viz.showWidget("text",tw);
-    viz.showWidget("pcw",pcw);
-    viz.showWidget("pcw2",pcw2);
+//     viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0)));
+//     viz.showWidget("text",tw);
+//     viz.showWidget("pcw",pcw);
+//     viz.showWidget("pcw2",pcw2);
     
 //     viz::LineWidget lw2 = lw;
 //     v.showPointCloud("cld",cloud, colors);
@@ -125,13 +130,55 @@ TEST(Viz_viz3d, accuracy)
     
     
     viz::PolyLineWidget plw(points, viz::Color::green());
-    viz.showWidget("polyline", plw);
+//     viz.showWidget("polyline", plw);
 //     lw = v.getWidget("polyline").cast<viz::LineWidget>();
     
-    viz::Mesh3d::Ptr mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
+    viz::Mesh3d mesh = cv::viz::Mesh3d::loadMesh("horse.ply");
     
-    viz::MeshWidget mw(*mesh);
-    viz.showWidget("mesh", mw);
+    viz::MeshWidget mw(mesh);
+//     viz.showWidget("mesh", mw);
+    
+    Mat img = imread("opencv.png");
+//     resize(img, img, Size(50,50));
+//     viz.showWidget("img", viz::ImageOverlayWidget(img, Point2i(50,50)));
+    
+    Matx33f K(657, 0, 320, 
+              0, 657, 240, 
+              0, 0, 1);
+    
+    viz::CameraPositionWidget cpw(Vec3f(0.5, 0.5, 3.0), Vec3f(0.0,0.0,0.0), Vec3f(0.0,-1.0,0.0), 0.5);
+    viz::CameraPositionWidget cpw2(0.5);
+    viz::CameraPositionWidget frustum(K, 2.0, viz::Color::green());
+//     viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
+    viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red());
+    viz::CameraPositionWidget frustum3(Vec2f(CV_PI, CV_PI/2), 4.0);
+    viz::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1);
+    viz::Text3DWidget t3w2("Camera2", Point3f(0,0,0), 0.1);
+    
+//     viz.showWidget("CameraPositionWidget", cpw);
+//     viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("camera_label", t3w1);
+//     viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+//     viz.showWidget("frustum3", frustum3, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5)));
+    
+    std::vector<Affine3f> trajectory;
+    
+    trajectory.push_back(Affine3f().translate(Vec3f(0.5,0.5,0.5)));
+    trajectory.push_back(Affine3f().translate(Vec3f(1.0,0.0,0.0)));
+    trajectory.push_back(Affine3f().translate(Vec3f(2.0,0.5,0.0)));
+    trajectory.push_back(Affine3f(0.5, 0.0, 0.0, Vec3f(1.0,0.0,1.0)));
+//     
+    viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory, viz::Color(0,255,255), true, 0.5));
+    viz.showWidget("trajectory2", viz::TrajectoryWidget(trajectory, K, 1.0, viz::Color(255,0,255)));
+    
+    
+    
+//     viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory/*, viz::Color::yellow()*/));
+    
+//     viz.showWidget("CameraPositionWidget2", cpw2);
+//     viz.showWidget("CameraPositionWidget3", cpw3);
     
     viz.spin();
 
@@ -156,13 +203,16 @@ TEST(Viz_viz3d, accuracy)
         
         //plw.setColor(viz::Color(col_blue, col_green, col_red));
         
-        sw.setPose(cloudPosition);
+//         sw.setPose(cloudPosition);
 //         pw.setPose(cloudPosition);
         aw.setPose(cloudPosition);
         cw.setPose(cloudPosition);
         cyw.setPose(cloudPosition);
+        
+        frustum.setPose(cloudPosition);
 //         lw.setPose(cloudPosition);
-        cuw.setPose(cloudPosition);
+//         cpw.updatePose(Affine3f(0.1,0.0,0.0, cv::Vec3f(0.0,0.0,0.0)));
+//         cpw.setPose(cloudPosition);
 //         cnw.setPose(cloudPosition);
 //         v.showWidget("pcw",pcw, cloudPosition);
 //         v.showWidget("pcw2",pcw2, cloudPosition2);