added GlArrays class and pointCloudShow function

modules/core/include/opencv2/core/gpumat.hpp

@@ -241,11 +241,15 @@ namespace cv { namespace gpu
         GlBuffer(Size size, int type, Usage usage);
 
         //! copy from host/device memory
-        GlBuffer(const Mat& mat, Usage usage);
+        GlBuffer(InputArray mat, Usage usage);
         GlBuffer(const GpuMat& d_mat, Usage usage);
 
+        GlBuffer(const GlBuffer& other);
+
         ~GlBuffer();
 
+        GlBuffer& operator =(const GlBuffer& other);
+
         void create(int rows, int cols, int type, Usage usage);
         inline void create(Size size, int type, Usage usage) { create(size.height, size.width, type, usage); }
         inline void create(int rows, int cols, int type) { create(rows, cols, type, usage()); }
@@ -254,7 +258,7 @@ namespace cv { namespace gpu
         void release();
 
         //! copy from host/device memory
-        void copyFrom(const Mat& mat);
+        void copyFrom(InputArray mat);
         void copyFrom(const GpuMat& d_mat);
 
         void bind() const;
@@ -267,11 +271,12 @@ namespace cv { namespace gpu
         //! map to device memory
         GpuMat mapDevice();
         void unmapDevice();
+        
+        int rows;
+        int cols;
 
-        inline int rows() const { return rows_; }
-        inline int cols() const { return cols_; }
-        inline Size size() const { return Size(cols_, rows_); }
-        inline bool empty() const { return rows_ == 0 || cols_ == 0; }
+        inline Size size() const { return Size(cols, rows); }
+        inline bool empty() const { return rows == 0 || cols == 0; }
 
         inline int type() const { return type_; }
         inline int depth() const { return CV_MAT_DEPTH(type_); }
@@ -282,8 +287,6 @@ namespace cv { namespace gpu
         inline Usage usage() const { return usage_; }
 
     private:
-        int rows_;
-        int cols_;
         int type_;
         Usage usage_;
 
@@ -303,26 +306,31 @@ namespace cv { namespace gpu
         GlTexture(Size size, int type);
 
         //! copy from host/device memory
-        explicit GlTexture(const Mat& mat, bool bgra = true);
+        explicit GlTexture(InputArray mat, bool bgra = true);
         explicit GlTexture(const GlBuffer& buf, bool bgra = true);
 
+        GlTexture(const GlTexture& other);
+
         ~GlTexture();
 
+        GlTexture& operator =(const GlTexture& other);
+
         void create(int rows, int cols, int type);
         inline void create(Size size, int type) { create(size.height, size.width, type); }
         void release();
 
         //! copy from host/device memory
-        void copyFrom(const Mat& mat, bool bgra = true);
+        void copyFrom(InputArray mat, bool bgra = true);
         void copyFrom(const GlBuffer& buf, bool bgra = true);
 
         void bind() const;
         void unbind() const;
 
-        inline int rows() const { return rows_; }
-        inline int cols() const { return cols_; }
-        inline Size size() const { return Size(cols_, rows_); }
-        inline bool empty() const { return rows_ == 0 || cols_ == 0; }
+        int rows;
+        int cols;
+
+        inline Size size() const { return Size(cols, rows); }
+        inline bool empty() const { return rows == 0 || cols == 0; }
 
         inline int type() const { return type_; }
         inline int depth() const { return CV_MAT_DEPTH(type_); }
@@ -331,22 +339,136 @@ namespace cv { namespace gpu
         inline int elemSize1() const { return CV_ELEM_SIZE1(type_); }
 
     private:
-        int rows_;
-        int cols_;
         int type_;
 
         class Impl;
         Ptr<Impl> impl_;
     };
 
+    //! OpenGL Arrays
+    class CV_EXPORTS GlArrays
+    {
+    public:
+        inline GlArrays() 
+            : vertex_(GlBuffer::ARRAY_BUFFER), color_(GlBuffer::ARRAY_BUFFER), bgra_(true), normal_(GlBuffer::ARRAY_BUFFER), texCoord_(GlBuffer::ARRAY_BUFFER)
+        {
+        }
+
+        void setVertexArray(const GlBuffer& vertex);
+        void setVertexArray(const GpuMat& vertex);
+        void setVertexArray(InputArray vertex);
+        inline void resetVertexArray() { vertex_.release(); }
+
+        void setColorArray(const GlBuffer& color, bool bgra = true);
+        void setColorArray(const GpuMat& color, bool bgra = true);
+        void setColorArray(InputArray color, bool bgra = true);
+        inline void resetColorArray() { color_.release(); }
+        
+        void setNormalArray(const GlBuffer& normal);
+        void setNormalArray(const GpuMat& normal);
+        void setNormalArray(InputArray normal);
+        inline void resetNormalArray() { normal_.release(); }
+        
+        void setTexCoordArray(const GlBuffer& texCoord);
+        void setTexCoordArray(const GpuMat& texCoord);
+        void setTexCoordArray(InputArray texCoord);
+        inline void resetTexCoordArray() { texCoord_.release(); }
+
+        void bind() const;
+        void unbind() const;
+
+        inline int rows() const { return vertex_.rows; }
+        inline int cols() const { return vertex_.cols; }
+        inline Size size() const { return vertex_.size(); }
+        inline bool empty() const { return vertex_.empty(); }
+
+    private:
+        GlBuffer vertex_;
+        GlBuffer color_;
+        bool bgra_;
+        GlBuffer normal_;
+        GlBuffer texCoord_;
+    };
+
     //! render functions
-    CV_EXPORTS void render(const GlTexture& tex);
+
+    //! render texture rectangle in window
+    CV_EXPORTS void render(const GlTexture& tex, 
+        Rect_<double> wndRect = Rect_<double>(0.0, 0.0, 1.0, 1.0), 
+        Rect_<double> texRect = Rect_<double>(0.0, 0.0, 1.0, 1.0));
+
+    //! render mode
+    namespace RenderMode {
+        enum {
+            POINTS         = 0x0000,
+            LINES          = 0x0001,
+            LINE_LOOP      = 0x0002,
+            LINE_STRIP     = 0x0003,
+            TRIANGLES      = 0x0004,
+            TRIANGLE_STRIP = 0x0005,
+            TRIANGLE_FAN   = 0x0006,
+            QUADS          = 0x0007,
+            QUAD_STRIP     = 0x0008,
+            POLYGON        = 0x0009
+        };
+    }
+
+    //! render OpenGL arrays
+    CV_EXPORTS void render(const GlArrays& arr, int mode = RenderMode::POINTS);
+
+    //! OpenGL camera
+    class CV_EXPORTS GlCamera
+    {
+    public:
+        GlCamera();
+
+        void lookAt(Point3d eye, Point3d center, Point3d up);
+        void setCameraPos(Point3d pos, double yaw, double pitch, double roll);
+
+        void setScale(Point3d scale);
+
+        void setProjectionMatrix(const Mat& projectionMatrix, bool transpose = true);
+        void setPerspectiveProjection(double fov, double aspect, double zNear, double zFar);
+        void setOrthoProjection(double left, double right, double bottom, double top, double zNear, double zFar);
+
+        void setupProjectionMatrix() const;
+        void setupModelViewMatrix() const;
+
+    private:
+        Point3d eye_;
+        Point3d center_;
+        Point3d up_;
+
+        Point3d pos_;
+        double yaw_;
+        double pitch_;
+        double roll_;
+
+        bool useLookAtParams_;
+
+        Point3d scale_;
+
+        Mat projectionMatrix_;
+
+        double fov_;
+        double aspect_;
+
+        double left_;
+        double right_;
+        double bottom_;
+        double top_;
+
+        double zNear_;
+        double zFar_;
+
+        bool perspectiveProjection_;
+    };
 
     //! OpenGL extension table
     class CV_EXPORTS GlFuncTab
     {
     public:
-        virtual ~GlFuncTab() {}
+        virtual ~GlFuncTab();
 
         virtual void genBuffers(int n, unsigned int* buffers) const = 0;        
         virtual void deleteBuffers(int n, const unsigned int* buffers) const = 0;

modules/highgui/include/opencv2/highgui/highgui.hpp

@@ -139,12 +139,17 @@ CV_EXPORTS void setOpenGlContext(const string& winname);
 
 CV_EXPORTS void updateWindow(const string& winname);
 
-CV_EXPORTS void imshow(const string& winname, const gpu::GpuMat& d_mat);
-
-CV_EXPORTS void imshow(const string& winname, const gpu::GlBuffer& buf);
-
 CV_EXPORTS void imshow(const string& winname, const gpu::GlTexture& tex);
+CV_EXPORTS void imshow(const string& winname, const gpu::GlBuffer& buf);
+CV_EXPORTS void imshow(const string& winname, const gpu::GpuMat& d_mat);
 
+CV_EXPORTS void pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GlArrays& arr);
+CV_EXPORTS void pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GlBuffer& points, 
+                               const gpu::GlBuffer& colors = gpu::GlBuffer(gpu::GlBuffer::ARRAY_BUFFER));
+CV_EXPORTS void pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GpuMat& points, 
+                               const gpu::GpuMat& colors = gpu::GpuMat());
+CV_EXPORTS void pointCloudShow(const string& winname, const gpu::GlCamera& camera, InputArray points, 
+                               InputArray colors = InputArray());
 
 //Only for Qt
 

modules/highgui/src/window_w32.cpp

@@ -405,12 +405,12 @@ double cvGetModeWindow_W32(const char* name)//YV
 
     CvWindow* window;
 
-    if(!name)
+    if (!name)
         CV_ERROR( CV_StsNullPtr, "NULL name string" );
 
     window = icvFindWindowByName( name );
-    if( !window )
-        CV_ERROR( CV_StsNullPtr, "NULL window" );
+    if (!window)
+        EXIT; // keep silence here
         
     result = window->status;
         
@@ -503,7 +503,7 @@ double cvGetPropWindowAutoSize_W32(const char* name)
 
     window = icvFindWindowByName( name );
     if (!window)
-        EXIT;
+        EXIT; // keep silence here
 
     result = window->flags & CV_WINDOW_AUTOSIZE;
 
@@ -527,7 +527,7 @@ double cvGetRatioWindow_W32(const char* name)
 
     window = icvFindWindowByName( name );
     if (!window)
-        CV_ERROR( CV_StsNullPtr, "NULL window" );
+        EXIT; // keep silence here
         
     result = static_cast<double>(window->width) / window->height;
         
@@ -552,7 +552,7 @@ double cvGetOpenGlProp_W32(const char* name)
 
     window = icvFindWindowByName( name );
     if (!window)
-        __CV_EXIT__;
+        EXIT; // keep silence here
         
     result = window->useGl;
         
@@ -808,7 +808,7 @@ namespace
             0,                             // Shift Bit Ignored
             0,                             // No Accumulation Buffer
             0, 0, 0, 0,                    // Accumulation Bits Ignored
-            16,                            // 16Bit Z-Buffer (Depth Buffer)  
+            32,                            // 32 Bit Z-Buffer (Depth Buffer)  
             0,                             // No Stencil Buffer
             0,                             // No Auxiliary Buffer
             PFD_MAIN_PLANE,                // Main Drawing Layer
@@ -845,6 +845,8 @@ namespace
 
         __BEGIN__;
 
+        delete window->glFuncTab;
+
         if (window->hGLRC)
         {
             wglDeleteContext(window->hGLRC);
@@ -1177,9 +1179,6 @@ static void icvRemoveWindow( CvWindow* window )
 #ifdef HAVE_OPENGL
     if (window->useGl)
     {
-        delete window->glFuncTab;
-         cv::gpu::setGlFuncTab(0);
-
         wglMakeCurrent(window->dc, window->hGLRC);
 
         if (window->glCleanCallback)

samples/cpp/point_cloud.cpp

+#include <cstring>
+#include <cmath>
+#include <iostream>
+#include "opencv2/core/core.hpp"
+#include "opencv2/core/gpumat.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+
+using namespace std;
+using namespace cv;
+using namespace cv::gpu;
+
+void mouseCallback(int event, int x, int y, int flags, void* userdata)
+{
+    int* dx = static_cast<int*>(userdata);
+    int* dy = dx + 1;
+
+    static int oldx = x;
+    static int oldy = y;
+    static bool moving = false;
+
+    if (event == EVENT_LBUTTONDOWN)
+    {
+        oldx = x;
+        oldy = y;
+        moving = true;
+    }
+    else if (event == EVENT_LBUTTONUP)
+    {
+        moving = false;
+    }
+
+    if (moving)
+    {
+        *dx = oldx - x;
+        *dy = oldy - y;
+    }
+    else
+    {
+        *dx = 0;
+        *dy = 0;
+    }
+}
+
+inline int clamp(int val, int minVal, int maxVal)
+{
+    return max(min(val, maxVal), minVal);
+}
+
+Point3d rotate(Point3d v, double yaw, double pitch)
+{
+    Point3d t1;
+    t1.x = v.x * cos(-yaw / 180.0 * CV_PI) - v.z * sin(-yaw / 180.0 * CV_PI);
+    t1.y = v.y;
+    t1.z = v.x * sin(-yaw / 180.0 * CV_PI) + v.z * cos(-yaw / 180.0 * CV_PI);
+
+    Point3d t2;
+    t2.x = t1.x;
+    t2.y = t1.y * cos(pitch / 180.0 * CV_PI) - t1.z * sin(pitch / 180.0 * CV_PI);
+    t2.z = t1.y * sin(pitch / 180.0 * CV_PI) + t1.z * cos(pitch / 180.0 * CV_PI);
+
+    return t2;
+}
+
+int main(int argc, const char* argv[])
+{
+    const char* keys =
+       "{ l | left      |       | left image file name }"
+       "{ r | right     |       | right image file name }"
+       "{ i | intrinsic |       | intrinsic camera parameters file name }"
+       "{ e | extrinsic |       | extrinsic camera parameters file name }"
+       "{ d | ndisp     | 256   | number of disparities }"
+       "{ s | scale     | 1.0   | scale factor for point cloud }"
+       "{ h | help      | false | print help message }";
+
+    CommandLineParser cmd(argc, argv, keys);
+
+    if (cmd.get<bool>("help"))    
+    {
+        cout << "Avaible options:" << endl;
+        cmd.printParams();
+        return 0;
+    }
+
+    string left = cmd.get<string>("left");
+    string right = cmd.get<string>("right");
+    string intrinsic = cmd.get<string>("intrinsic");
+    string extrinsic = cmd.get<string>("extrinsic");
+    int ndisp = cmd.get<int>("ndisp");
+    double scale = cmd.get<double>("scale");
+
+    if (left.empty() || right.empty())
+    {
+        cout << "Missed input images" << endl;
+        cout << "Avaible options:" << endl;
+        cmd.printParams();
+        return 0;
+    }
+
+    if (intrinsic.empty() ^ extrinsic.empty())
+    {
+        cout << "Boss camera parameters must be specified" << endl;
+        cout << "Avaible options:" << endl;
+        cmd.printParams();
+        return 0;
+    }
+
+    Mat imgLeftColor = imread(left, IMREAD_COLOR);
+    Mat imgRightColor = imread(right, IMREAD_COLOR);
+
+    if (imgLeftColor.empty())
+    {
+        cout << "Can't load image " << left << endl;
+        return -1;
+    }
+
+    if (imgRightColor.empty())
+    {
+        cout << "Can't load image " << right << endl;
+        return -1;
+    }    
+    
+    Mat Q = Mat::eye(4, 4, CV_32F);
+    if (!intrinsic.empty() && !extrinsic.empty())
+    {
+        FileStorage fs;
+    
+        // reading intrinsic parameters
+        fs.open(intrinsic, CV_STORAGE_READ);
+        if (!fs.isOpened())
+        {
+            cout << "Failed to open file " << intrinsic << endl;
+            return -1;
+        }
+        
+        Mat M1, D1, M2, D2;
+        fs["M1"] >> M1;
+        fs["D1"] >> D1;
+        fs["M2"] >> M2;
+        fs["D2"] >> D2;
+        
+        // reading extrinsic parameters
+        fs.open(extrinsic, CV_STORAGE_READ);
+        if (!fs.isOpened())
+        {
+            cout << "Failed to open file " << extrinsic << endl;
+            return -1;
+        }
+        
+        Mat R, T, R1, P1, R2, P2;
+        fs["R"] >> R;
+        fs["T"] >> T;
+
+        Size img_size = imgLeftColor.size();
+
+        Rect roi1, roi2;
+        stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);
+        
+        Mat map11, map12, map21, map22;
+        initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
+        initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
+        
+        Mat img1r, img2r;
+        remap(imgLeftColor, img1r, map11, map12, INTER_LINEAR);
+        remap(imgRightColor, img2r, map21, map22, INTER_LINEAR);
+        
+        imgLeftColor = img1r(roi1);
+        imgRightColor = img2r(roi2);
+    }
+
+    Mat imgLeftGray, imgRightGray;
+    cvtColor(imgLeftColor, imgLeftGray, COLOR_BGR2GRAY);
+    cvtColor(imgRightColor, imgRightGray, COLOR_BGR2GRAY);
+
+    cvtColor(imgLeftColor, imgLeftColor, COLOR_BGR2RGB);
+
+    Mat disp, points;
+
+    StereoBM bm(0, ndisp);
+
+    bm(imgLeftGray, imgRightGray, disp);
+    disp.convertTo(disp, CV_8U, 1.0 / 16.0);
+
+    disp = disp(Range(21, disp.rows - 21), Range(ndisp, disp.cols - 21)).clone();
+    imgLeftColor = imgLeftColor(Range(21, imgLeftColor.rows - 21), Range(ndisp, imgLeftColor.cols - 21)).clone();
+
+    reprojectImageTo3D(disp, points, Q);
+
+    namedWindow("OpenGL Sample", WINDOW_OPENGL);
+
+    int fov = 0;
+    createTrackbar("Fov", "OpenGL Sample", &fov, 100);
+
+    int mouse[2] = {0, 0};
+    setMouseCallback("OpenGL Sample", mouseCallback, mouse);
+    
+    GlArrays pointCloud;
+
+    pointCloud.setVertexArray(points);
+    pointCloud.setColorArray(imgLeftColor, false);
+
+    GlCamera camera;
+    camera.setScale(Point3d(scale, scale, scale));
+    
+    double yaw = 0.0;
+    double pitch = 0.0;
+
+    const Point3d dirVec(0.0, 0.0, -1.0);
+    const Point3d upVec(0.0, 1.0, 0.0);
+    const Point3d leftVec(-1.0, 0.0, 0.0);
+    Point3d pos;
+
+    while (true)
+    {
+        int key = waitKey(1);
+
+        if (key == 27)
+            break;        
+        
+        double aspect = getWindowProperty("OpenGL Sample", WND_PROP_ASPECT_RATIO);
+
+        const double posStep = 0.1;
+        const double mouseStep = 0.001;
+        const int mouseClamp = 300;
+
+        camera.setPerspectiveProjection(30.0 + fov / 100.0 * 40.0, aspect, 0.1, 1000.0); 
+
+        int mouse_dx = clamp(mouse[0], -mouseClamp, mouseClamp);
+        int mouse_dy = clamp(mouse[1], -mouseClamp, mouseClamp);
+
+        yaw += mouse_dx * mouseStep;
+        pitch += mouse_dy * mouseStep;
+
+        key = tolower(key);
+        if (key == 'w')
+            pos += posStep * rotate(dirVec, yaw, pitch);
+        else if (key == 's')
+            pos -= posStep * rotate(dirVec, yaw, pitch);
+        else if (key == 'a')
+            pos += posStep * rotate(leftVec, yaw, pitch);
+        else if (key == 'd')
+            pos -= posStep * rotate(leftVec, yaw, pitch);
+        else if (key == 'q')
+            pos += posStep * rotate(upVec, yaw, pitch);
+        else if (key == 'e')
+            pos -= posStep * rotate(upVec, yaw, pitch);
+
+        camera.setCameraPos(pos, yaw, pitch, 0.0);
+
+        pointCloudShow("OpenGL Sample", camera, pointCloud);
+    }
+
+    return 0;
+}