diff --git a/modules/core/include/opencv2/core/gpumat.hpp b/modules/core/include/opencv2/core/gpumat.hpp index f9fbcedfdd9596b81d9028097cf62e32d4b951f6..6682fff801d726db8c16775978d1ef06f7c4932b 100644 --- a/modules/core/include/opencv2/core/gpumat.hpp +++ b/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_; }; + //! 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_ wndRect = Rect_(0.0, 0.0, 1.0, 1.0), + Rect_ texRect = Rect_(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; diff --git a/modules/core/src/gpumat.cpp b/modules/core/src/gpumat.cpp index 365f050b92ccaf7b8bcd3ca6c4372dc0837f7709..87b7f604aaed0be1463935268b8a53c16cc1a238 100644 --- a/modules/core/src/gpumat.cpp +++ b/modules/core/src/gpumat.cpp @@ -52,6 +52,7 @@ #ifdef HAVE_OPENGL #include + #include #ifdef HAVE_CUDA #include @@ -982,6 +983,12 @@ namespace } } +cv::gpu::GlFuncTab::~GlFuncTab() +{ + if (g_glFuncTab == this) + g_glFuncTab = 0; +} + void cv::gpu::setGlFuncTab(const GlFuncTab* tab) { g_glFuncTab = tab; @@ -1327,7 +1334,7 @@ inline void cv::gpu::GlBuffer::Impl::unmapDevice(cudaStream_t stream) #endif // HAVE_OPENGL -cv::gpu::GlBuffer::GlBuffer(Usage usage) : rows_(0), cols_(0), type_(0), usage_(usage) +cv::gpu::GlBuffer::GlBuffer(Usage usage) : rows(0), cols(0), type_(0), usage_(usage) { #ifndef HAVE_OPENGL throw_nogl(); @@ -1336,43 +1343,44 @@ cv::gpu::GlBuffer::GlBuffer(Usage usage) : rows_(0), cols_(0), type_(0), usage_( #endif } -cv::gpu::GlBuffer::GlBuffer(int rows, int cols, int type, Usage usage) : rows_(0), cols_(0), type_(0), usage_(usage) +cv::gpu::GlBuffer::GlBuffer(int rows_, int cols_, int type, Usage usage) : rows(0), cols(0), type_(0), usage_(usage) { #ifndef HAVE_OPENGL throw_nogl(); #else - impl_ = new Impl(rows, cols, type, usage); - rows_ = rows; - cols_ = cols; + impl_ = new Impl(rows_, cols_, type, usage); + rows = rows_; + cols = cols_; type_ = type; #endif } -cv::gpu::GlBuffer::GlBuffer(Size size, int type, Usage usage) : rows_(0), cols_(0), type_(0), usage_(usage) +cv::gpu::GlBuffer::GlBuffer(Size size, int type, Usage usage) : rows(0), cols(0), type_(0), usage_(usage) { #ifndef HAVE_OPENGL throw_nogl(); #else impl_ = new Impl(size.height, size.width, type, usage); - rows_ = size.height; - cols_ = size.width; + rows = size.height; + cols = size.width; type_ = type; #endif } -cv::gpu::GlBuffer::GlBuffer(const Mat& mat, Usage usage) : rows_(0), cols_(0), type_(0), usage_(usage) +cv::gpu::GlBuffer::GlBuffer(InputArray mat_, Usage usage) : rows(0), cols(0), type_(0), usage_(usage) { #ifndef HAVE_OPENGL throw_nogl(); #else + Mat mat = mat_.getMat(); impl_ = new Impl(mat, usage); - rows_ = mat.rows; - cols_ = mat.cols; + rows = mat.rows; + cols = mat.cols; type_ = mat.type(); #endif } -cv::gpu::GlBuffer::GlBuffer(const GpuMat& d_mat, Usage usage) : rows_(0), cols_(0), type_(0), usage_(usage) +cv::gpu::GlBuffer::GlBuffer(const GpuMat& d_mat, Usage usage) : rows(0), cols(0), type_(0), usage_(usage) { #ifndef HAVE_OPENGL throw_nogl(); @@ -1382,27 +1390,42 @@ cv::gpu::GlBuffer::GlBuffer(const GpuMat& d_mat, Usage usage) : rows_(0), cols_( #else impl_ = new Impl(d_mat.rows, d_mat.cols, d_mat.type(), usage); impl_->copyFrom(d_mat); - rows_ = d_mat.rows; - cols_ = d_mat.cols; + rows = d_mat.rows; + cols = d_mat.cols; type_ = d_mat.type(); #endif #endif } +cv::gpu::GlBuffer::GlBuffer(const GlBuffer& other) + : rows(other.rows), cols(other.cols), type_(other.type_), usage_(other.usage_), impl_(other.impl_) +{ +} + cv::gpu::GlBuffer::~GlBuffer() { } -void cv::gpu::GlBuffer::create(int rows, int cols, int type, Usage usage) +GlBuffer& cv::gpu::GlBuffer::operator =(const GlBuffer& other) +{ + rows = other.rows; + cols = other.cols; + type_ = other.type_; + usage_ = other.usage_; + impl_ = other.impl_; + return *this; +} + +void cv::gpu::GlBuffer::create(int rows_, int cols_, int type, Usage usage) { #ifndef HAVE_OPENGL throw_nogl(); #else if (rows_ != rows || cols_ != cols || type_ != type || usage_ != usage) { - impl_ = new Impl(rows, cols, type, usage); - rows_ = rows; - cols_ = cols; + impl_ = new Impl(rows_, cols_, type, usage); + rows = rows_; + cols = cols_; type_ = type; usage_ = usage; } @@ -1418,11 +1441,12 @@ void cv::gpu::GlBuffer::release() #endif } -void cv::gpu::GlBuffer::copyFrom(const Mat& mat) +void cv::gpu::GlBuffer::copyFrom(InputArray mat_) { #ifndef HAVE_OPENGL throw_nogl(); #else + Mat mat = mat_.getMat(); create(mat.rows, mat.cols, mat.type()); impl_->copyFrom(mat, usage_); #endif @@ -1466,7 +1490,7 @@ Mat cv::gpu::GlBuffer::mapHost() throw_nogl(); return Mat(); #else - return impl_->mapHost(rows_, cols_, type_, usage_); + return impl_->mapHost(rows, cols, type_, usage_); #endif } @@ -1489,7 +1513,7 @@ GpuMat cv::gpu::GlBuffer::mapDevice() throw_nogpu(); return GpuMat(); #else - return impl_->mapDevice(rows_, cols_, type_); + return impl_->mapDevice(rows, cols, type_); #endif #endif } @@ -1643,7 +1667,7 @@ cv::gpu::GlTexture::Impl::Impl(const GlBuffer& buf, bool bgra) : tex_(0) glPixelStorei(GL_UNPACK_ALIGNMENT, 1); CV_CheckGlError(); - glTexImage2D(GL_TEXTURE_2D, 0, cn, buf.cols(), buf.rows(), 0, format, gl_types[depth], 0); + glTexImage2D(GL_TEXTURE_2D, 0, cn, buf.cols, buf.rows, 0, format, gl_types[depth], 0); CV_CheckGlError(); buf.unbind(); @@ -1682,7 +1706,7 @@ void cv::gpu::GlTexture::Impl::copyFrom(const GlBuffer& buf, bool bgra) int cn = buf.channels(); GLenum format = cn == 1 ? GL_LUMINANCE : (cn == 3 ? (bgra ? GL_BGR : GL_RGB) : (bgra ? GL_BGRA : GL_RGBA)); - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, buf.cols(), buf.rows(), format, gl_types[buf.depth()], 0); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, buf.cols, buf.rows, format, gl_types[buf.depth()], 0); CV_CheckGlError(); buf.unbind(); @@ -1708,7 +1732,7 @@ inline void cv::gpu::GlTexture::Impl::unbind() const #endif // HAVE_OPENGL -cv::gpu::GlTexture::GlTexture() : rows_(0), cols_(0), type_(0) +cv::gpu::GlTexture::GlTexture() : rows(0), cols(0), type_(0) { #ifndef HAVE_OPENGL throw_nogl(); @@ -1717,68 +1741,83 @@ cv::gpu::GlTexture::GlTexture() : rows_(0), cols_(0), type_(0) #endif } -cv::gpu::GlTexture::GlTexture(int rows, int cols, int type) : rows_(0), cols_(0), type_(0) +cv::gpu::GlTexture::GlTexture(int rows_, int cols_, int type) : rows(0), cols(0), type_(0) { #ifndef HAVE_OPENGL throw_nogl(); #else - impl_ = new Impl(rows, cols, type); - rows_ = rows; - cols_ = cols; + impl_ = new Impl(rows_, cols_, type); + rows = rows_; + cols = cols_; type_ = type; #endif } -cv::gpu::GlTexture::GlTexture(Size size, int type) : rows_(0), cols_(0), type_(0) +cv::gpu::GlTexture::GlTexture(Size size, int type) : rows(0), cols(0), type_(0) { #ifndef HAVE_OPENGL throw_nogl(); #else impl_ = new Impl(size.height, size.width, type); - rows_ = size.height; - cols_ = size.width; + rows = size.height; + cols = size.width; type_ = type; #endif } -cv::gpu::GlTexture::GlTexture(const Mat& mat, bool bgra) : rows_(0), cols_(0), type_(0) +cv::gpu::GlTexture::GlTexture(InputArray mat_, bool bgra) : rows(0), cols(0), type_(0) { #ifndef HAVE_OPENGL throw_nogl(); #else + Mat mat = mat_.getMat(); impl_ = new Impl(mat, bgra); - rows_ = mat.rows; - cols_ = mat.cols; + rows = mat.rows; + cols = mat.cols; type_ = mat.type(); #endif } -cv::gpu::GlTexture::GlTexture(const GlBuffer& buf, bool bgra) : rows_(0), cols_(0), type_(0) +cv::gpu::GlTexture::GlTexture(const GlBuffer& buf, bool bgra) : rows(0), cols(0), type_(0) { #ifndef HAVE_OPENGL throw_nogl(); #else impl_ = new Impl(buf, bgra); - rows_ = buf.rows(); - cols_ = buf.cols(); + rows = buf.rows; + cols = buf.cols; type_ = buf.type(); #endif } +cv::gpu::GlTexture::GlTexture(const GlTexture& other) + : rows(other.rows), cols(other.cols), type_(other.type_), impl_(other.impl_) +{ +} + cv::gpu::GlTexture::~GlTexture() { } -void cv::gpu::GlTexture::create(int rows, int cols, int type) +GlTexture& cv::gpu::GlTexture::operator =(const GlTexture& other) +{ + rows = other.rows; + cols = other.cols; + type_ = other.type_; + impl_ = other.impl_; + return *this; +} + +void cv::gpu::GlTexture::create(int rows_, int cols_, int type) { #ifndef HAVE_OPENGL throw_nogl(); #else if (rows_ != rows || cols_ != cols || type_ != type) { - impl_ = new Impl(rows, cols, type); - rows_ = rows; - cols_ = cols; + impl_ = new Impl(rows_, cols_, type); + rows = rows_; + cols = cols_; type_ = type; } #endif @@ -1793,11 +1832,12 @@ void cv::gpu::GlTexture::release() #endif } -void cv::gpu::GlTexture::copyFrom(const Mat& mat, bool bgra) +void cv::gpu::GlTexture::copyFrom(InputArray mat_, bool bgra) { #ifndef HAVE_OPENGL throw_nogl(); #else + Mat mat = mat_.getMat(); create(mat.rows, mat.cols, mat.type()); impl_->copyFrom(mat, bgra); #endif @@ -1808,7 +1848,7 @@ void cv::gpu::GlTexture::copyFrom(const GlBuffer& buf, bool bgra) #ifndef HAVE_OPENGL throw_nogl(); #else - create(buf.rows(), buf.cols(), buf.type()); + create(buf.rows, buf.cols, buf.type()); impl_->copyFrom(buf, bgra); #endif } @@ -1832,40 +1872,268 @@ void cv::gpu::GlTexture::unbind() const } //////////////////////////////////////////////////////////////////////// -// Rendering +// GlArrays + +void cv::gpu::GlArrays::setVertexArray(const GlBuffer& vertex) +{ + CV_Assert(vertex.usage() == GlBuffer::ARRAY_BUFFER); + + int cn = vertex.channels(); + int depth = vertex.depth(); -void cv::gpu::render(const GlTexture& tex) + CV_Assert(cn == 2 || cn == 3 || cn == 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + vertex_ = vertex; +} + +void cv::gpu::GlArrays::setVertexArray(const GpuMat& vertex) +{ + int cn = vertex.channels(); + int depth = vertex.depth(); + + CV_Assert(cn == 2 || cn == 3 || cn == 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + vertex_.copyFrom(vertex); +} + +void cv::gpu::GlArrays::setVertexArray(InputArray vertex) +{ + int cn = vertex.channels(); + int depth = vertex.depth(); + + CV_Assert(cn == 2 || cn == 3 || cn == 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + vertex_.copyFrom(vertex); +} + +void cv::gpu::GlArrays::setColorArray(const GlBuffer& color, bool bgra) +{ + CV_Assert(color.usage() == GlBuffer::ARRAY_BUFFER); + + int cn = color.channels(); + + CV_Assert((cn == 3 && !bgra) || cn == 4); + + color_ = color; + bgra_ = bgra; +} + +void cv::gpu::GlArrays::setColorArray(const GpuMat& color, bool bgra) +{ + int cn = color.channels(); + + CV_Assert((cn == 3 && !bgra) || cn == 4); + + color_.copyFrom(color); + bgra_ = bgra; +} + +void cv::gpu::GlArrays::setColorArray(InputArray color, bool bgra) +{ + int cn = color.channels(); + + CV_Assert((cn == 3 && !bgra) || cn == 4); + + color_.copyFrom(color); + bgra_ = bgra; +} + +void cv::gpu::GlArrays::setNormalArray(const GlBuffer& normal) +{ + CV_Assert(normal.usage() == GlBuffer::ARRAY_BUFFER); + + int cn = normal.channels(); + int depth = normal.depth(); + + CV_Assert(cn == 3); + CV_Assert(depth == CV_8S || depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + normal_ = normal; +} + +void cv::gpu::GlArrays::setNormalArray(const GpuMat& normal) +{ + int cn = normal.channels(); + int depth = normal.depth(); + + CV_Assert(cn == 3); + CV_Assert(depth == CV_8S || depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + normal_.copyFrom(normal); +} + +void cv::gpu::GlArrays::setNormalArray(InputArray normal) +{ + int cn = normal.channels(); + int depth = normal.depth(); + + CV_Assert(cn == 3); + CV_Assert(depth == CV_8S || depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + normal_.copyFrom(normal); +} + +void cv::gpu::GlArrays::setTexCoordArray(const GlBuffer& texCoord) +{ + CV_Assert(texCoord.usage() == GlBuffer::ARRAY_BUFFER); + + int cn = texCoord.channels(); + int depth = texCoord.depth(); + + CV_Assert(cn >= 1 && cn <= 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + texCoord_ = texCoord; +} + +void cv::gpu::GlArrays::setTexCoordArray(const GpuMat& texCoord) +{ + int cn = texCoord.channels(); + int depth = texCoord.depth(); + + CV_Assert(cn >= 1 && cn <= 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + texCoord_.copyFrom(texCoord); +} + +void cv::gpu::GlArrays::setTexCoordArray(InputArray texCoord) +{ + int cn = texCoord.channels(); + int depth = texCoord.depth(); + + CV_Assert(cn >= 1 && cn <= 4); + CV_Assert(depth == CV_16S || depth == CV_32S || depth == CV_32F || depth == CV_64F); + + texCoord_.copyFrom(texCoord); +} + +void cv::gpu::GlArrays::bind() const { #ifndef HAVE_OPENGL throw_nogl(); #else - if (!tex.empty()) + CV_DbgAssert(texCoord_.empty() || texCoord_.size().area() == vertex_.size().area()); + CV_DbgAssert(normal_.empty() || normal_.size().area() == vertex_.size().area()); + CV_DbgAssert(color_.empty() || color_.size().area() == vertex_.size().area()); + + if (!texCoord_.empty()) { - tex.bind(); + glEnableClientState(GL_TEXTURE_COORD_ARRAY); + CV_CheckGlError(); - glDisable(GL_DEPTH_TEST); + texCoord_.bind(); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - glOrtho(0, 1, 1, 0, -1, 1); + glTexCoordPointer(texCoord_.channels(), gl_types[texCoord_.depth()], 0, 0); + CV_CheckGlError(); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); + texCoord_.unbind(); + } - glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); + if (!normal_.empty()) + { + glEnableClientState(GL_NORMAL_ARRAY); + CV_CheckGlError(); - glBegin(GL_QUADS); - glVertex2d(0.0, 0.0); - glTexCoord2d(1.0, 0.0); + normal_.bind(); - glVertex2d(1.0, 0.0); - glTexCoord2d(1.0, 1.0); + glNormalPointer(gl_types[normal_.depth()], 0, 0); + CV_CheckGlError(); - glVertex2d(1.0, 1.0); - glTexCoord2d(0.0, 1.0); + normal_.unbind(); + } + + if (!color_.empty()) + { + glEnableClientState(GL_COLOR_ARRAY); + CV_CheckGlError(); + + color_.bind(); + + int cn = color_.channels(); + int format = cn == 3 ? cn : (bgra_ ? GL_BGRA : 4); + + glColorPointer(format, gl_types[color_.depth()], 0, 0); + CV_CheckGlError(); + + color_.unbind(); + } + + if (!vertex_.empty()) + { + glEnableClientState(GL_VERTEX_ARRAY); + CV_CheckGlError(); + + vertex_.bind(); + + glVertexPointer(vertex_.channels(), gl_types[vertex_.depth()], 0, 0); + CV_CheckGlError(); - glVertex2d(0.0, 1.0); - glTexCoord2d(0.0, 0.0); + vertex_.unbind(); + } +#endif +} + +void cv::gpu::GlArrays::unbind() const +{ +#ifndef HAVE_OPENGL + throw_nogl(); +#else + if (!texCoord_.empty()) + { + glDisableClientState(GL_TEXTURE_COORD_ARRAY); + CV_CheckGlError(); + } + + if (!normal_.empty()) + { + glDisableClientState(GL_NORMAL_ARRAY); + CV_CheckGlError(); + } + + if (!color_.empty()) + { + glDisableClientState(GL_COLOR_ARRAY); + CV_CheckGlError(); + } + + if (!vertex_.empty()) + { + glDisableClientState(GL_VERTEX_ARRAY); + CV_CheckGlError(); + } +#endif +} + +//////////////////////////////////////////////////////////////////////// +// Rendering + +void cv::gpu::render(const GlTexture& tex, Rect_ wndRect, Rect_ texRect) +{ +#ifndef HAVE_OPENGL + throw_nogl(); +#else + if (!tex.empty()) + { + tex.bind(); + + glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); + + glBegin(GL_QUADS); + glTexCoord2d(texRect.x, texRect.y); + glVertex2d(wndRect.x, wndRect.y); + + glTexCoord2d(texRect.x, texRect.y + texRect.height); + glVertex2d(wndRect.x, (wndRect.y + wndRect.height)); + + glTexCoord2d(texRect.x + texRect.width, texRect.y + texRect.height); + glVertex2d(wndRect.x + wndRect.width, (wndRect.y + wndRect.height)); + + glTexCoord2d(texRect.x + texRect.width, texRect.y); + glVertex2d(wndRect.x + wndRect.width, wndRect.y); glEnd(); CV_CheckGlError(); @@ -1875,6 +2143,142 @@ void cv::gpu::render(const GlTexture& tex) #endif } +void cv::gpu::render(const GlArrays& arr, int mode) +{ +#ifndef HAVE_OPENGL + throw_nogl(); +#else + arr.bind(); + + glDrawArrays(mode, 0, arr.size().area()); + + arr.unbind(); +#endif +} + +//////////////////////////////////////////////////////////////////////// +// GlCamera + +cv::gpu::GlCamera::GlCamera() : + eye_(0.0, 0.0, -5.0), center_(0.0, 0.0, 0.0), up_(0.0, 1.0, 0.0), + pos_(0.0, 0.0, -5.0), yaw_(0.0), pitch_(0.0), roll_(0.0), + useLookAtParams_(false), + + scale_(1.0, 1.0, 1.0), + + projectionMatrix_(), + fov_(45.0), aspect_(0.0), + left_(0.0), right_(1.0), bottom_(1.0), top_(0.0), + zNear_(-1.0), zFar_(1.0), + perspectiveProjection_(false) +{ +} + +void cv::gpu::GlCamera::lookAt(Point3d eye, Point3d center, Point3d up) +{ + eye_ = eye; + center_ = center; + up_ = up; + useLookAtParams_ = true; +} + +void cv::gpu::GlCamera::setCameraPos(Point3d pos, double yaw, double pitch, double roll) +{ + pos_ = pos; + yaw_ = yaw; + pitch_ = pitch; + roll_ = roll; + useLookAtParams_ = false; +} + +void cv::gpu::GlCamera::setScale(Point3d scale) +{ + scale_ = scale; +} + +void cv::gpu::GlCamera::setProjectionMatrix(const Mat& projectionMatrix, bool transpose) +{ + CV_Assert(projectionMatrix.type() == CV_32F || projectionMatrix.type() == CV_64F); + CV_Assert(projectionMatrix.cols == 4 && projectionMatrix.rows == 4); + + projectionMatrix_ = transpose ? projectionMatrix.t() : projectionMatrix; +} + +void cv::gpu::GlCamera::setPerspectiveProjection(double fov, double aspect, double zNear, double zFar) +{ + fov_ = fov; + aspect_ = aspect; + zNear_ = zNear; + zFar_ = zFar; + + projectionMatrix_.release(); + perspectiveProjection_ = true; +} + +void cv::gpu::GlCamera::setOrthoProjection(double left, double right, double bottom, double top, double zNear, double zFar) +{ + left_ = left; + right_ = right; + bottom_ = bottom; + top_ = top; + zNear_ = zNear; + zFar_ = zFar; + + projectionMatrix_.release(); + perspectiveProjection_ = false; +} + +void cv::gpu::GlCamera::setupProjectionMatrix() const +{ +#ifndef HAVE_OPENGL + throw_nogl(); +#else + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + if (projectionMatrix_.empty()) + { + if (perspectiveProjection_) + gluPerspective(fov_, aspect_, zNear_, zFar_); + else + glOrtho(left_, right_, bottom_, top_, zNear_, zFar_); + } + else + { + if (projectionMatrix_.type() == CV_32F) + glLoadMatrixf(projectionMatrix_.ptr()); + else + glLoadMatrixd(projectionMatrix_.ptr()); + } + + CV_CheckGlError(); +#endif +} + +void cv::gpu::GlCamera::setupModelViewMatrix() const +{ +#ifndef HAVE_OPENGL + throw_nogl(); +#else + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + + if (useLookAtParams_) + gluLookAt(eye_.x, eye_.y, eye_.z, center_.x, center_.y, center_.z, up_.x, up_.y, up_.z); + else + { + glRotated(-yaw_, 0.0, 1.0, 0.0); + glRotated(-pitch_, 1.0, 0.0, 0.0); + glRotated(-roll_, 0.0, 0.0, 1.0); + glTranslated(-pos_.x, -pos_.y, -pos_.z); + } + + glScaled(scale_.x, scale_.y, scale_.z); + + CV_CheckGlError(); +#endif +} + //////////////////////////////////////////////////////////////////////// // Error handling diff --git a/modules/highgui/include/opencv2/highgui/highgui.hpp b/modules/highgui/include/opencv2/highgui/highgui.hpp index 0452e0dad8c213895a5b86cb1809f880cded3315..5dbad1baaa942c710f8054e923fc9491135a00e7 100644 --- a/modules/highgui/include/opencv2/highgui/highgui.hpp +++ b/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 diff --git a/modules/highgui/src/window.cpp b/modules/highgui/src/window.cpp index ddf615f0bc3e3608ea21b25b42b13cabeac0da54..556a7af02ef7db05fa6ae324bafdee604a05d324 100644 --- a/modules/highgui/src/window.cpp +++ b/modules/highgui/src/window.cpp @@ -228,6 +228,7 @@ void cv::updateWindow(const string& windowName) namespace { const int CV_TEXTURE_MAGIC_VAL = 0x00287653; + const int CV_POINT_CLOUD_MAGIC_VAL = 0x00287654; struct GlObjBase { @@ -273,19 +274,40 @@ namespace delete glObj; } - template - struct GlObj : GlObjBase + struct GlObjTex : GlObjBase { - T obj; + cv::gpu::GlTexture tex; }; void CV_CDECL glDrawTextureCallback(void* userdata) { - GlObj* texObj = static_cast*>(userdata); + GlObjTex* texObj = static_cast(userdata); CV_DbgAssert(texObj->flag == CV_TEXTURE_MAGIC_VAL); - cv::gpu::render(texObj->obj); + static cv::gpu::GlCamera glCamera; + + glCamera.setupProjectionMatrix(); + + cv::gpu::render(texObj->tex); + } + + struct GlObjPointCloud : GlObjBase + { + cv::gpu::GlArrays arr; + cv::gpu::GlCamera camera; + }; + + void CV_CDECL glDrawPointCloudCallback(void* userdata) + { + GlObjPointCloud* pointCloudObj = static_cast(userdata); + + CV_DbgAssert(pointCloudObj->flag == CV_POINT_CLOUD_MAGIC_VAL); + + pointCloudObj->camera.setupProjectionMatrix(); + pointCloudObj->camera.setupModelViewMatrix(); + + cv::gpu::render(pointCloudObj->arr); } void CV_CDECL glCleanCallback(void* userdata) @@ -297,22 +319,16 @@ namespace } #endif // HAVE_OPENGL -void cv::imshow( const string& winname, InputArray _img ) -{ - Mat img = _img.getMat(); +#ifdef HAVE_OPENGL -#ifndef HAVE_OPENGL - CvMat c_img = img; - cvShowImage(winname.c_str(), &c_img); -#else - double useGl = getWindowProperty(winname, WND_PROP_OPENGL); - if (useGl <= 0) - { - CvMat c_img = img; - cvShowImage(winname.c_str(), &c_img); - } - else +namespace +{ + template void imshowImpl(const std::string& winname, const T& img) { + using namespace cv; + + namedWindow(winname, WINDOW_OPENGL | WINDOW_AUTOSIZE); + double autoSize = getWindowProperty(winname, WND_PROP_AUTOSIZE); if (autoSize > 0) @@ -330,13 +346,13 @@ void cv::imshow( const string& winname, InputArray _img ) if (glObj) { - GlObj* texObj = static_cast*>(glObj); - texObj->obj.copyFrom(img); + GlObjTex* texObj = static_cast(glObj); + texObj->tex.copyFrom(img); } else { - GlObj* texObj = new GlObj; - texObj->obj.copyFrom(img); + GlObjTex* texObj = new GlObjTex; + texObj->tex.copyFrom(img); glObj = texObj; glObj->flag = CV_TEXTURE_MAGIC_VAL; @@ -351,6 +367,28 @@ void cv::imshow( const string& winname, InputArray _img ) updateWindow(winname); } +} + +#endif // HAVE_OPENGL + +void cv::imshow( const string& winname, InputArray _img ) +{ + Mat img = _img.getMat(); + +#ifndef HAVE_OPENGL + CvMat c_img = img; + cvShowImage(winname.c_str(), &c_img); +#else + double useGl = getWindowProperty(winname, WND_PROP_OPENGL); + if (useGl <= 0) + { + CvMat c_img = img; + cvShowImage(winname.c_str(), &c_img); + } + else + { + imshowImpl(winname, img); + } #endif } @@ -359,14 +397,32 @@ void cv::imshow(const string& winname, const gpu::GlBuffer& buf) #ifndef HAVE_OPENGL CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); #else - CV_Assert(buf.usage() == gpu::GlBuffer::TEXTURE_BUFFER); + imshowImpl(winname, buf); +#endif +} +void cv::imshow(const string& winname, const gpu::GpuMat& d_mat) +{ +#ifndef HAVE_OPENGL + CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); +#else + setOpenGlContext(winname); + gpu::GlBuffer buf(d_mat, gpu::GlBuffer::TEXTURE_BUFFER); + imshow(winname, buf); +#endif +} + +void cv::imshow(const string& winname, const gpu::GlTexture& tex) +{ +#ifndef HAVE_OPENGL + CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); +#else namedWindow(winname, WINDOW_OPENGL | WINDOW_AUTOSIZE); double autoSize = getWindowProperty(winname, WND_PROP_AUTOSIZE); if (autoSize > 0) - resizeWindow(winname, buf.cols(), buf.rows()); + resizeWindow(winname, tex.cols, tex.rows); setOpenGlContext(winname); @@ -380,13 +436,13 @@ void cv::imshow(const string& winname, const gpu::GlBuffer& buf) if (glObj) { - GlObj* texObj = static_cast*>(glObj); - texObj->obj.copyFrom(buf); + GlObjTex* texObj = static_cast(glObj); + texObj->tex = tex; } else { - GlObj* texObj = new GlObj; - texObj->obj.copyFrom(buf); + GlObjTex* texObj = new GlObjTex; + texObj->tex = tex; glObj = texObj; glObj->flag = CV_TEXTURE_MAGIC_VAL; @@ -395,7 +451,7 @@ void cv::imshow(const string& winname, const gpu::GlBuffer& buf) addGlObj(glObj); icvSetOpenGlCleanCallback(winname.c_str(), glCleanCallback, glObj); - } + } setOpenGlDrawCallback(winname, glDrawTextureCallback, glObj); @@ -403,34 +459,18 @@ void cv::imshow(const string& winname, const gpu::GlBuffer& buf) #endif } -void cv::imshow(const string& winname, const gpu::GpuMat& d_mat) -{ +void cv::pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GlArrays& arr) +{ #ifndef HAVE_OPENGL CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); #else - setOpenGlContext(winname); - gpu::GlBuffer buf(d_mat, gpu::GlBuffer::TEXTURE_BUFFER); - imshow(winname, buf); -#endif -} - -void cv::imshow(const string& winname, const gpu::GlTexture& tex) -{ -#ifndef HAVE_OPENGL - CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); -#else - namedWindow(winname, WINDOW_OPENGL | WINDOW_AUTOSIZE); - - double autoSize = getWindowProperty(winname, WND_PROP_AUTOSIZE); - - if (autoSize > 0) - resizeWindow(winname, tex.cols(), tex.rows()); + namedWindow(winname, WINDOW_OPENGL); setOpenGlContext(winname); GlObjBase* glObj = findGlObjByName(winname); - if (glObj && glObj->flag != CV_TEXTURE_MAGIC_VAL) + if (glObj && glObj->flag != CV_POINT_CLOUD_MAGIC_VAL) { icvSetOpenGlCleanCallback(winname.c_str(), 0, 0); glObj = 0; @@ -438,16 +478,18 @@ void cv::imshow(const string& winname, const gpu::GlTexture& tex) if (glObj) { - GlObj* texObj = static_cast*>(glObj); - texObj->obj = tex; + GlObjPointCloud* pointCloudObj = static_cast(glObj); + pointCloudObj->arr = arr; + pointCloudObj->camera = camera; } else { - GlObj* texObj = new GlObj; - texObj->obj = tex; + GlObjPointCloud* pointCloudObj = new GlObjPointCloud; + pointCloudObj->arr = arr; + pointCloudObj->camera = camera; - glObj = texObj; - glObj->flag = CV_TEXTURE_MAGIC_VAL; + glObj = pointCloudObj; + glObj->flag = CV_POINT_CLOUD_MAGIC_VAL; glObj->winname = winname; addGlObj(glObj); @@ -455,12 +497,98 @@ void cv::imshow(const string& winname, const gpu::GlTexture& tex) icvSetOpenGlCleanCallback(winname.c_str(), glCleanCallback, glObj); } - setOpenGlDrawCallback(winname, glDrawTextureCallback, glObj); + setOpenGlDrawCallback(winname, glDrawPointCloudCallback, glObj); updateWindow(winname); #endif } +#ifdef HAVE_OPENGL + +namespace +{ + template void pointCloudShowImpl(const std::string& winname, const cv::gpu::GlCamera& camera, const T& points, const T& colors) + { + using namespace cv; + + namedWindow(winname, WINDOW_OPENGL); + + setOpenGlContext(winname); + + GlObjBase* glObj = findGlObjByName(winname); + + if (glObj && glObj->flag != CV_POINT_CLOUD_MAGIC_VAL) + { + icvSetOpenGlCleanCallback(winname.c_str(), 0, 0); + glObj = 0; + } + + if (glObj) + { + GlObjPointCloud* pointCloudObj = static_cast(glObj); + + pointCloudObj->arr.setVertexArray(points); + if (colors.empty()) + pointCloudObj->arr.resetColorArray(); + else + pointCloudObj->arr.setColorArray(colors); + + pointCloudObj->camera = camera; + } + else + { + GlObjPointCloud* pointCloudObj = new GlObjPointCloud; + + pointCloudObj->arr.setVertexArray(points); + if (!colors.empty()) + pointCloudObj->arr.setColorArray(colors); + + pointCloudObj->camera = camera; + + glObj = pointCloudObj; + glObj->flag = CV_POINT_CLOUD_MAGIC_VAL; + glObj->winname = winname; + + addGlObj(glObj); + + icvSetOpenGlCleanCallback(winname.c_str(), glCleanCallback, glObj); + } + + setOpenGlDrawCallback(winname, glDrawPointCloudCallback, glObj); + + updateWindow(winname); + } +} + +#endif // HAVE_OPENGL + +void cv::pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GlBuffer& points, const gpu::GlBuffer& colors) +{ +#ifndef HAVE_OPENGL + CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); +#else + pointCloudShowImpl(winname, camera, points, colors); +#endif +} + +void cv::pointCloudShow(const string& winname, const gpu::GlCamera& camera, const gpu::GpuMat& points, const gpu::GpuMat& colors) +{ +#ifndef HAVE_OPENGL + CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); +#else + pointCloudShowImpl(winname, camera, points, colors); +#endif +} + +void cv::pointCloudShow(const string& winname, const gpu::GlCamera& camera, InputArray points, InputArray colors) +{ +#ifndef HAVE_OPENGL + CV_Error(CV_OpenGlNotSupported, "The library is compiled without OpenGL support"); +#else + pointCloudShowImpl(winname, camera, points, colors); +#endif +} + #ifndef HAVE_OPENGL CV_IMPL void cvCreateOpenGLCallback(const char*, CvOpenGLCallback, void*, double, double, double) diff --git a/modules/highgui/src/window_w32.cpp b/modules/highgui/src/window_w32.cpp index e42dd14004a1208c08f962037a3685dcd705b91f..57a6a576781bbb787950ca265a0df59026477fc7 100644 --- a/modules/highgui/src/window_w32.cpp +++ b/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(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) diff --git a/samples/cpp/point_cloud.cpp b/samples/cpp/point_cloud.cpp new file mode 100644 index 0000000000000000000000000000000000000000..27eec1cc6285621d28d7c54f8c1dd2630245774a --- /dev/null +++ b/samples/cpp/point_cloud.cpp @@ -0,0 +1,255 @@ +#include +#include +#include +#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(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("help")) + { + cout << "Avaible options:" << endl; + cmd.printParams(); + return 0; + } + + string left = cmd.get("left"); + string right = cmd.get("right"); + string intrinsic = cmd.get("intrinsic"); + string extrinsic = cmd.get("extrinsic"); + int ndisp = cmd.get("ndisp"); + double scale = cmd.get("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; +}