From bf7ab8eebdf6d4f57f81a7cb5bb112b7ae24faaf Mon Sep 17 00:00:00 2001 From: Junyan721113 Date: Wed, 9 Oct 2024 00:35:27 +0800 Subject: [PATCH 01/10] feat: medianBlur & bilateralFilter --- 3rdparty/ndsrvp/include/imgproc.hpp | 18 ++ 3rdparty/ndsrvp/src/bilateralFilter.cpp | 270 +++++++++++++++++++++ 3rdparty/ndsrvp/src/filter.cpp | 4 +- 3rdparty/ndsrvp/src/medianBlur.cpp | 300 ++++++++++++++++++++++++ 4 files changed, 590 insertions(+), 2 deletions(-) create mode 100644 3rdparty/ndsrvp/src/bilateralFilter.cpp create mode 100644 3rdparty/ndsrvp/src/medianBlur.cpp diff --git a/3rdparty/ndsrvp/include/imgproc.hpp b/3rdparty/ndsrvp/include/imgproc.hpp index db0ee05132..16ecc81cfc 100644 --- a/3rdparty/ndsrvp/include/imgproc.hpp +++ b/3rdparty/ndsrvp/include/imgproc.hpp @@ -101,6 +101,24 @@ int filterFree(cvhalFilter2D *context); #undef cv_hal_filterFree #define cv_hal_filterFree (cv::ndsrvp::filterFree) +// ################ medianBlur ################ + +int medianBlur(const uchar* src_data, size_t src_step, + uchar* dst_data, size_t dst_step, + int width, int height, int depth, int cn, int ksize); + +#undef cv_hal_medianBlur +#define cv_hal_medianBlur (cv::ndsrvp::medianBlur) + +// ################ bilateralFilter ################ + +int bilateralFilter(const uchar* src_data, size_t src_step, + uchar* dst_data, size_t dst_step, int width, int height, int depth, + int cn, int d, double sigma_color, double sigma_space, int border_type); + +#undef cv_hal_bilateralFilter +#define cv_hal_bilateralFilter (cv::ndsrvp::bilateralFilter) + } // namespace ndsrvp } // namespace cv diff --git a/3rdparty/ndsrvp/src/bilateralFilter.cpp b/3rdparty/ndsrvp/src/bilateralFilter.cpp new file mode 100644 index 0000000000..c7a51b4199 --- /dev/null +++ b/3rdparty/ndsrvp/src/bilateralFilter.cpp @@ -0,0 +1,270 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "ndsrvp_hal.hpp" +#include "opencv2/imgproc/hal/interface.h" +#include "cvutils.hpp" + +namespace cv { + +namespace ndsrvp { + +static void bilateralFilterProcess(uchar* dst_data, size_t dst_step, uchar* pad_data, size_t pad_step, + int width, int height, int cn, int radius, int maxk, + int* space_ofs, float *space_weight, float *color_weight) +{ + int i, j, k; + + for( i = 0; i < height; i++ ) + { + const uchar* sptr = pad_data + (i + radius) * pad_step + radius * cn; + uchar* dptr = dst_data + i * dst_step; + + if( cn == 1 ) + { + std::vector buf(width + width, 0.0); + float *sum = &buf[0]; + float *wsum = sum + width; + k = 0; + for(; k <= maxk-4; k+=4) + { + const uchar* ksptr0 = sptr + space_ofs[k]; + const uchar* ksptr1 = sptr + space_ofs[k+1]; + const uchar* ksptr2 = sptr + space_ofs[k+2]; + const uchar* ksptr3 = sptr + space_ofs[k+3]; + j = 0; + for (; j < width; j++) + { + int rval = sptr[j]; + + int val = ksptr0[j]; + float w = space_weight[k] * color_weight[std::abs(val - rval)]; + wsum[j] += w; + sum[j] += val * w; + + val = ksptr1[j]; + w = space_weight[k+1] * color_weight[std::abs(val - rval)]; + wsum[j] += w; + sum[j] += val * w; + + val = ksptr2[j]; + w = space_weight[k+2] * color_weight[std::abs(val - rval)]; + wsum[j] += w; + sum[j] += val * w; + + val = ksptr3[j]; + w = space_weight[k+3] * color_weight[std::abs(val - rval)]; + wsum[j] += w; + sum[j] += val * w; + } + } + for(; k < maxk; k++) + { + const uchar* ksptr = sptr + space_ofs[k]; + j = 0; + for (; j < width; j++) + { + int val = ksptr[j]; + float w = space_weight[k] * color_weight[std::abs(val - sptr[j])]; + wsum[j] += w; + sum[j] += val * w; + } + } + j = 0; + for (; j < width; j++) + { + // overflow is not possible here => there is no need to use cv::saturate_cast + ndsrvp_assert(fabs(wsum[j]) > 0); + dptr[j] = (uchar)(sum[j] / wsum[j] + 0.5); + } + } + else + { + ndsrvp_assert( cn == 3 ); + std::vector buf(width * 3 + width); + float *sum_b = &buf[0]; + float *sum_g = sum_b + width; + float *sum_r = sum_g + width; + float *wsum = sum_r + width; + k = 0; + for(; k <= maxk-4; k+=4) + { + const uchar* ksptr0 = sptr + space_ofs[k]; + const uchar* ksptr1 = sptr + space_ofs[k+1]; + const uchar* ksptr2 = sptr + space_ofs[k+2]; + const uchar* ksptr3 = sptr + space_ofs[k+3]; + const uchar* rsptr = sptr; + j = 0; + for(; j < width; j++, rsptr += 3, ksptr0 += 3, ksptr1 += 3, ksptr2 += 3, ksptr3 += 3) + { + int rb = rsptr[0], rg = rsptr[1], rr = rsptr[2]; + + int b = ksptr0[0], g = ksptr0[1], r = ksptr0[2]; + float w = space_weight[k] * color_weight[std::abs(b - rb) + std::abs(g - rg) + std::abs(r - rr)]; + wsum[j] += w; + sum_b[j] += b * w; sum_g[j] += g * w; sum_r[j] += r * w; + + b = ksptr1[0]; g = ksptr1[1]; r = ksptr1[2]; + w = space_weight[k+1] * color_weight[std::abs(b - rb) + std::abs(g - rg) + std::abs(r - rr)]; + wsum[j] += w; + sum_b[j] += b * w; sum_g[j] += g * w; sum_r[j] += r * w; + + b = ksptr2[0]; g = ksptr2[1]; r = ksptr2[2]; + w = space_weight[k+2] * color_weight[std::abs(b - rb) + std::abs(g - rg) + std::abs(r - rr)]; + wsum[j] += w; + sum_b[j] += b * w; sum_g[j] += g * w; sum_r[j] += r * w; + + b = ksptr3[0]; g = ksptr3[1]; r = ksptr3[2]; + w = space_weight[k+3] * color_weight[std::abs(b - rb) + std::abs(g - rg) + std::abs(r - rr)]; + wsum[j] += w; + sum_b[j] += b * w; sum_g[j] += g * w; sum_r[j] += r * w; + } + } + for(; k < maxk; k++) + { + const uchar* ksptr = sptr + space_ofs[k]; + const uchar* rsptr = sptr; + j = 0; + for(; j < width; j++, ksptr += 3, rsptr += 3) + { + int b = ksptr[0], g = ksptr[1], r = ksptr[2]; + float w = space_weight[k] * color_weight[std::abs(b - rsptr[0]) + std::abs(g - rsptr[1]) + std::abs(r - rsptr[2])]; + wsum[j] += w; + sum_b[j] += b * w; sum_g[j] += g * w; sum_r[j] += r * w; + } + } + j = 0; + for(; j < width; j++) + { + ndsrvp_assert(fabs(wsum[j]) > 0); + wsum[j] = 1.f / wsum[j]; + *(dptr++) = (uchar)(sum_b[j] * wsum[j] + 0.5); + *(dptr++) = (uchar)(sum_g[j] * wsum[j] + 0.5); + *(dptr++) = (uchar)(sum_r[j] * wsum[j] + 0.5); + } + } + } +} + +int bilateralFilter(const uchar* src_data, size_t src_step, + uchar* dst_data, size_t dst_step, int width, int height, int depth, + int cn, int d, double sigma_color, double sigma_space, int border_type) +{ + if( depth != CV_8U || !(cn == 1 || cn == 3) || src_data == dst_data) + return CV_HAL_ERROR_NOT_IMPLEMENTED; + + int i, j, maxk, radius; + + if( sigma_color <= 0 ) + sigma_color = 1; + if( sigma_space <= 0 ) + sigma_space = 1; + + double gauss_color_coeff = -0.5/(sigma_color * sigma_color); + double gauss_space_coeff = -0.5/(sigma_space * sigma_space); + + if( d <= 0 ) + radius = (int)(sigma_space * 1.5 + 0.5); + else + radius = d / 2; + + radius = MAX(radius, 1); + d = radius * 2 + 1; + + // no enough submatrix info + // fetch original image data + const uchar *ogn_data = src_data; + int ogn_step = src_step; + + // ROI fully used in the computation + int cal_width = width + d - 1; + int cal_height = height + d - 1; + int cal_x = 0 - radius; // negative if left border exceeded + int cal_y = 0 - radius; // negative if top border exceeded + + // calculate source border + std::vector padding; + padding.resize(cal_width * cal_height * cn); + uchar* pad_data = &padding[0]; + int pad_step = cal_width * cn; + + uchar* pad_ptr; + const uchar* ogn_ptr; + std::vector vec_zeros(cn, 0); + for(i = 0; i < cal_height; i++) + { + int y = borderInterpolate(i + cal_y, height, border_type); + if(y < 0) { + memset(pad_data + i * pad_step, 0, cn * cal_width); + continue; + } + + // left border + j = 0; + for(; j + cal_x < 0; j++) + { + int x = borderInterpolate(j + cal_x, width, border_type); + if(x < 0) // border constant return value -1 + ogn_ptr = &vec_zeros[0]; + else + ogn_ptr = ogn_data + y * ogn_step + x * cn; + pad_ptr = pad_data + i * pad_step + j * cn; + memcpy(pad_ptr, ogn_ptr, cn); + } + + // center + int rborder = MIN(cal_width, width - cal_x); + ogn_ptr = ogn_data + y * ogn_step + (j + cal_x) * cn; + pad_ptr = pad_data + i * pad_step + j * cn; + memcpy(pad_ptr, ogn_ptr, cn * (rborder - j)); + + // right border + j = rborder; + for(; j < cal_width; j++) + { + int x = borderInterpolate(j + cal_x, width, border_type); + if(x < 0) // border constant return value -1 + ogn_ptr = &vec_zeros[0]; + else + ogn_ptr = ogn_data + y * ogn_step + x * cn; + pad_ptr = pad_data + i * pad_step + j * cn; + memcpy(pad_ptr, ogn_ptr, cn); + } + } + + std::vector _color_weight(cn * 256); + std::vector _space_weight(d * d); + std::vector _space_ofs(d * d); + float* color_weight = &_color_weight[0]; + float* space_weight = &_space_weight[0]; + int* space_ofs = &_space_ofs[0]; + + // initialize color-related bilateral filter coefficients + + for( i = 0; i < 256 * cn; i++ ) + color_weight[i] = (float)std::exp(i * i * gauss_color_coeff); + + // initialize space-related bilateral filter coefficients + for( i = -radius, maxk = 0; i <= radius; i++ ) + { + j = -radius; + + for( ; j <= radius; j++ ) + { + double r = std::sqrt((double)i * i + (double)j * j); + if( r > radius ) + continue; + space_weight[maxk] = (float)std::exp(r * r * gauss_space_coeff); + space_ofs[maxk++] = (int)(i * pad_step + j * cn); + } + } + + bilateralFilterProcess(dst_data, dst_step, pad_data, pad_step, width, height, cn, radius, maxk, space_ofs, space_weight, color_weight); + + return CV_HAL_ERROR_OK; +} + +} // namespace ndsrvp + +} // namespace cv diff --git a/3rdparty/ndsrvp/src/filter.cpp b/3rdparty/ndsrvp/src/filter.cpp index 89508eea11..6dea08df64 100644 --- a/3rdparty/ndsrvp/src/filter.cpp +++ b/3rdparty/ndsrvp/src/filter.cpp @@ -132,8 +132,8 @@ int filter(cvhalFilter2D *context, // ROI fully used in the computation int cal_width = width + ctx->kernel_width - 1; int cal_height = height + ctx->kernel_height - 1; - int cal_x = offset_x - ctx->anchor_x; - int cal_y = offset_y - ctx->anchor_y; + int cal_x = offset_x - ctx->anchor_x; // negative if left border exceeded + int cal_y = offset_y - ctx->anchor_y; // negative if top border exceeded // calculate source border ctx->padding.resize(cal_width * cal_height * cnes); diff --git a/3rdparty/ndsrvp/src/medianBlur.cpp b/3rdparty/ndsrvp/src/medianBlur.cpp new file mode 100644 index 0000000000..c511367f31 --- /dev/null +++ b/3rdparty/ndsrvp/src/medianBlur.cpp @@ -0,0 +1,300 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "ndsrvp_hal.hpp" +#include "opencv2/imgproc/hal/interface.h" +#include "cvutils.hpp" + +namespace cv { + +namespace ndsrvp { + +struct operators_minmax_t { + inline void vector(uint8x8_t & a, uint8x8_t & b) const { + uint8x8_t t = a; + a = __nds__v_umin8(a, b); + b = __nds__v_umax8(t, b); + } + inline void scalar(uchar & a, uchar & b) const { + uchar t = a; + a = __nds__umin8(a, b); + b = __nds__umax8(t, b); + } + inline void vector(int8x8_t & a, int8x8_t & b) const { + int8x8_t t = a; + a = __nds__v_smin8(a, b); + b = __nds__v_smax8(t, b); + } + inline void scalar(schar & a, schar & b) const { + schar t = a; + a = __nds__smin8(a, b); + b = __nds__smax8(t, b); + } + inline void vector(uint16x4_t & a, uint16x4_t & b) const { + uint16x4_t t = a; + a = __nds__v_umin16(a, b); + b = __nds__v_umax16(t, b); + } + inline void scalar(ushort & a, ushort & b) const { + ushort t = a; + a = __nds__umin16(a, b); + b = __nds__umax16(t, b); + } + inline void vector(int16x4_t & a, int16x4_t & b) const { + int16x4_t t = a; + a = __nds__v_smin16(a, b); + b = __nds__v_smax16(t, b); + } + inline void scalar(short & a, short & b) const { + short t = a; + a = __nds__smin16(a, b); + b = __nds__smax16(t, b); + } +}; + +template // type, widen type, vector type +static void +medianBlur_SortNet( const uchar* src_data, size_t src_step, + uchar* dst_data, size_t dst_step, + int width, int height, int cn, int ksize ) +{ + const T* src = (T*)src_data; + T* dst = (T*)dst_data; + int sstep = (int)(src_step / sizeof(T)); + int dstep = (int)(dst_step / sizeof(T)); + int i, j, k; + operators_minmax_t op; + + if( ksize == 3 ) + { + if( width == 1 || height == 1 ) + { + int len = width + height - 1; + int sdelta = height == 1 ? cn : sstep; + int sdelta0 = height == 1 ? 0 : sstep - cn; + int ddelta = height == 1 ? cn : dstep; + + for( i = 0; i < len; i++, src += sdelta0, dst += ddelta ) + for( j = 0; j < cn; j++, src++ ) + { + T p0 = src[i > 0 ? -sdelta : 0]; + T p1 = src[0]; + T p2 = src[i < len - 1 ? sdelta : 0]; + + op.scalar(p0, p1); op.scalar(p1, p2); op.scalar(p0, p1); + dst[j] = (T)p1; + } + return; + } + + width *= cn; + for( i = 0; i < height; i++, dst += dstep ) + { + const T* row0 = src + std::max(i - 1, 0)*sstep; + const T* row1 = src + i*sstep; + const T* row2 = src + std::min(i + 1, height-1)*sstep; + int limit = cn; + + for(j = 0;; ) + { + for( ; j < limit; j++ ) + { + int j0 = j >= cn ? j - cn : j; + int j2 = j < width - cn ? j + cn : j; + T p0 = row0[j0], p1 = row0[j], p2 = row0[j2]; + T p3 = row1[j0], p4 = row1[j], p5 = row1[j2]; + T p6 = row2[j0], p7 = row2[j], p8 = row2[j2]; + + op.scalar(p1, p2); op.scalar(p4, p5); op.scalar(p7, p8); op.scalar(p0, p1); + op.scalar(p3, p4); op.scalar(p6, p7); op.scalar(p1, p2); op.scalar(p4, p5); + op.scalar(p7, p8); op.scalar(p0, p3); op.scalar(p5, p8); op.scalar(p4, p7); + op.scalar(p3, p6); op.scalar(p1, p4); op.scalar(p2, p5); op.scalar(p4, p7); + op.scalar(p4, p2); op.scalar(p6, p4); op.scalar(p4, p2); + dst[j] = (T)p4; + } + + if( limit == width ) + break; + + int nlanes = 8 / sizeof(T); + + for( ; (cn % nlanes == 0) && (j <= width - nlanes - cn); j += nlanes ) // alignment + { + VT p0 = *(VT*)(row0+j-cn), p1 = *(VT*)(row0+j), p2 = *(VT*)(row0+j+cn); + VT p3 = *(VT*)(row1+j-cn), p4 = *(VT*)(row1+j), p5 = *(VT*)(row1+j+cn); + VT p6 = *(VT*)(row2+j-cn), p7 = *(VT*)(row2+j), p8 = *(VT*)(row2+j+cn); + + op.vector(p1, p2); op.vector(p4, p5); op.vector(p7, p8); op.vector(p0, p1); + op.vector(p3, p4); op.vector(p6, p7); op.vector(p1, p2); op.vector(p4, p5); + op.vector(p7, p8); op.vector(p0, p3); op.vector(p5, p8); op.vector(p4, p7); + op.vector(p3, p6); op.vector(p1, p4); op.vector(p2, p5); op.vector(p4, p7); + op.vector(p4, p2); op.vector(p6, p4); op.vector(p4, p2); + *(VT*)(dst+j) = p4; + } + + limit = width; + } + } + } + else if( ksize == 5 ) + { + if( width == 1 || height == 1 ) + { + int len = width + height - 1; + int sdelta = height == 1 ? cn : sstep; + int sdelta0 = height == 1 ? 0 : sstep - cn; + int ddelta = height == 1 ? cn : dstep; + + for( i = 0; i < len; i++, src += sdelta0, dst += ddelta ) + for( j = 0; j < cn; j++, src++ ) + { + int i1 = i > 0 ? -sdelta : 0; + int i0 = i > 1 ? -sdelta*2 : i1; + int i3 = i < len-1 ? sdelta : 0; + int i4 = i < len-2 ? sdelta*2 : i3; + T p0 = src[i0], p1 = src[i1], p2 = src[0], p3 = src[i3], p4 = src[i4]; + + op.scalar(p0, p1); op.scalar(p3, p4); op.scalar(p2, p3); op.scalar(p3, p4); op.scalar(p0, p2); + op.scalar(p2, p4); op.scalar(p1, p3); op.scalar(p1, p2); + dst[j] = (T)p2; + } + return; + } + + width *= cn; + for( i = 0; i < height; i++, dst += dstep ) + { + const T* row[5]; + row[0] = src + std::max(i - 2, 0)*sstep; + row[1] = src + std::max(i - 1, 0)*sstep; + row[2] = src + i*sstep; + row[3] = src + std::min(i + 1, height-1)*sstep; + row[4] = src + std::min(i + 2, height-1)*sstep; + int limit = cn*2; + + for(j = 0;; ) + { + for( ; j < limit; j++ ) + { + T p[25]; + int j1 = j >= cn ? j - cn : j; + int j0 = j >= cn*2 ? j - cn*2 : j1; + int j3 = j < width - cn ? j + cn : j; + int j4 = j < width - cn*2 ? j + cn*2 : j3; + for( k = 0; k < 5; k++ ) + { + const T* rowk = row[k]; + p[k*5] = rowk[j0]; p[k*5+1] = rowk[j1]; + p[k*5+2] = rowk[j]; p[k*5+3] = rowk[j3]; + p[k*5+4] = rowk[j4]; + } + + op.scalar(p[1], p[2]); op.scalar(p[0], p[1]); op.scalar(p[1], p[2]); op.scalar(p[4], p[5]); op.scalar(p[3], p[4]); + op.scalar(p[4], p[5]); op.scalar(p[0], p[3]); op.scalar(p[2], p[5]); op.scalar(p[2], p[3]); op.scalar(p[1], p[4]); + op.scalar(p[1], p[2]); op.scalar(p[3], p[4]); op.scalar(p[7], p[8]); op.scalar(p[6], p[7]); op.scalar(p[7], p[8]); + op.scalar(p[10], p[11]); op.scalar(p[9], p[10]); op.scalar(p[10], p[11]); op.scalar(p[6], p[9]); op.scalar(p[8], p[11]); + op.scalar(p[8], p[9]); op.scalar(p[7], p[10]); op.scalar(p[7], p[8]); op.scalar(p[9], p[10]); op.scalar(p[0], p[6]); + op.scalar(p[4], p[10]); op.scalar(p[4], p[6]); op.scalar(p[2], p[8]); op.scalar(p[2], p[4]); op.scalar(p[6], p[8]); + op.scalar(p[1], p[7]); op.scalar(p[5], p[11]); op.scalar(p[5], p[7]); op.scalar(p[3], p[9]); op.scalar(p[3], p[5]); + op.scalar(p[7], p[9]); op.scalar(p[1], p[2]); op.scalar(p[3], p[4]); op.scalar(p[5], p[6]); op.scalar(p[7], p[8]); + op.scalar(p[9], p[10]); op.scalar(p[13], p[14]); op.scalar(p[12], p[13]); op.scalar(p[13], p[14]); op.scalar(p[16], p[17]); + op.scalar(p[15], p[16]); op.scalar(p[16], p[17]); op.scalar(p[12], p[15]); op.scalar(p[14], p[17]); op.scalar(p[14], p[15]); + op.scalar(p[13], p[16]); op.scalar(p[13], p[14]); op.scalar(p[15], p[16]); op.scalar(p[19], p[20]); op.scalar(p[18], p[19]); + op.scalar(p[19], p[20]); op.scalar(p[21], p[22]); op.scalar(p[23], p[24]); op.scalar(p[21], p[23]); op.scalar(p[22], p[24]); + op.scalar(p[22], p[23]); op.scalar(p[18], p[21]); op.scalar(p[20], p[23]); op.scalar(p[20], p[21]); op.scalar(p[19], p[22]); + op.scalar(p[22], p[24]); op.scalar(p[19], p[20]); op.scalar(p[21], p[22]); op.scalar(p[23], p[24]); op.scalar(p[12], p[18]); + op.scalar(p[16], p[22]); op.scalar(p[16], p[18]); op.scalar(p[14], p[20]); op.scalar(p[20], p[24]); op.scalar(p[14], p[16]); + op.scalar(p[18], p[20]); op.scalar(p[22], p[24]); op.scalar(p[13], p[19]); op.scalar(p[17], p[23]); op.scalar(p[17], p[19]); + op.scalar(p[15], p[21]); op.scalar(p[15], p[17]); op.scalar(p[19], p[21]); op.scalar(p[13], p[14]); op.scalar(p[15], p[16]); + op.scalar(p[17], p[18]); op.scalar(p[19], p[20]); op.scalar(p[21], p[22]); op.scalar(p[23], p[24]); op.scalar(p[0], p[12]); + op.scalar(p[8], p[20]); op.scalar(p[8], p[12]); op.scalar(p[4], p[16]); op.scalar(p[16], p[24]); op.scalar(p[12], p[16]); + op.scalar(p[2], p[14]); op.scalar(p[10], p[22]); op.scalar(p[10], p[14]); op.scalar(p[6], p[18]); op.scalar(p[6], p[10]); + op.scalar(p[10], p[12]); op.scalar(p[1], p[13]); op.scalar(p[9], p[21]); op.scalar(p[9], p[13]); op.scalar(p[5], p[17]); + op.scalar(p[13], p[17]); op.scalar(p[3], p[15]); op.scalar(p[11], p[23]); op.scalar(p[11], p[15]); op.scalar(p[7], p[19]); + op.scalar(p[7], p[11]); op.scalar(p[11], p[13]); op.scalar(p[11], p[12]); + dst[j] = (T)p[12]; + } + + if( limit == width ) + break; + + int nlanes = 8 / sizeof(T); + + for( ; (cn % nlanes == 0) && (j <= width - nlanes - cn*2); j += nlanes ) + { + VT p0 = *(VT*)(row[0]+j-cn*2), p5 = *(VT*)(row[1]+j-cn*2), p10 = *(VT*)(row[2]+j-cn*2), p15 = *(VT*)(row[3]+j-cn*2), p20 = *(VT*)(row[4]+j-cn*2); + VT p1 = *(VT*)(row[0]+j-cn*1), p6 = *(VT*)(row[1]+j-cn*1), p11 = *(VT*)(row[2]+j-cn*1), p16 = *(VT*)(row[3]+j-cn*1), p21 = *(VT*)(row[4]+j-cn*1); + VT p2 = *(VT*)(row[0]+j-cn*0), p7 = *(VT*)(row[1]+j-cn*0), p12 = *(VT*)(row[2]+j-cn*0), p17 = *(VT*)(row[3]+j-cn*0), p22 = *(VT*)(row[4]+j-cn*0); + VT p3 = *(VT*)(row[0]+j+cn*1), p8 = *(VT*)(row[1]+j+cn*1), p13 = *(VT*)(row[2]+j+cn*1), p18 = *(VT*)(row[3]+j+cn*1), p23 = *(VT*)(row[4]+j+cn*1); + VT p4 = *(VT*)(row[0]+j+cn*2), p9 = *(VT*)(row[1]+j+cn*2), p14 = *(VT*)(row[2]+j+cn*2), p19 = *(VT*)(row[3]+j+cn*2), p24 = *(VT*)(row[4]+j+cn*2); + + op.vector(p1, p2); op.vector(p0, p1); op.vector(p1, p2); op.vector(p4, p5); op.vector(p3, p4); + op.vector(p4, p5); op.vector(p0, p3); op.vector(p2, p5); op.vector(p2, p3); op.vector(p1, p4); + op.vector(p1, p2); op.vector(p3, p4); op.vector(p7, p8); op.vector(p6, p7); op.vector(p7, p8); + op.vector(p10, p11); op.vector(p9, p10); op.vector(p10, p11); op.vector(p6, p9); op.vector(p8, p11); + op.vector(p8, p9); op.vector(p7, p10); op.vector(p7, p8); op.vector(p9, p10); op.vector(p0, p6); + op.vector(p4, p10); op.vector(p4, p6); op.vector(p2, p8); op.vector(p2, p4); op.vector(p6, p8); + op.vector(p1, p7); op.vector(p5, p11); op.vector(p5, p7); op.vector(p3, p9); op.vector(p3, p5); + op.vector(p7, p9); op.vector(p1, p2); op.vector(p3, p4); op.vector(p5, p6); op.vector(p7, p8); + op.vector(p9, p10); op.vector(p13, p14); op.vector(p12, p13); op.vector(p13, p14); op.vector(p16, p17); + op.vector(p15, p16); op.vector(p16, p17); op.vector(p12, p15); op.vector(p14, p17); op.vector(p14, p15); + op.vector(p13, p16); op.vector(p13, p14); op.vector(p15, p16); op.vector(p19, p20); op.vector(p18, p19); + op.vector(p19, p20); op.vector(p21, p22); op.vector(p23, p24); op.vector(p21, p23); op.vector(p22, p24); + op.vector(p22, p23); op.vector(p18, p21); op.vector(p20, p23); op.vector(p20, p21); op.vector(p19, p22); + op.vector(p22, p24); op.vector(p19, p20); op.vector(p21, p22); op.vector(p23, p24); op.vector(p12, p18); + op.vector(p16, p22); op.vector(p16, p18); op.vector(p14, p20); op.vector(p20, p24); op.vector(p14, p16); + op.vector(p18, p20); op.vector(p22, p24); op.vector(p13, p19); op.vector(p17, p23); op.vector(p17, p19); + op.vector(p15, p21); op.vector(p15, p17); op.vector(p19, p21); op.vector(p13, p14); op.vector(p15, p16); + op.vector(p17, p18); op.vector(p19, p20); op.vector(p21, p22); op.vector(p23, p24); op.vector(p0, p12); + op.vector(p8, p20); op.vector(p8, p12); op.vector(p4, p16); op.vector(p16, p24); op.vector(p12, p16); + op.vector(p2, p14); op.vector(p10, p22); op.vector(p10, p14); op.vector(p6, p18); op.vector(p6, p10); + op.vector(p10, p12); op.vector(p1, p13); op.vector(p9, p21); op.vector(p9, p13); op.vector(p5, p17); + op.vector(p13, p17); op.vector(p3, p15); op.vector(p11, p23); op.vector(p11, p15); op.vector(p7, p19); + op.vector(p7, p11); op.vector(p11, p13); op.vector(p11, p12); + *(VT*)(dst+j) = p12; + } + + limit = width; + } + } + } +} + +int medianBlur(const uchar* src_data, size_t src_step, + uchar* dst_data, size_t dst_step, + int width, int height, int depth, int cn, int ksize) +{ + bool useSortNet = ((ksize == 3) || (ksize == 5 && ( depth > CV_8U || cn == 2 || cn > 4 ))); + + if( useSortNet ) + { + uchar* src_data_rep; + if( dst_data == src_data ) { + std::vector src_data_copy(src_step * height); + memcpy(src_data_copy.data(), src_data, src_step * height); + src_data_rep = &src_data_copy[0]; + } + else { + src_data_rep = (uchar*)src_data; + } + + if( depth == CV_8U ) + medianBlur_SortNet( src_data_rep, src_step, dst_data, dst_step, width, height, cn, ksize ); + else if( depth == CV_8S ) + medianBlur_SortNet( src_data_rep, src_step, dst_data, dst_step, width, height, cn, ksize ); + else if( depth == CV_16U ) + medianBlur_SortNet( src_data_rep, src_step, dst_data, dst_step, width, height, cn, ksize ); + else if( depth == CV_16S ) + medianBlur_SortNet( src_data_rep, src_step, dst_data, dst_step, width, height, cn, ksize ); + else + return CV_HAL_ERROR_NOT_IMPLEMENTED; + + return CV_HAL_ERROR_OK; + } + else return CV_HAL_ERROR_NOT_IMPLEMENTED; +} + +} // namespace ndsrvp + +} // namespace cv From 04818d6dd5c3501bb3cf0895dfc01b46c23e275a Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Thu, 17 Oct 2024 23:35:38 +0300 Subject: [PATCH 02/10] build: made environment access a separate feature --- CMakeLists.txt | 1 + cmake/platforms/OpenCV-WinRT.cmake | 2 +- cmake/platforms/OpenCV-WindowsCE.cmake | 2 +- .../env_reference/env_reference.markdown | 15 +++--- modules/core/CMakeLists.txt | 4 ++ .../core/utils/configuration.private.hpp | 6 +-- modules/core/src/ocl.cpp | 43 +++++++-------- .../core/src/opencl/runtime/opencl_core.cpp | 45 ++++++++-------- .../src/parallel/registry_parallel.impl.hpp | 2 +- modules/core/src/system.cpp | 53 +++++++------------ modules/core/src/utils/datafile.cpp | 2 +- modules/core/src/utils/filesystem.cpp | 12 ++--- modules/core/src/va_wrapper.impl.hpp | 5 +- modules/dnn/perf/perf_main.cpp | 11 +--- modules/dnn/src/precomp.hpp | 3 +- modules/dnn/src/vkcom/vulkan/vk_loader.cpp | 15 ++---- modules/dnn/test/test_common.impl.hpp | 9 +--- modules/dnn/test/test_ie_models.cpp | 12 ++--- modules/dnn/test/test_precomp.hpp | 1 + modules/gapi/src/compiler/gcompiler.cpp | 6 +-- modules/gapi/src/precomp.hpp | 1 + .../gapi/test/infer/gapi_infer_ie_test.cpp | 10 ++-- .../gapi/test/infer/gapi_infer_onnx_test.cpp | 5 +- .../gapi/test/infer/gapi_infer_ov_tests.cpp | 8 ++- modules/gapi/test/test_precomp.hpp | 2 + modules/highgui/src/precomp.hpp | 1 + modules/highgui/src/registry.impl.hpp | 2 +- modules/highgui/src/window.cpp | 2 +- modules/highgui/src/window_wayland.cpp | 2 +- modules/objdetect/test/test_main.cpp | 9 +--- modules/ts/include/opencv2/ts.hpp | 1 + modules/ts/src/precomp.hpp | 1 + modules/ts/src/ts.cpp | 29 ++++------ modules/ts/src/ts_perf.cpp | 34 +++++------- modules/video/perf/perf_main.cpp | 10 +--- modules/video/perf/perf_precomp.hpp | 1 + modules/video/test/test_main.cpp | 10 +--- modules/video/test/test_precomp.hpp | 1 + modules/videoio/src/backend_plugin.cpp | 8 ++- modules/videoio/src/cap_dshow.cpp | 22 +++----- modules/videoio/src/cap_ffmpeg_impl.hpp | 36 ++++++------- modules/videoio/src/videoio_registry.cpp | 2 +- modules/videoio/test/test_main.cpp | 4 +- modules/videoio/test/test_precomp.hpp | 1 + 44 files changed, 176 insertions(+), 275 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8411604d32..c8e4ef09c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -536,6 +536,7 @@ OCV_OPTION(ENABLE_CONFIG_VERIFICATION "Fail build if actual configuration doesn' OCV_OPTION(OPENCV_ENABLE_MEMALIGN "Enable posix_memalign or memalign usage" ON) OCV_OPTION(OPENCV_DISABLE_FILESYSTEM_SUPPORT "Disable filesystem support" OFF) OCV_OPTION(OPENCV_DISABLE_THREAD_SUPPORT "Build the library without multi-threaded code." OFF) +OCV_OPTION(OPENCV_DISABLE_ENV_SUPPORT "Disable environment variables access (getenv)" (CMAKE_SYSTEM_NAME MATCHES "Windows(CE|Phone|Store)")) OCV_OPTION(OPENCV_SEMIHOSTING "Build the library for semihosting target (Arm). See https://developer.arm.com/documentation/100863/latest." OFF) OCV_OPTION(ENABLE_CUDA_FIRST_CLASS_LANGUAGE "Enable CUDA as a first class language, if enabled dependant projects will need to use CMake >= 3.18" OFF VISIBLE_IF (WITH_CUDA AND NOT CMAKE_VERSION VERSION_LESS 3.18) diff --git a/cmake/platforms/OpenCV-WinRT.cmake b/cmake/platforms/OpenCV-WinRT.cmake index 0a5e9f3c7d..350c280fd2 100644 --- a/cmake/platforms/OpenCV-WinRT.cmake +++ b/cmake/platforms/OpenCV-WinRT.cmake @@ -1,6 +1,6 @@ set(WINRT TRUE) -add_definitions(-DWINRT -DNO_GETENV) +add_definitions(-DWINRT) # Making definitions available to other configurations and # to filter dependency restrictions at compile time. diff --git a/cmake/platforms/OpenCV-WindowsCE.cmake b/cmake/platforms/OpenCV-WindowsCE.cmake index f5ac590754..1bb8bf6d7f 100644 --- a/cmake/platforms/OpenCV-WindowsCE.cmake +++ b/cmake/platforms/OpenCV-WindowsCE.cmake @@ -1 +1 @@ -add_definitions(-DNO_GETENV) +# empty diff --git a/doc/tutorials/introduction/env_reference/env_reference.markdown b/doc/tutorials/introduction/env_reference/env_reference.markdown index afbde7b715..88b4008c8f 100644 --- a/doc/tutorials/introduction/env_reference/env_reference.markdown +++ b/doc/tutorials/introduction/env_reference/env_reference.markdown @@ -59,7 +59,6 @@ import cv2 # variables set after this may not have effect ## Types -- _non-null_ - set to anything to enable feature, in some cases can be interpreted as other types (e.g. path) - _bool_ - `1`, `True`, `true`, `TRUE` / `0`, `False`, `false`, `FALSE` - _number_/_size_ - unsigned number, suffixes `MB`, `Mb`, `mb`, `KB`, `Kb`, `kb` - _string_ - plain string or can have a structure @@ -70,7 +69,7 @@ import cv2 # variables set after this may not have effect ## General, core | name | type | default | description | |------|------|---------|-------------| -| OPENCV_SKIP_CPU_BASELINE_CHECK | non-null | | do not check that current CPU supports all features used by the build (baseline) | +| OPENCV_SKIP_CPU_BASELINE_CHECK | bool | false | do not check that current CPU supports all features used by the build (baseline) | | OPENCV_CPU_DISABLE | `,` or `;`-separated | | disable code branches which use CPU features (dispatched code) | | OPENCV_SETUP_TERMINATE_HANDLER | bool | true (Windows) | use std::set_terminate to install own termination handler | | OPENCV_LIBVA_RUNTIME | file path | | libva for VA interoperability utils | @@ -78,9 +77,9 @@ import cv2 # variables set after this may not have effect | OPENCV_BUFFER_AREA_ALWAYS_SAFE | bool | false | enable safe mode for multi-buffer allocations (each buffer separately) | | OPENCV_KMEANS_PARALLEL_GRANULARITY | num | 1000 | tune algorithm parallel work distribution parameter `parallel_for_(..., ..., ..., granularity)` | | OPENCV_DUMP_ERRORS | bool | true (Debug or Android), false (others) | print extra information on exception (log to Android) | -| OPENCV_DUMP_CONFIG | non-null | | print build configuration to stderr (`getBuildInformation`) | +| OPENCV_DUMP_CONFIG | bool | false | print build configuration to stderr (`getBuildInformation`) | | OPENCV_PYTHON_DEBUG | bool | false | enable extra warnings in Python bindings | -| OPENCV_TEMP_PATH | non-null / path | `/tmp/` (Linux), `/data/local/tmp/` (Android), `GetTempPathA` (Windows) | directory for temporary files | +| OPENCV_TEMP_PATH | path | `/tmp/` (Linux), `/data/local/tmp/` (Android), `GetTempPathA` (Windows) | directory for temporary files | | OPENCV_DATA_PATH_HINT | paths | | paths for findDataFile | | OPENCV_DATA_PATH | paths | | paths for findDataFile | | OPENCV_SAMPLES_DATA_PATH_HINT | paths | | paths for findDataFile | @@ -272,7 +271,7 @@ Some external dependencies can be detached into a dynamic library, which will be | ⭐ OPENCV_FFMPEG_CAPTURE_OPTIONS | string (see note) | | extra options for VideoCapture FFmpeg backend | | ⭐ OPENCV_FFMPEG_WRITER_OPTIONS | string (see note) | | extra options for VideoWriter FFmpeg backend | | OPENCV_FFMPEG_THREADS | num | | set FFmpeg thread count | -| OPENCV_FFMPEG_DEBUG | non-null | | enable logging messages from FFmpeg | +| OPENCV_FFMPEG_DEBUG | bool | false | enable logging messages from FFmpeg | | OPENCV_FFMPEG_LOGLEVEL | num | | set FFmpeg logging level | | OPENCV_FFMPEG_DLL_DIR | dir path | | directory with FFmpeg plugin (legacy) | | OPENCV_FFMPEG_IS_THREAD_SAFE | bool | false | enabling this option will turn off thread safety locks in the FFmpeg backend (use only if you are sure FFmpeg is built with threading support, tested on Linux) | @@ -286,7 +285,7 @@ Some external dependencies can be detached into a dynamic library, which will be | OPENCV_VIDEOIO_MFX_BITRATE_DIVISOR | num | 300 | this option allows to tune encoding bitrate (video quality/size) | | OPENCV_VIDEOIO_MFX_WRITER_TIMEOUT | num | 1 | timeout for encoding operation (in seconds) | | OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS | bool | true | allow HW-accelerated transformations (DXVA) in MediaFoundation processing graph (may slow down camera probing process) | -| OPENCV_DSHOW_DEBUG | non-null | | enable verbose logging in the DShow backend | +| OPENCV_DSHOW_DEBUG | bool | false | enable verbose logging in the DShow backend | | OPENCV_DSHOW_SAVEGRAPH_FILENAME | file path | | enable processing graph tump in the DShow backend | | OPENCV_VIDEOIO_V4L_RANGE_NORMALIZED | bool | false | use (0, 1) range for properties (V4L) | | OPENCV_VIDEOIO_V4L_SELECT_TIMEOUT | num | 10 | timeout for select call (in seconds) (V4L) | @@ -297,7 +296,7 @@ Some external dependencies can be detached into a dynamic library, which will be ### videoio tests | name | type | default | description | |------|------|---------|-------------| -| OPENCV_TEST_VIDEOIO_BACKEND_REQUIRE_FFMPEG | | | test app will exit if no FFmpeg backend is available | +| OPENCV_TEST_VIDEOIO_BACKEND_REQUIRE_FFMPEG | bool | false | test app will exit if no FFmpeg backend is available | | OPENCV_TEST_V4L2_VIVID_DEVICE | file path | | path to VIVID virtual camera device for V4L2 test (e.g. `/dev/video5`) | | OPENCV_TEST_PERF_CAMERA_LIST | paths | | cameras to use in performance test (waitAny_V4L test) | | OPENCV_TEST_CAMERA_%d_FPS | num | | fps to set for N-th camera (0-based index) (waitAny_V4L test) | @@ -327,7 +326,7 @@ Some external dependencies can be detached into a dynamic library, which will be | name | type | default | description | |------|------|---------|-------------| -| OPENCV_LEGACY_WAITKEY | non-null | | switch `waitKey` return result (default behavior: `return code & 0xff` (or -1), legacy behavior: `return code`) | +| OPENCV_LEGACY_WAITKEY | bool | false | switch `waitKey` return result (default behavior: `return code & 0xff` (or -1), legacy behavior: `return code`) | | $XDG_RUNTIME_DIR | | | Wayland backend specific - create shared memory-mapped file for interprocess communication (named `opencv-shared-??????`) | | OPENCV_HIGHGUI_FB_MODE | string | `FB` | Selects output mode for the framebuffer backend (`FB` - regular frambuffer, `EMU` - emulation, perform internal checks but does nothing, `XVFB` - compatible with _xvfb_ virtual frambuffer) | | OPENCV_HIGHGUI_FB_DEVICE | file path | | Path to frambuffer device to use (will be checked first) | diff --git a/modules/core/CMakeLists.txt b/modules/core/CMakeLists.txt index ea1100c954..95fe8d6a4c 100644 --- a/modules/core/CMakeLists.txt +++ b/modules/core/CMakeLists.txt @@ -182,6 +182,10 @@ if(OPENCV_DISABLE_THREAD_SUPPORT) ocv_target_compile_definitions(${the_module} PUBLIC "OPENCV_DISABLE_THREAD_SUPPORT=1") endif() +if(OPENCV_DISABLE_ENV_SUPPORT) + ocv_append_source_file_compile_definitions(${CMAKE_CURRENT_SOURCE_DIR}/src/system.cpp "NO_GETENV") +endif() + if(OPENCV_SEMIHOSTING) ocv_target_compile_definitions(${the_module} PRIVATE "-DOPENCV_SEMIHOSTING") endif(OPENCV_SEMIHOSTING) diff --git a/modules/core/include/opencv2/core/utils/configuration.private.hpp b/modules/core/include/opencv2/core/utils/configuration.private.hpp index b35f35e9d4..756b477165 100644 --- a/modules/core/include/opencv2/core/utils/configuration.private.hpp +++ b/modules/core/include/opencv2/core/utils/configuration.private.hpp @@ -12,9 +12,9 @@ namespace cv { namespace utils { typedef std::vector Paths; -CV_EXPORTS bool getConfigurationParameterBool(const char* name, bool defaultValue); -CV_EXPORTS size_t getConfigurationParameterSizeT(const char* name, size_t defaultValue); -CV_EXPORTS cv::String getConfigurationParameterString(const char* name, const char* defaultValue); +CV_EXPORTS bool getConfigurationParameterBool(const char* name, bool defaultValue = false); +CV_EXPORTS size_t getConfigurationParameterSizeT(const char* name, size_t defaultValue = 0); +CV_EXPORTS std::string getConfigurationParameterString(const char* name, const std::string & defaultValue = std::string()); CV_EXPORTS Paths getConfigurationParameterPaths(const char* name, const Paths &defaultValue = Paths()); }} // namespace diff --git a/modules/core/src/ocl.cpp b/modules/core/src/ocl.cpp index 8d7d7faf44..8479667fd7 100644 --- a/modules/core/src/ocl.cpp +++ b/modules/core/src/ocl.cpp @@ -1170,10 +1170,10 @@ bool haveOpenCL() if (!g_isOpenCLInitialized) { CV_TRACE_REGION("Init_OpenCL_Runtime"); - const char* envPath = getenv("OPENCV_OPENCL_RUNTIME"); - if (envPath) + std::string envPath = utils::getConfigurationParameterString("OPENCV_OPENCL_RUNTIME"); + if (!envPath.empty()) { - if (cv::String(envPath) == "disabled") + if (envPath == "disabled") { g_isOpenCLAvailable = false; g_isOpenCLInitialized = true; @@ -2119,24 +2119,18 @@ static bool parseOpenCLDeviceConfiguration(const std::string& configurationStr, return true; } -#if defined WINRT || defined _WIN32_WCE -static cl_device_id selectOpenCLDevice(const char* configuration = NULL) -{ - CV_UNUSED(configuration) - return NULL; -} -#else -static cl_device_id selectOpenCLDevice(const char* configuration = NULL) +static cl_device_id selectOpenCLDevice(const std::string & configuration_ = std::string()) { std::string platform, deviceName; std::vector deviceTypes; - if (!configuration) - configuration = getenv("OPENCV_OPENCL_DEVICE"); + std::string configuration(configuration_); + if (configuration.empty()) + configuration = utils::getConfigurationParameterString("OPENCV_OPENCL_DEVICE"); - if (configuration && - (strcmp(configuration, "disabled") == 0 || - !parseOpenCLDeviceConfiguration(std::string(configuration), platform, deviceTypes, deviceName) + if (!configuration.empty() && + (configuration == "disabled" || + !parseOpenCLDeviceConfiguration(configuration, platform, deviceTypes, deviceName) )) return NULL; @@ -2204,7 +2198,7 @@ static cl_device_id selectOpenCLDevice(const char* configuration = NULL) if (!isID) { deviceTypes.push_back("GPU"); - if (configuration) + if (!configuration.empty()) deviceTypes.push_back("CPU"); } else @@ -2272,7 +2266,7 @@ static cl_device_id selectOpenCLDevice(const char* configuration = NULL) } not_found: - if (!configuration) + if (configuration.empty()) return NULL; // suppress messages on stderr std::ostringstream msg; @@ -2287,7 +2281,6 @@ not_found: CV_LOG_ERROR(NULL, msg.str()); return NULL; } -#endif #ifdef HAVE_OPENCL_SVM namespace svm { @@ -2340,12 +2333,12 @@ static unsigned int getSVMCapabilitiesMask() static unsigned int mask = 0; if (!initialized) { - const char* envValue = getenv("OPENCV_OPENCL_SVM_CAPABILITIES_MASK"); - if (envValue == NULL) + const std::string envValue = utils::getConfigurationParameterString("OPENCV_OPENCL_SVM_CAPABILITIES_MASK"); + if (envValue.empty()) { return ~0U; // all bits 1 } - mask = atoi(envValue); + mask = atoi(envValue.c_str()); initialized = true; } return mask; @@ -2482,8 +2475,8 @@ public: std::string configuration = configuration_; if (configuration_.empty()) { - const char* c = getenv("OPENCV_OPENCL_DEVICE"); - if (c) + const std::string c = utils::getConfigurationParameterString("OPENCV_OPENCL_DEVICE"); + if (!c.empty()) configuration = c; } Impl* impl = findContext(configuration); @@ -2494,7 +2487,7 @@ public: return impl; } - cl_device_id d = selectOpenCLDevice(configuration.empty() ? NULL : configuration.c_str()); + cl_device_id d = selectOpenCLDevice(configuration); if (d == NULL) return NULL; diff --git a/modules/core/src/opencl/runtime/opencl_core.cpp b/modules/core/src/opencl/runtime/opencl_core.cpp index f264d623c5..3a454faa29 100644 --- a/modules/core/src/opencl/runtime/opencl_core.cpp +++ b/modules/core/src/opencl/runtime/opencl_core.cpp @@ -44,6 +44,7 @@ #if defined(HAVE_OPENCL) #include "opencv2/core.hpp" // CV_Error +#include "opencv2/core/utils/configuration.private.hpp" #if defined(HAVE_OPENCL_STATIC) #if defined __APPLE__ @@ -64,18 +65,14 @@ CV_SUPPRESS_DEPRECATED_END #define ERROR_MSG_CANT_LOAD "Failed to load OpenCL runtime\n" #define ERROR_MSG_INVALID_VERSION "Failed to load OpenCL runtime (expected version 1.1+)\n" -static const char* getRuntimePath(const char* defaultPath) +static std::string getRuntimePath(const std::string & defaultPath) { - const char* envPath = getenv("OPENCV_OPENCL_RUNTIME"); - if (envPath) - { - static const char disabled_str[] = "disabled"; - if ((strlen(envPath) == sizeof(disabled_str) - 1) && - (memcmp(envPath, disabled_str, sizeof(disabled_str) - 1) == 0)) - return NULL; - return envPath; - } - return defaultPath; + const std::string res = cv::utils::getConfigurationParameterString( + "OPENCV_OPENCL_RUNTIME", defaultPath); + if (res == "disabled") + return std::string(); + else + return res; } #if defined(__APPLE__) @@ -91,9 +88,9 @@ static void* AppleCLGetProcAddress(const char* name) if (!initialized) { const char* defaultPath = "/System/Library/Frameworks/OpenCL.framework/Versions/Current/OpenCL"; - const char* path = getRuntimePath(defaultPath); - if (path) - handle = dlopen(path, RTLD_LAZY | RTLD_GLOBAL); + std::string path = getRuntimePath(defaultPath); + if (!path.empty()) + handle = dlopen(path.c_str(), RTLD_LAZY | RTLD_GLOBAL); if (handle == NULL) { if (path != NULL && path != defaultPath) @@ -129,13 +126,13 @@ static void* WinGetProcAddress(const char* name) handle = GetModuleHandleA("OpenCL.dll"); if (!handle) { - const char* defaultPath = "OpenCL.dll"; - const char* path = getRuntimePath(defaultPath); - if (path) - handle = LoadLibraryA(path); + const std::string defaultPath = "OpenCL.dll"; + const std::string path = getRuntimePath(defaultPath); + if (!path.empty()) + handle = LoadLibraryA(path.c_str()); if (!handle) { - if (path != NULL && path != defaultPath) + if (!path.empty() && path != defaultPath) fprintf(stderr, ERROR_MSG_CANT_LOAD); } else if (GetProcAddress(handle, OPENCL_FUNC_TO_CHECK_1_1) == NULL) @@ -205,8 +202,8 @@ static void* GetProcAddress(const char* name) bool foundOpenCL = false; for (unsigned int i = 0; i < (sizeof(defaultAndroidPaths)/sizeof(char*)); i++) { - const char* path = (i==0) ? getRuntimePath(defaultAndroidPaths[i]) : defaultAndroidPaths[i]; - if (path) { + const std::string path = (i==0) ? getRuntimePath(defaultAndroidPaths[i]) : defaultAndroidPaths[i]; + if (!path.empty()) { handle = GetHandle(path); if (handle) { foundOpenCL = true; @@ -236,10 +233,10 @@ static void* GetProcAddress(const char* name) if (!initialized) { const char* defaultPath = "libOpenCL.so"; - const char* path = getRuntimePath(defaultPath); - if (path) + const std::string path = getRuntimePath(defaultPath); + if (!path.empty()) { - handle = GetHandle(path); + handle = GetHandle(path.c_str()); if (!handle) { if (path == defaultPath) diff --git a/modules/core/src/parallel/registry_parallel.impl.hpp b/modules/core/src/parallel/registry_parallel.impl.hpp index 2208748bb1..471cd681e4 100644 --- a/modules/core/src/parallel/registry_parallel.impl.hpp +++ b/modules/core/src/parallel/registry_parallel.impl.hpp @@ -111,7 +111,7 @@ protected: bool readPrioritySettings() { bool hasChanges = false; - cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_PARALLEL_PRIORITY_LIST", NULL); + cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_PARALLEL_PRIORITY_LIST"); if (prioritized_backends.empty()) return hasChanges; CV_LOG_INFO(NULL, "core(parallel): Configured priority list (OPENCV_PARALLEL_PRIORITY_LIST): " << prioritized_backends); diff --git a/modules/core/src/system.cpp b/modules/core/src/system.cpp index 7e819acac7..810f62297a 100644 --- a/modules/core/src/system.cpp +++ b/modules/core/src/system.cpp @@ -450,13 +450,11 @@ struct HWFeatures void initialize(void) { -#ifndef NO_GETENV - if (getenv("OPENCV_DUMP_CONFIG")) + if (utils::getConfigurationParameterBool("OPENCV_DUMP_CONFIG")) { fprintf(stderr, "\nOpenCV build configuration is:\n%s\n", cv::getBuildInformation().c_str()); } -#endif initializeNames(); @@ -731,12 +729,10 @@ struct HWFeatures #endif bool skip_baseline_check = false; -#ifndef NO_GETENV - if (getenv("OPENCV_SKIP_CPU_BASELINE_CHECK")) + if (utils::getConfigurationParameterBool("OPENCV_SKIP_CPU_BASELINE_CHECK")) { skip_baseline_check = true; } -#endif int baseline_features[] = { CV_CPU_BASELINE_FEATURES }; if (!checkFeatures(baseline_features, sizeof(baseline_features) / sizeof(baseline_features[0])) && !skip_baseline_check) @@ -786,15 +782,10 @@ struct HWFeatures void readSettings(const int* baseline_features, int baseline_count) { bool dump = true; - const char* disabled_features = -#ifdef NO_GETENV - NULL; -#else - getenv("OPENCV_CPU_DISABLE"); -#endif - if (disabled_features && disabled_features[0] != 0) + std::string disabled_features = utils::getConfigurationParameterString("OPENCV_CPU_DISABLE"); + if (!disabled_features.empty()) { - const char* start = disabled_features; + const char* start = disabled_features.c_str(); for (;;) { while (start[0] != 0 && isSymbolSeparator(start[0])) @@ -1080,20 +1071,19 @@ String tempfile( const char* suffix ) { #if OPENCV_HAVE_FILESYSTEM_SUPPORT String fname; -#ifndef NO_GETENV - const char *temp_dir = getenv("OPENCV_TEMP_PATH"); -#endif + + std::string temp_dir = utils::getConfigurationParameterString("OPENCV_TEMP_PATH"); #if defined _WIN32 #ifdef WINRT RoInitialize(RO_INIT_MULTITHREADED); - std::wstring temp_dir = GetTempPathWinRT(); + std::wstring temp_dir_rt = GetTempPathWinRT(); std::wstring temp_file = GetTempFileNameWinRT(L"ocv"); if (temp_file.empty()) return String(); - temp_file = temp_dir.append(std::wstring(L"\\")).append(temp_file); + temp_file = temp_dir_rt.append(std::wstring(L"\\")).append(temp_file); DeleteFileW(temp_file.c_str()); char aname[MAX_PATH]; @@ -1103,12 +1093,12 @@ String tempfile( const char* suffix ) RoUninitialize(); #elif defined(_WIN32_WCE) const auto kMaxPathSize = MAX_PATH+1; - wchar_t temp_dir[kMaxPathSize] = {0}; + wchar_t temp_dir_ce[kMaxPathSize] = {0}; wchar_t temp_file[kMaxPathSize] = {0}; - ::GetTempPathW(kMaxPathSize, temp_dir); + ::GetTempPathW(kMaxPathSize, temp_dir_ce); - if(0 != ::GetTempFileNameW(temp_dir, L"ocv", 0, temp_file)) { + if(0 != ::GetTempFileNameW(temp_dir_ce, L"ocv", 0, temp_file)) { DeleteFileW(temp_file); char aname[MAX_PATH]; size_t copied = wcstombs(aname, temp_file, MAX_PATH); @@ -1119,12 +1109,12 @@ String tempfile( const char* suffix ) char temp_dir2[MAX_PATH] = { 0 }; char temp_file[MAX_PATH] = { 0 }; - if (temp_dir == 0 || temp_dir[0] == 0) + if (temp_dir.empty()) { ::GetTempPathA(sizeof(temp_dir2), temp_dir2); - temp_dir = temp_dir2; + temp_dir = std::string(temp_dir2); } - if(0 == ::GetTempFileNameA(temp_dir, "ocv", 0, temp_file)) + if(0 == ::GetTempFileNameA(temp_dir.c_str(), "ocv", 0, temp_file)) return String(); DeleteFileA(temp_file); @@ -1139,7 +1129,7 @@ String tempfile( const char* suffix ) char defaultTemplate[] = "/tmp/__opencv_temp.XXXXXX"; # endif - if (temp_dir == 0 || temp_dir[0] == 0) + if (temp_dir.empty()) fname = defaultTemplate; else { @@ -2289,9 +2279,9 @@ size_t utils::getConfigurationParameterSizeT(const char* name, size_t defaultVal return read(name, defaultValue); } -cv::String utils::getConfigurationParameterString(const char* name, const char* defaultValue) +std::string utils::getConfigurationParameterString(const char* name, const std::string & defaultValue) { - return read(name, defaultValue ? cv::String(defaultValue) : cv::String()); + return read(name, defaultValue); } utils::Paths utils::getConfigurationParameterPaths(const char* name, const utils::Paths &defaultValue) @@ -2588,11 +2578,8 @@ public: } ippFeatures = cpuFeatures; - const char* pIppEnv = getenv("OPENCV_IPP"); - cv::String env; - if(pIppEnv != NULL) - env = pIppEnv; - if(env.size()) + std::string env = utils::getConfigurationParameterString("OPENCV_IPP"); + if(!env.empty()) { #if IPP_VERSION_X100 >= 201900 const Ipp64u minorFeatures = ippCPUID_MOVBE|ippCPUID_AES|ippCPUID_CLMUL|ippCPUID_ABR|ippCPUID_RDRAND|ippCPUID_F16C| diff --git a/modules/core/src/utils/datafile.cpp b/modules/core/src/utils/datafile.cpp index 67cc2571ac..380ba59258 100644 --- a/modules/core/src/utils/datafile.cpp +++ b/modules/core/src/utils/datafile.cpp @@ -60,7 +60,7 @@ static std::vector& _getDataSearchSubDirectory() CV_EXPORTS void addDataSearchPath(const cv::String& path) { - if (utils::fs::isDirectory(path)) + if (!path.empty() && utils::fs::isDirectory(path)) _getDataSearchPath().push_back(path); } CV_EXPORTS void addDataSearchSubDirectory(const cv::String& subdir) diff --git a/modules/core/src/utils/filesystem.cpp b/modules/core/src/utils/filesystem.cpp index d416284062..0a44d48d40 100644 --- a/modules/core/src/utils/filesystem.cpp +++ b/modules/core/src/utils/filesystem.cpp @@ -447,8 +447,8 @@ cv::String getCacheDirectory(const char* sub_directory_name, const char* configu #elif defined __ANDROID__ // no defaults #elif defined __APPLE__ - const char* tmpdir_env = getenv("TMPDIR"); - if (tmpdir_env && utils::fs::isDirectory(tmpdir_env)) + const std::string tmpdir_env = utils::getConfigurationParameterString("TMPDIR"); + if (!tmpdir_env.empty() && utils::fs::isDirectory(tmpdir_env)) { default_cache_path = tmpdir_env; } @@ -461,16 +461,16 @@ cv::String getCacheDirectory(const char* sub_directory_name, const char* configu // https://specifications.freedesktop.org/basedir-spec/basedir-spec-latest.html if (default_cache_path.empty()) { - const char* xdg_cache_env = getenv("XDG_CACHE_HOME"); - if (xdg_cache_env && xdg_cache_env[0] && utils::fs::isDirectory(xdg_cache_env)) + const std::string xdg_cache_env = utils::getConfigurationParameterString("XDG_CACHE_HOME"); + if (!xdg_cache_env.empty() && utils::fs::isDirectory(xdg_cache_env)) { default_cache_path = xdg_cache_env; } } if (default_cache_path.empty()) { - const char* home_env = getenv("HOME"); - if (home_env && home_env[0] && utils::fs::isDirectory(home_env)) + const std::string home_env = utils::getConfigurationParameterString("HOME"); + if (!home_env.empty() && utils::fs::isDirectory(home_env)) { cv::String home_path = home_env; cv::String home_cache_path = utils::fs::join(home_path, ".cache/"); diff --git a/modules/core/src/va_wrapper.impl.hpp b/modules/core/src/va_wrapper.impl.hpp index 77faa984d0..23b1b69dcb 100644 --- a/modules/core/src/va_wrapper.impl.hpp +++ b/modules/core/src/va_wrapper.impl.hpp @@ -7,6 +7,7 @@ // #include "opencv2/core/utils/plugin_loader.private.hpp" // DynamicLib +#include "opencv2/core/utils/configuration.private.hpp" namespace cv { namespace detail { @@ -47,8 +48,8 @@ static FN_vaGetImage fn_vaGetImage = NULL; static std::shared_ptr loadLibVA() { std::shared_ptr lib; - const char* envPath = getenv("OPENCV_LIBVA_RUNTIME"); - if (envPath) + const std::string envPath = utils::getConfigurationParameterString("OPENCV_LIBVA_RUNTIME"); + if (!envPath.empty()) { lib = std::make_shared(envPath); return lib; diff --git a/modules/dnn/perf/perf_main.cpp b/modules/dnn/perf/perf_main.cpp index 073921efaf..bdb360fcb2 100644 --- a/modules/dnn/perf/perf_main.cpp +++ b/modules/dnn/perf/perf_main.cpp @@ -1,16 +1,7 @@ #include "perf_precomp.hpp" -static const char* extraTestDataPath = -#ifdef WINRT - NULL; -#else - getenv("OPENCV_DNN_TEST_DATA_PATH"); -#endif - #if defined(HAVE_HPX) #include #endif -CV_PERF_TEST_MAIN(dnn, - extraTestDataPath ? (void)cvtest::addDataSearchPath(extraTestDataPath) : (void)0 -) +CV_PERF_TEST_MAIN(dnn, cvtest::addDataSearchEnv("OPENCV_DNN_TEST_DATA_PATH")) diff --git a/modules/dnn/src/precomp.hpp b/modules/dnn/src/precomp.hpp index 178a8f42cf..cf17e0109d 100644 --- a/modules/dnn/src/precomp.hpp +++ b/modules/dnn/src/precomp.hpp @@ -47,6 +47,7 @@ #endif #include +#include "opencv2/core/utils/configuration.private.hpp" #ifndef CV_OCL4DNN #define CV_OCL4DNN 0 @@ -89,4 +90,4 @@ #include #include -#include "dnn_common.hpp" \ No newline at end of file +#include "dnn_common.hpp" diff --git a/modules/dnn/src/vkcom/vulkan/vk_loader.cpp b/modules/dnn/src/vkcom/vulkan/vk_loader.cpp index 4640d0a0cb..64cb7741b9 100644 --- a/modules/dnn/src/vkcom/vulkan/vk_loader.cpp +++ b/modules/dnn/src/vkcom/vulkan/vk_loader.cpp @@ -115,21 +115,12 @@ bool loadVulkanLibrary() if (handle != nullptr) return true; - const char* path; - const char* envPath = getenv("OPENCV_VULKAN_RUNTIME"); - if (envPath) - { - path = envPath; - } - else - { - path = DEFAULT_VK_LIBRARY_PATH; - } + const std::string path = cv::utils::getConfigurationParameterString("OPENCV_VULKAN_RUNTIME", DEFAULT_VK_LIBRARY_PATH); - handle = LOAD_VK_LIBRARY(path); + handle = LOAD_VK_LIBRARY(path.c_str()); if( handle == nullptr ) { - fprintf(stderr, "Could not load Vulkan library: %s!\n", path); + fprintf(stderr, "Could not load Vulkan library: %s!\n", path.c_str()); fprintf(stderr, "Please download the Vulkan SDK and set the environment variable of OPENCV_VULKAN_RUNTIME according " "to your system environment.\n"); fprintf(stderr, "For M1 Mac and IOS, we use MoltenVK to map the Vulkan code to native apple Metal code.\n"); diff --git a/modules/dnn/test/test_common.impl.hpp b/modules/dnn/test/test_common.impl.hpp index e78dc6c1e4..a12f2fe5b4 100644 --- a/modules/dnn/test/test_common.impl.hpp +++ b/modules/dnn/test/test_common.impl.hpp @@ -437,14 +437,7 @@ bool validateVPUType() void initDNNTests() { - const char* extraTestDataPath = -#ifdef WINRT - NULL; -#else - getenv("OPENCV_DNN_TEST_DATA_PATH"); -#endif - if (extraTestDataPath) - cvtest::addDataSearchPath(extraTestDataPath); + cvtest::addDataSearchEnv("OPENCV_DNN_TEST_DATA_PATH"); registerGlobalSkipTag( CV_TEST_TAG_DNN_SKIP_OPENCV_BACKEND, diff --git a/modules/dnn/test/test_ie_models.cpp b/modules/dnn/test/test_ie_models.cpp index eff389035d..647ee907bc 100644 --- a/modules/dnn/test/test_ie_models.cpp +++ b/modules/dnn/test/test_ie_models.cpp @@ -40,15 +40,11 @@ static void initDLDTDataPath() if (!initialized) { #if INF_ENGINE_RELEASE <= 2018050000 - const char* dldtTestDataPath = getenv("INTEL_CVSDK_DIR"); - if (dldtTestDataPath) - cvtest::addDataSearchPath(dldtTestDataPath); + cvtest::addDataSearchEnv("INTEL_CVSDK_DIR"); #else - const char* omzDataPath = getenv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); - if (omzDataPath) - cvtest::addDataSearchPath(omzDataPath); - const char* dnnDataPath = getenv("OPENCV_DNN_TEST_DATA_PATH"); - if (dnnDataPath) + cvtest::addDataSearchEnv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); + const std::string dnnDataPath = cv::utils::getConfigurationParameterString("OPENCV_DNN_TEST_DATA_PATH"); + if (!dnnDataPath.empty()) cvtest::addDataSearchPath(std::string(dnnDataPath) + "/omz_intel_models"); #endif initialized = true; diff --git a/modules/dnn/test/test_precomp.hpp b/modules/dnn/test/test_precomp.hpp index cc1ea639f7..931ae7698d 100644 --- a/modules/dnn/test/test_precomp.hpp +++ b/modules/dnn/test/test_precomp.hpp @@ -45,6 +45,7 @@ #include "opencv2/ts/ts_perf.hpp" #include "opencv2/core/utility.hpp" #include "opencv2/core/ocl.hpp" +#include "opencv2/core/utils/configuration.private.hpp" #include "opencv2/dnn.hpp" #include "test_common.hpp" diff --git a/modules/gapi/src/compiler/gcompiler.cpp b/modules/gapi/src/compiler/gcompiler.cpp index 568251f19e..666271d7ba 100644 --- a/modules/gapi/src/compiler/gcompiler.cpp +++ b/modules/gapi/src/compiler/gcompiler.cpp @@ -99,9 +99,9 @@ namespace auto dump_info = cv::gapi::getCompileArg(args); if (!dump_info.has_value()) { - const char* path = std::getenv("GRAPH_DUMP_PATH"); - return path - ? cv::util::make_optional(std::string(path)) + const std::string path = cv::utils::getConfigurationParameterString("GRAPH_DUMP_PATH"); + return !path.empty() + ? cv::util::make_optional(path) : cv::util::optional(); } else diff --git a/modules/gapi/src/precomp.hpp b/modules/gapi/src/precomp.hpp index f72d0a1ecc..6aeaa0e58b 100644 --- a/modules/gapi/src/precomp.hpp +++ b/modules/gapi/src/precomp.hpp @@ -10,6 +10,7 @@ #if !defined(GAPI_STANDALONE) # include +# include # include # include # include diff --git a/modules/gapi/test/infer/gapi_infer_ie_test.cpp b/modules/gapi/test/infer/gapi_infer_ie_test.cpp index a7c9637a4f..ff27489434 100644 --- a/modules/gapi/test/infer/gapi_infer_ie_test.cpp +++ b/modules/gapi/test/infer/gapi_infer_ie_test.cpp @@ -101,22 +101,18 @@ public: // FIXME: taken from DNN module static void initDLDTDataPath() { -#ifndef WINRT static bool initialized = false; if (!initialized) { - const char* omzDataPath = getenv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); - if (omzDataPath) - cvtest::addDataSearchPath(omzDataPath); - const char* dnnDataPath = getenv("OPENCV_DNN_TEST_DATA_PATH"); - if (dnnDataPath) { + cvtest::addDataSearchEnv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); + const std::string dnnDataPath = cv::utils::getConfigurationParameterString("OPENCV_DNN_TEST_DATA_PATH"); + if (!dnnDataPath.empty()) { // Add the dnnDataPath itself - G-API is using some images there directly cvtest::addDataSearchPath(dnnDataPath); cvtest::addDataSearchPath(dnnDataPath + std::string("/omz_intel_models")); } initialized = true; } -#endif // WINRT } #if INF_ENGINE_RELEASE >= 2020010000 diff --git a/modules/gapi/test/infer/gapi_infer_onnx_test.cpp b/modules/gapi/test/infer/gapi_infer_onnx_test.cpp index dff91c597e..6ca394aef2 100644 --- a/modules/gapi/test/infer/gapi_infer_onnx_test.cpp +++ b/modules/gapi/test/infer/gapi_infer_onnx_test.cpp @@ -58,10 +58,7 @@ public: }; struct ONNXInitPath { ONNXInitPath() { - const char* env_path = getenv("OPENCV_GAPI_ONNX_MODEL_PATH"); - if (env_path) { - cvtest::addDataSearchPath(env_path); - } + cvtest::addDataSearchEnv("OPENCV_GAPI_ONNX_MODEL_PATH"); } }; static ONNXInitPath g_init_path; diff --git a/modules/gapi/test/infer/gapi_infer_ov_tests.cpp b/modules/gapi/test/infer/gapi_infer_ov_tests.cpp index 49652db387..85df0b24da 100644 --- a/modules/gapi/test/infer/gapi_infer_ov_tests.cpp +++ b/modules/gapi/test/infer/gapi_infer_ov_tests.cpp @@ -25,11 +25,9 @@ void initDLDTDataPath() static bool initialized = false; if (!initialized) { - const char* omzDataPath = getenv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); - if (omzDataPath) - cvtest::addDataSearchPath(omzDataPath); - const char* dnnDataPath = getenv("OPENCV_DNN_TEST_DATA_PATH"); - if (dnnDataPath) { + cvtest::addDataSearchEnv("OPENCV_OPEN_MODEL_ZOO_DATA_PATH"); + const std::string dnnDataPath = cv::utils::getConfigurationParameterString("OPENCV_DNN_TEST_DATA_PATH"); + if (!dnnDataPath.empty()) { // Add the dnnDataPath itself - G-API is using some images there directly cvtest::addDataSearchPath(dnnDataPath); cvtest::addDataSearchPath(dnnDataPath + std::string("/omz_intel_models")); diff --git a/modules/gapi/test/test_precomp.hpp b/modules/gapi/test/test_precomp.hpp index e92b1d03bf..708964dfe6 100644 --- a/modules/gapi/test/test_precomp.hpp +++ b/modules/gapi/test/test_precomp.hpp @@ -16,6 +16,8 @@ #include +#include + #include #include #include diff --git a/modules/highgui/src/precomp.hpp b/modules/highgui/src/precomp.hpp index 2bbfd6c14a..1301a4456a 100644 --- a/modules/highgui/src/precomp.hpp +++ b/modules/highgui/src/precomp.hpp @@ -54,6 +54,7 @@ #include "opencv2/core/utility.hpp" #if defined(__OPENCV_BUILD) #include "opencv2/core/private.hpp" +#include "opencv2/core/utils/configuration.private.hpp" #endif #include "opencv2/imgproc.hpp" diff --git a/modules/highgui/src/registry.impl.hpp b/modules/highgui/src/registry.impl.hpp index 782738a820..9cdf6b17eb 100644 --- a/modules/highgui/src/registry.impl.hpp +++ b/modules/highgui/src/registry.impl.hpp @@ -133,7 +133,7 @@ protected: bool readPrioritySettings() { bool hasChanges = false; - cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_UI_PRIORITY_LIST", NULL); + cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_UI_PRIORITY_LIST"); if (prioritized_backends.empty()) return hasChanges; CV_LOG_INFO(NULL, "UI: Configured priority list (OPENCV_UI_PRIORITY_LIST): " << prioritized_backends); diff --git a/modules/highgui/src/window.cpp b/modules/highgui/src/window.cpp index e6972b300b..4fb2169da9 100644 --- a/modules/highgui/src/window.cpp +++ b/modules/highgui/src/window.cpp @@ -662,7 +662,7 @@ int cv::waitKey(int delay) static int use_legacy = -1; if (use_legacy < 0) { - use_legacy = getenv("OPENCV_LEGACY_WAITKEY") != NULL ? 1 : 0; + use_legacy = utils::getConfigurationParameterBool("OPENCV_LEGACY_WAITKEY"); } if (use_legacy > 0) return code; diff --git a/modules/highgui/src/window_wayland.cpp b/modules/highgui/src/window_wayland.cpp index 3bf87c06ec..c04873ba35 100644 --- a/modules/highgui/src/window_wayland.cpp +++ b/modules/highgui/src/window_wayland.cpp @@ -1378,7 +1378,7 @@ int cv_wl_buffer::create_tmpfile(std::string const &tmpname) { } int cv_wl_buffer::create_anonymous_file(off_t size) { - auto path = getenv("XDG_RUNTIME_DIR") + std::string("/opencv-shared-XXXXXX"); + auto path = cv::utils::getConfigurationParameterString("XDG_RUNTIME_DIR") + std::string("/opencv-shared-XXXXXX"); int fd = create_tmpfile(path); int ret = posix_fallocate(fd, 0, size); diff --git a/modules/objdetect/test/test_main.cpp b/modules/objdetect/test/test_main.cpp index 4031f0522b..a7e31f2dd8 100644 --- a/modules/objdetect/test/test_main.cpp +++ b/modules/objdetect/test/test_main.cpp @@ -11,14 +11,7 @@ static void initTests() { #ifdef HAVE_OPENCV_DNN - const char* extraTestDataPath = -#ifdef WINRT - NULL; -#else - getenv("OPENCV_DNN_TEST_DATA_PATH"); -#endif - if (extraTestDataPath) - cvtest::addDataSearchPath(extraTestDataPath); + cvtest::addDataSearchEnv("OPENCV_DNN_TEST_DATA_PATH"); #endif // HAVE_OPENCV_DNN } diff --git a/modules/ts/include/opencv2/ts.hpp b/modules/ts/include/opencv2/ts.hpp index 943da1d067..14b9b2084f 100644 --- a/modules/ts/include/opencv2/ts.hpp +++ b/modules/ts/include/opencv2/ts.hpp @@ -754,6 +754,7 @@ void smoothBorder(Mat& img, const Scalar& color, int delta = 3); // Utility functions void addDataSearchPath(const std::string& path); +void addDataSearchEnv(const std::string& env_name); void addDataSearchSubDirectory(const std::string& subdir); /*! @brief Try to find requested data file diff --git a/modules/ts/src/precomp.hpp b/modules/ts/src/precomp.hpp index 2725f63429..8508765079 100644 --- a/modules/ts/src/precomp.hpp +++ b/modules/ts/src/precomp.hpp @@ -1,5 +1,6 @@ #include "opencv2/ts.hpp" #include +#include "opencv2/core/utils/configuration.private.hpp" #include "opencv2/core/utility.hpp" #if !defined(__EMSCRIPTEN__) #include "opencv2/core/private.hpp" diff --git a/modules/ts/src/ts.cpp b/modules/ts/src/ts.cpp index 163b573b3e..2cc3cc9a4a 100644 --- a/modules/ts/src/ts.cpp +++ b/modules/ts/src/ts.cpp @@ -553,13 +553,9 @@ static int tsErrorCallback( int status, const char* func_name, const char* err_m void TS::init( const string& modulename ) { data_search_subdir.push_back(modulename); -#ifndef WINRT - char* datapath_dir = getenv("OPENCV_TEST_DATA_PATH"); -#else - char* datapath_dir = OPENCV_TEST_DATA_PATH; -#endif + std::string datapath_dir = cv::utils::getConfigurationParameterString("OPENCV_TEST_DATA_PATH"); - if( datapath_dir ) + if( !datapath_dir.empty() ) { data_path = path_join(path_join(datapath_dir, modulename), ""); } @@ -903,11 +899,7 @@ void parseCustomOptions(int argc, char **argv) test_ipp_check = parser.get("test_ipp_check"); if (!test_ipp_check) -#ifndef WINRT - test_ipp_check = getenv("OPENCV_IPP_CHECK") != NULL; -#else - test_ipp_check = false; -#endif + test_ipp_check = cv::utils::getConfigurationParameterBool("OPENCV_IPP_CHECK"); param_seed = parser.get("test_seed"); @@ -953,9 +945,14 @@ static bool isDirectory(const std::string& path) void addDataSearchPath(const std::string& path) { - if (isDirectory(path)) + if (!path.empty() && isDirectory(path)) TS::ptr()->data_search_path.push_back(path); } +void addDataSearchEnv(const std::string& env_name) +{ + const std::string val = cv::utils::getConfigurationParameterString(env_name.c_str()); + cvtest::addDataSearchPath(val); +} void addDataSearchSubDirectory(const std::string& subdir) { TS::ptr()->data_search_subdir.push_back(subdir); @@ -1001,14 +998,10 @@ static std::string findData(const std::string& relative_path, bool required, boo const std::vector& search_subdir = TS::ptr()->data_search_subdir; -#ifndef WINRT - char* datapath_dir = getenv("OPENCV_TEST_DATA_PATH"); -#else - char* datapath_dir = OPENCV_TEST_DATA_PATH; -#endif + std::string datapath_dir = cv::utils::getConfigurationParameterString("OPENCV_TEST_DATA_PATH"); std::string datapath; - if (datapath_dir) + if (!datapath_dir.empty()) { datapath = datapath_dir; //CV_Assert(isDirectory(datapath) && "OPENCV_TEST_DATA_PATH is specified but it doesn't exist"); diff --git a/modules/ts/src/ts_perf.cpp b/modules/ts/src/ts_perf.cpp index 39147228b8..db57a2f0a4 100644 --- a/modules/ts/src/ts_perf.cpp +++ b/modules/ts/src/ts_perf.cpp @@ -192,22 +192,18 @@ void Regression::init(const std::string& testSuitName, const std::string& ext) return; } -#ifndef WINRT - const char *data_path_dir = getenv("OPENCV_TEST_DATA_PATH"); -#else - const char *data_path_dir = OPENCV_TEST_DATA_PATH; -#endif + const std::string data_path_dir = utils::getConfigurationParameterString("OPENCV_TEST_DATA_PATH"); cvtest::addDataSearchSubDirectory(""); cvtest::addDataSearchSubDirectory(testSuitName); const char *path_separator = "/"; - if (data_path_dir) + if (!data_path_dir.empty()) { - int len = (int)strlen(data_path_dir)-1; + int len = (int)data_path_dir.size()-1; if (len < 0) len = 0; - std::string path_base = (data_path_dir[0] == 0 ? std::string(".") : std::string(data_path_dir)) + std::string path_base = (data_path_dir[0] == 0 ? std::string(".") : data_path_dir) + (data_path_dir[len] == '/' || data_path_dir[len] == '\\' ? "" : path_separator) + "perf" + path_separator; @@ -1042,7 +1038,7 @@ void TestBase::Init(const std::vector & availableImpls, param_verify_sanity = args.get("perf_verify_sanity"); #ifdef HAVE_IPP - test_ipp_check = !args.get("perf_ipp_check") ? getenv("OPENCV_IPP_CHECK") != NULL : true; + test_ipp_check = !args.get("perf_ipp_check") ? utils::getConfigurationParameterBool("OPENCV_IPP_CHECK") : true; #endif testThreads = args.get("perf_threads"); #ifdef CV_COLLECT_IMPL_DATA @@ -1125,12 +1121,11 @@ void TestBase::Init(const std::vector & availableImpls, #endif { -#ifndef WINRT - const char* path = getenv("OPENCV_PERF_VALIDATION_DIR"); -#else - const char* path = OPENCV_PERF_VALIDATION_DIR; + std::string path = utils::getConfigurationParameterString("OPENCV_PERF_VALIDATION_DIR"); +#ifdef WINRT + path = OPENCV_PERF_VALIDATION_DIR; #endif - if (path) + if (!path.empty()) perf_validation_results_directory = path; } @@ -1888,17 +1883,16 @@ std::string TestBase::getDataPath(const std::string& relativePath) throw PerfEarlyExitException(); } -#ifndef WINRT - const char *data_path_dir = getenv("OPENCV_TEST_DATA_PATH"); -#else - const char *data_path_dir = OPENCV_TEST_DATA_PATH; + std::string data_path_dir = utils::getConfigurationParameterString("OPENCV_TEST_DATA_PATH"); +#ifdef WINRT + data_path_dir = OPENCV_TEST_DATA_PATH; #endif const char *path_separator = "/"; std::string path; - if (data_path_dir) + if (!data_path_dir.empty()) { - int len = (int)strlen(data_path_dir) - 1; + int len = (int)data_path_dir.size() - 1; if (len < 0) len = 0; path = (data_path_dir[0] == 0 ? std::string(".") : std::string(data_path_dir)) + (data_path_dir[len] == '/' || data_path_dir[len] == '\\' ? "" : path_separator); diff --git a/modules/video/perf/perf_main.cpp b/modules/video/perf/perf_main.cpp index 1879aeff90..2bf9123d96 100644 --- a/modules/video/perf/perf_main.cpp +++ b/modules/video/perf/perf_main.cpp @@ -7,15 +7,7 @@ static void initTests() { - const char* extraTestDataPath = -#ifdef WINRT - NULL; -#else - getenv("OPENCV_DNN_TEST_DATA_PATH"); -#endif - if (extraTestDataPath) - cvtest::addDataSearchPath(extraTestDataPath); - + cvtest::addDataSearchEnv("OPENCV_DNN_TEST_DATA_PATH"); cvtest::addDataSearchSubDirectory(""); // override "cv" prefix below to access without "../dnn" hacks } diff --git a/modules/video/perf/perf_precomp.hpp b/modules/video/perf/perf_precomp.hpp index ccc1865ff7..529a1436f9 100644 --- a/modules/video/perf/perf_precomp.hpp +++ b/modules/video/perf/perf_precomp.hpp @@ -7,6 +7,7 @@ #include "opencv2/ts.hpp" #include #include "opencv2/ts/ts_perf.hpp" +#include "opencv2/core/utils/configuration.private.hpp" namespace cvtest { diff --git a/modules/video/test/test_main.cpp b/modules/video/test/test_main.cpp index 9968380a17..68c90f775e 100644 --- a/modules/video/test/test_main.cpp +++ b/modules/video/test/test_main.cpp @@ -10,15 +10,7 @@ static void initTests() { - const char* extraTestDataPath = -#ifdef WINRT - NULL; -#else - getenv("OPENCV_DNN_TEST_DATA_PATH"); -#endif - if (extraTestDataPath) - cvtest::addDataSearchPath(extraTestDataPath); - + cvtest::addDataSearchEnv("OPENCV_DNN_TEST_DATA_PATH"); cvtest::addDataSearchSubDirectory(""); // override "cv" prefix below to access without "../dnn" hacks } diff --git a/modules/video/test/test_precomp.hpp b/modules/video/test/test_precomp.hpp index ba1f10998f..a6810ccc1f 100644 --- a/modules/video/test/test_precomp.hpp +++ b/modules/video/test/test_precomp.hpp @@ -7,6 +7,7 @@ #include "opencv2/ts.hpp" #include "opencv2/video.hpp" #include +#include "opencv2/core/utils/configuration.private.hpp" namespace opencv_test { using namespace perf; diff --git a/modules/videoio/src/backend_plugin.cpp b/modules/videoio/src/backend_plugin.cpp index 039083c6ad..cc9b33b80c 100644 --- a/modules/videoio/src/backend_plugin.cpp +++ b/modules/videoio/src/backend_plugin.cpp @@ -332,16 +332,14 @@ std::vector getPluginCandidates(const std::string& baseName) std::vector results; #ifdef _WIN32 FileSystemPath_t moduleName = toFileSystemPath(libraryPrefix() + "opencv_videoio_" + baseName_l + librarySuffix()); -#ifndef WINRT if (baseName_u == "FFMPEG") // backward compatibility { - const wchar_t* ffmpeg_env_path = _wgetenv(L"OPENCV_FFMPEG_DLL_DIR"); - if (ffmpeg_env_path) + const std::string ffmpeg_env_path = cv::utils::getConfigurationParameterString("OPENCV_FFMPEG_DLL_DIR"); + if (!ffmpeg_env_path.empty()) { - results.push_back(FileSystemPath_t(ffmpeg_env_path) + L"\\" + moduleName); + results.push_back(toFileSystemPath(ffmpeg_env_path + "\\" + toPrintablePath(moduleName))); } } -#endif if (plugin_expr != default_expr) { moduleName = toFileSystemPath(plugin_expr); diff --git a/modules/videoio/src/cap_dshow.cpp b/modules/videoio/src/cap_dshow.cpp index 7cc0469200..d46bb08a0d 100644 --- a/modules/videoio/src/cap_dshow.cpp +++ b/modules/videoio/src/cap_dshow.cpp @@ -303,15 +303,7 @@ interface ISampleGrabber : public IUnknown static void DebugPrintOut(const char *format, ...) { - static int gs_verbose = -1; - if (gs_verbose < 0) - { - // Fetch initial debug state from environment - defaults to disabled - const char* s = getenv("OPENCV_DSHOW_DEBUG"); - gs_verbose = s != NULL && atoi(s) != 0; - } - - + static const bool gs_verbose = utils::getConfigurationParameterBool("OPENCV_DSHOW_DEBUG"); if (gs_verbose) { va_list args; @@ -2982,18 +2974,18 @@ int videoInput::start(int deviceID, videoDevice *VD){ VD->readyToCapture = true; // check for optional saving the direct show graph to a file - const char* graph_filename = getenv("OPENCV_DSHOW_SAVEGRAPH_FILENAME"); - if (graph_filename) { - size_t filename_len = strlen(graph_filename); + std::string graph_filename = utils::getConfigurationParameterString("OPENCV_DSHOW_SAVEGRAPH_FILENAME"); + if (!graph_filename.empty()) { + size_t filename_len = graph_filename.size(); std::vector wfilename(filename_len + 1); - size_t len = mbstowcs(&wfilename[0], graph_filename, filename_len + 1); + size_t len = mbstowcs(&wfilename[0], &graph_filename[0], filename_len + 1); CV_Assert(len == filename_len); HRESULT res = SaveGraphFile(VD->pGraph, &wfilename[0]); if (SUCCEEDED(res)) { - DebugPrintOut("Saved DSHOW graph to %s\n", graph_filename); + DebugPrintOut("Saved DSHOW graph to %s\n", graph_filename.c_str()); } else { - DebugPrintOut("Failed to save DSHOW graph to %s\n", graph_filename); + DebugPrintOut("Failed to save DSHOW graph to %s\n", graph_filename.c_str()); } } diff --git a/modules/videoio/src/cap_ffmpeg_impl.hpp b/modules/videoio/src/cap_ffmpeg_impl.hpp index 17e9f4903f..f13cb8462b 100644 --- a/modules/videoio/src/cap_ffmpeg_impl.hpp +++ b/modules/videoio/src/cap_ffmpeg_impl.hpp @@ -928,21 +928,19 @@ public: } static void initLogger_() { -#ifndef NO_GETENV - char* debug_option = getenv("OPENCV_FFMPEG_DEBUG"); - char* level_option = getenv("OPENCV_FFMPEG_LOGLEVEL"); + const bool debug_option = utils::getConfigurationParameterBool("OPENCV_FFMPEG_DEBUG"); + std::string level_option = utils::getConfigurationParameterString("OPENCV_FFMPEG_LOGLEVEL"); int level = AV_LOG_VERBOSE; - if (level_option != NULL) + if (!level_option.empty()) { - level = atoi(level_option); + level = atoi(level_option.c_str()); } - if ( (debug_option != NULL) || (level_option != NULL) ) + if ( debug_option || (!level_option.empty()) ) { av_log_set_level(level); av_log_set_callback(ffmpeg_log_callback); } else -#endif { av_log_set_level(AV_LOG_ERROR); } @@ -979,10 +977,10 @@ inline void fill_codec_context(AVCodecContext * enc, AVDictionary * dict) { int nCpus = cv::getNumberOfCPUs(); int requestedThreads = std::min(nCpus, 16); // [OPENCV:FFMPEG:24] Application has requested XX threads. Using a thread count greater than 16 is not recommended. - char* threads_option = getenv("OPENCV_FFMPEG_THREADS"); - if (threads_option != NULL) + std::string threads_option = utils::getConfigurationParameterString("OPENCV_FFMPEG_THREADS"); + if (!threads_option.empty()) { - requestedThreads = atoi(threads_option); + requestedThreads = atoi(threads_option.c_str()); } enc->thread_count = requestedThreads; } @@ -1122,9 +1120,8 @@ bool CvCapture_FFMPEG::open(const char* _filename, const VideoCaptureParameters& ic->interrupt_callback.opaque = &interrupt_metadata; #endif -#ifndef NO_GETENV - char* options = getenv("OPENCV_FFMPEG_CAPTURE_OPTIONS"); - if(options == NULL) + std::string options = utils::getConfigurationParameterString("OPENCV_FFMPEG_CAPTURE_OPTIONS"); + if(!options.empty()) { #if LIBAVFORMAT_VERSION_MICRO >= 100 && LIBAVFORMAT_BUILD >= CALC_FFMPEG_VERSION(55, 48, 100) av_dict_set(&dict, "rtsp_flags", "prefer_tcp", 0); @@ -1136,14 +1133,11 @@ bool CvCapture_FFMPEG::open(const char* _filename, const VideoCaptureParameters& { CV_LOG_DEBUG(NULL, "VIDEOIO/FFMPEG: using capture options from environment: " << options); #if LIBAVUTIL_BUILD >= (LIBAVUTIL_VERSION_MICRO >= 100 ? CALC_FFMPEG_VERSION(52, 17, 100) : CALC_FFMPEG_VERSION(52, 7, 0)) - av_dict_parse_string(&dict, options, ";", "|", 0); + av_dict_parse_string(&dict, options.c_str(), ";", "|", 0); #else av_dict_set(&dict, "rtsp_transport", "tcp", 0); #endif } -#else - av_dict_set(&dict, "rtsp_transport", "tcp", 0); -#endif CV_FFMPEG_FMT_CONST AVInputFormat* input_format = NULL; AVDictionaryEntry* entry = av_dict_get(dict, "input_format", NULL, 0); if (entry != 0) @@ -3095,12 +3089,12 @@ bool CvVideoWriter_FFMPEG::open( const char * filename, int fourcc, } AVDictionary *dict = NULL; -#if !defined(NO_GETENV) && (LIBAVUTIL_VERSION_MAJOR >= 53) - char* options = getenv("OPENCV_FFMPEG_WRITER_OPTIONS"); - if (options) +#if (LIBAVUTIL_VERSION_MAJOR >= 53) + std::string options = utils::getConfigurationParameterString("OPENCV_FFMPEG_WRITER_OPTIONS"); + if (!options.empty()) { CV_LOG_DEBUG(NULL, "VIDEOIO/FFMPEG: using writer options from environment: " << options); - av_dict_parse_string(&dict, options, ";", "|", 0); + av_dict_parse_string(&dict, options.c_str(), ";", "|", 0); } #endif diff --git a/modules/videoio/src/videoio_registry.cpp b/modules/videoio/src/videoio_registry.cpp index b333797fe0..a3aee5abfe 100644 --- a/modules/videoio/src/videoio_registry.cpp +++ b/modules/videoio/src/videoio_registry.cpp @@ -259,7 +259,7 @@ protected: bool readPrioritySettings() { bool hasChanges = false; - cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_VIDEOIO_PRIORITY_LIST", NULL); + cv::String prioritized_backends = utils::getConfigurationParameterString("OPENCV_VIDEOIO_PRIORITY_LIST"); if (prioritized_backends.empty()) return hasChanges; CV_LOG_INFO(NULL, "VIDEOIO: Configured priority list (OPENCV_VIDEOIO_PRIORITY_LIST): " << prioritized_backends); diff --git a/modules/videoio/test/test_main.cpp b/modules/videoio/test/test_main.cpp index d57a611c4b..db82968d76 100644 --- a/modules/videoio/test/test_main.cpp +++ b/modules/videoio/test/test_main.cpp @@ -11,15 +11,13 @@ static void initTests() { -#ifndef WINRT // missing getenv const std::vector backends = cv::videoio_registry::getStreamBackends(); - const char* requireFFmpeg = getenv("OPENCV_TEST_VIDEOIO_BACKEND_REQUIRE_FFMPEG"); + bool requireFFmpeg = cv::utils::getConfigurationParameterBool("OPENCV_TEST_VIDEOIO_BACKEND_REQUIRE_FFMPEG"); if (requireFFmpeg && !isBackendAvailable(cv::CAP_FFMPEG, backends)) { CV_LOG_FATAL(NULL, "OpenCV-Test: required FFmpeg backend is not available (broken plugin?). STOP."); exit(1); } -#endif } CV_TEST_MAIN("highgui", initTests()) diff --git a/modules/videoio/test/test_precomp.hpp b/modules/videoio/test/test_precomp.hpp index 835177729b..b17a2f0ab7 100644 --- a/modules/videoio/test/test_precomp.hpp +++ b/modules/videoio/test/test_precomp.hpp @@ -13,6 +13,7 @@ #include "opencv2/videoio.hpp" #include "opencv2/videoio/registry.hpp" #include "opencv2/core/private.hpp" +#include "opencv2/core/utils/configuration.private.hpp" namespace cv { From 6873bdee70528ba6101bd5f4091b5f5302ee70c9 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 8 Nov 2024 09:56:49 +0100 Subject: [PATCH 03/10] backport C++ 3d/calibration_base.cpp:5.x to calib3d/calibration_base.cpp:4.x (#26414) * Add vanilla calibration_base from 5.x This is from 55105719dd698bf3eeb5b58f6dbb3baaf0eeb5b9 * Have the C implementation use the new C++ one. --- modules/calib3d/src/calib3d_c_api.h | 65 - modules/calib3d/src/calibration.cpp | 1922 +--------------------- modules/calib3d/src/calibration_base.cpp | 1644 ++++++++++++++++++ modules/calib3d/src/checkchessboard.cpp | 1 - modules/calib3d/src/precomp.hpp | 13 + modules/calib3d/src/solvepnp.cpp | 8 +- 6 files changed, 1697 insertions(+), 1956 deletions(-) create mode 100644 modules/calib3d/src/calibration_base.cpp diff --git a/modules/calib3d/src/calib3d_c_api.h b/modules/calib3d/src/calib3d_c_api.h index cdbf021667..46cdbc824a 100644 --- a/modules/calib3d/src/calib3d_c_api.h +++ b/modules/calib3d/src/calib3d_c_api.h @@ -111,23 +111,6 @@ void cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, void cvCorrectMatches(CvMat* F, CvMat* points1, CvMat* points2, CvMat* new_points1, CvMat* new_points2); - -/* Computes the optimal new camera matrix according to the free scaling parameter alpha: - alpha=0 - only valid pixels will be retained in the undistorted image - alpha=1 - all the source image pixels will be retained in the undistorted image -*/ -void cvGetOptimalNewCameraMatrix( const CvMat* camera_matrix, - const CvMat* dist_coeffs, - CvSize image_size, double alpha, - CvMat* new_camera_matrix, - CvSize new_imag_size CV_DEFAULT(cvSize(0,0)), - CvRect* valid_pixel_ROI CV_DEFAULT(0), - int center_principal_point CV_DEFAULT(0)); - -/* Converts rotation vector to rotation matrix or vice versa */ -int cvRodrigues2( const CvMat* src, CvMat* dst, - CvMat* jacobian CV_DEFAULT(0) ); - /* Finds perspective transformation between the object plane and image (view) plane */ int cvFindHomography( const CvMat* src_points, const CvMat* dst_points, @@ -138,54 +121,6 @@ int cvFindHomography( const CvMat* src_points, int maxIters CV_DEFAULT(2000), double confidence CV_DEFAULT(0.995)); -/* Computes RQ decomposition for 3x3 matrices */ -void cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, - CvMat *matrixQx CV_DEFAULT(NULL), - CvMat *matrixQy CV_DEFAULT(NULL), - CvMat *matrixQz CV_DEFAULT(NULL), - CvPoint3D64f *eulerAngles CV_DEFAULT(NULL)); - -/* Computes projection matrix decomposition */ -void cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, - CvMat *rotMatr, CvMat *posVect, - CvMat *rotMatrX CV_DEFAULT(NULL), - CvMat *rotMatrY CV_DEFAULT(NULL), - CvMat *rotMatrZ CV_DEFAULT(NULL), - CvPoint3D64f *eulerAngles CV_DEFAULT(NULL)); - -/* Computes d(AB)/dA and d(AB)/dB */ -void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ); - -/* Computes r3 = rodrigues(rodrigues(r2)*rodrigues(r1)), - t3 = rodrigues(r2)*t1 + t2 and the respective derivatives */ -void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, - const CvMat* _rvec2, const CvMat* _tvec2, - CvMat* _rvec3, CvMat* _tvec3, - CvMat* dr3dr1 CV_DEFAULT(0), CvMat* dr3dt1 CV_DEFAULT(0), - CvMat* dr3dr2 CV_DEFAULT(0), CvMat* dr3dt2 CV_DEFAULT(0), - CvMat* dt3dr1 CV_DEFAULT(0), CvMat* dt3dt1 CV_DEFAULT(0), - CvMat* dt3dr2 CV_DEFAULT(0), CvMat* dt3dt2 CV_DEFAULT(0) ); - -/* Projects object points to the view plane using - the specified extrinsic and intrinsic camera parameters */ -void cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector, - const CvMat* translation_vector, const CvMat* camera_matrix, - const CvMat* distortion_coeffs, CvMat* image_points, - CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL), - CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL), - CvMat* dpddist CV_DEFAULT(NULL), - double aspect_ratio CV_DEFAULT(0)); - -/* Finds extrinsic camera parameters from - a few known corresponding point pairs and intrinsic parameters */ -void cvFindExtrinsicCameraParams2( const CvMat* object_points, - const CvMat* image_points, - const CvMat* camera_matrix, - const CvMat* distortion_coeffs, - CvMat* rotation_vector, - CvMat* translation_vector, - int use_extrinsic_guess CV_DEFAULT(0) ); - /* Computes initial estimate of the intrinsic camera parameters in case of planar calibration target (e.g. chessboard) */ void cvInitIntrinsicParams2D( const CvMat* object_points, diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 9729cb7bea..c791d64f00 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -42,7 +42,6 @@ #include "precomp.hpp" #include "hal_replacement.hpp" -#include "opencv2/imgproc/imgproc_c.h" #include "distortion_model.hpp" #include "calib3d_c_api.h" #include @@ -58,1317 +57,8 @@ using namespace cv; -// reimplementation of dAB.m -CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ) -{ - int i, j, M, N, L; - int bstep; - - CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) ); - CV_Assert( CV_ARE_TYPES_EQ(A, B) && - (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) ); - CV_Assert( A->cols == B->rows ); - - M = A->rows; - L = A->cols; - N = B->cols; - bstep = B->step/CV_ELEM_SIZE(B->type); - - if( dABdA ) - { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) && - dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols ); - } - - if( dABdB ) - { - CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) && - dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols ); - } - - if( CV_MAT_TYPE(A->type) == CV_32F ) - { - for( i = 0; i < M*N; i++ ) - { - int i1 = i / N, i2 = i % N; - - if( dABdA ) - { - float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i); - const float* b = (const float*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i); - const float* a = (const float*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; - } - } - } - else - { - for( i = 0; i < M*N; i++ ) - { - int i1 = i / N, i2 = i % N; - - if( dABdA ) - { - double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i); - const double* b = (const double*)B->data.ptr + i2; - - for( j = 0; j < M*L; j++ ) - dcda[j] = 0; - for( j = 0; j < L; j++ ) - dcda[i1*L + j] = b[j*bstep]; - } - - if( dABdB ) - { - double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i); - const double* a = (const double*)(A->data.ptr + A->step*i1); - - for( j = 0; j < L*N; j++ ) - dcdb[j] = 0; - for( j = 0; j < L; j++ ) - dcdb[j*N + i2] = a[j]; - } - } - } -} - -// reimplementation of compose_motion.m -CV_IMPL void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, - const CvMat* _rvec2, const CvMat* _tvec2, - CvMat* _rvec3, CvMat* _tvec3, - CvMat* dr3dr1, CvMat* dr3dt1, - CvMat* dr3dr2, CvMat* dr3dt2, - CvMat* dt3dr1, CvMat* dt3dt1, - CvMat* dt3dr2, CvMat* dt3dt2 ) -{ - double _r1[3], _r2[3]; - double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; - CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2); - CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2); - CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2); - - CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) ); - - CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F || - CV_MAT_TYPE(_rvec1->type) == CV_64F ); - - CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) ); - - cvConvert( _rvec1, &r1 ); - cvConvert( _rvec2, &r2 ); - - cvRodrigues2( &r1, &R1, &dR1dr1 ); - cvRodrigues2( &r2, &R2, &dR2dr2 ); - - if( _rvec3 || dr3dr1 || dr3dr2 ) - { - double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; - double _W1[9*3], _W2[3*3]; - CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3); - CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2); - CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3); - CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2); - - cvMatMul( &R2, &R1, &R3 ); - cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 ); - - cvRodrigues2( &R3, &r3, &dr3dR3 ); - - if( _rvec3 ) - cvConvert( &r3, _rvec3 ); - - if( dr3dr1 ) - { - cvMatMul( &dr3dR3, &dR3dR1, &W1 ); - cvMatMul( &W1, &dR1dr1, &W2 ); - cvConvert( &W2, dr3dr1 ); - } - - if( dr3dr2 ) - { - cvMatMul( &dr3dR3, &dR3dR2, &W1 ); - cvMatMul( &W1, &dR2dr2, &W2 ); - cvConvert( &W2, dr3dr2 ); - } - } - - if( dr3dt1 ) - cvZero( dr3dt1 ); - if( dr3dt2 ) - cvZero( dr3dt2 ); - - if( _tvec3 || dt3dr2 || dt3dt1 ) - { - double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; - CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2); - CvMat t3 = cvMat(3,1,CV_64F,_t3); - CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2); - CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1); - CvMat W3 = cvMat(3, 3, CV_64F, _W3); - - CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) ); - CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) ); - - cvConvert( _tvec1, &t1 ); - cvConvert( _tvec2, &t2 ); - cvMatMulAdd( &R2, &t1, &t2, &t3 ); - - if( _tvec3 ) - cvConvert( &t3, _tvec3 ); - - if( dt3dr2 || dt3dt1 ) - { - cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 ); - if( dt3dr2 ) - { - cvMatMul( &dxdR2, &dR2dr2, &W3 ); - cvConvert( &W3, dt3dr2 ); - } - if( dt3dt1 ) - cvConvert( &dxdt1, dt3dt1 ); - } - } - - if( dt3dt2 ) - cvSetIdentity( dt3dt2 ); - if( dt3dr1 ) - cvZero( dt3dr1 ); -} - -CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) -{ - double J[27] = {0}; - CvMat matJ = cvMat( 3, 9, CV_64F, J ); - - if( !CV_IS_MAT(src) ) - CV_Error( !src ? cv::Error::StsNullPtr : cv::Error::StsBadArg, "Input argument is not a valid matrix" ); - - if( !CV_IS_MAT(dst) ) - CV_Error( !dst ? cv::Error::StsNullPtr : cv::Error::StsBadArg, - "The first output argument is not a valid matrix" ); - - int depth = CV_MAT_DEPTH(src->type); - int elem_size = CV_ELEM_SIZE(depth); - - if( depth != CV_32F && depth != CV_64F ) - CV_Error( cv::Error::StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); - - if( !CV_ARE_DEPTHS_EQ(src, dst) ) - CV_Error( cv::Error::StsUnmatchedFormats, "All the matrices must have the same data type" ); - - if( jacobian ) - { - if( !CV_IS_MAT(jacobian) ) - CV_Error( cv::Error::StsBadArg, "Jacobian is not a valid matrix" ); - - if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 ) - CV_Error( cv::Error::StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" ); - - if( (jacobian->rows != 9 || jacobian->cols != 3) && - (jacobian->rows != 3 || jacobian->cols != 9)) - CV_Error( cv::Error::StsBadSize, "Jacobian must be 3x9 or 9x3" ); - } - - if( src->cols == 1 || src->rows == 1 ) - { - int step = src->rows > 1 ? src->step / elem_size : 1; - - if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) - CV_Error( cv::Error::StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" ); - - if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) - CV_Error( cv::Error::StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); - - Point3d r; - if( depth == CV_32F ) - { - r.x = src->data.fl[0]; - r.y = src->data.fl[step]; - r.z = src->data.fl[step*2]; - } - else - { - r.x = src->data.db[0]; - r.y = src->data.db[step]; - r.z = src->data.db[step*2]; - } - - double theta = norm(r); - - if( theta < DBL_EPSILON ) - { - cvSetIdentity( dst ); - - if( jacobian ) - { - memset( J, 0, sizeof(J) ); - J[5] = J[15] = J[19] = -1; - J[7] = J[11] = J[21] = 1; - } - } - else - { - double c = cos(theta); - double s = sin(theta); - double c1 = 1. - c; - double itheta = theta ? 1./theta : 0.; - - r *= itheta; - - Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z ); - Matx33d r_x( 0, -r.z, r.y, - r.z, 0, -r.x, - -r.y, r.x, 0 ); - - // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; - - Mat(R).convertTo(cvarrToMat(dst), dst->type); - - if( jacobian ) - { - const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0, - 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0, - 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z }; - double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, - 0, 0, 1, 0, 0, 0, -1, 0, 0, - 0, -1, 0, 1, 0, 0, 0, 0, 0 }; - for( int i = 0; i < 3; i++ ) - { - double ri = i == 0 ? r.x : i == 1 ? r.y : r.z; - double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; - double a3 = (c - s*itheta)*ri, a4 = s*itheta; - for( int k = 0; k < 9; k++ ) - J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] + - a3*r_x.val[k] + a4*d_r_x_[i*9+k]; - } - } - } - } - else if( src->cols == 3 && src->rows == 3 ) - { - Matx33d U, Vt; - Vec3d W; - double theta, s, c; - int step = dst->rows > 1 ? dst->step / elem_size : 1; - - if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) && - (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) - CV_Error( cv::Error::StsBadSize, "Output matrix must be 1x3 or 3x1" ); - - Matx33d R = cvarrToMat(src); - - if( !checkRange(R, true, NULL, -100, 100) ) - { - cvZero(dst); - if( jacobian ) - cvZero(jacobian); - return 0; - } - - SVD::compute(R, W, U, Vt); - R = U*Vt; - - Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); - - s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25); - c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5; - c = c > 1. ? 1. : c < -1. ? -1. : c; - theta = acos(c); - - if( s < 1e-5 ) - { - double t; - - if( c > 0 ) - r = Point3d(0, 0, 0); - else - { - t = (R(0, 0) + 1)*0.5; - r.x = std::sqrt(MAX(t,0.)); - t = (R(1, 1) + 1)*0.5; - r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.); - t = (R(2, 2) + 1)*0.5; - r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.); - if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) ) - r.z = -r.z; - theta /= norm(r); - r *= theta; - } - - if( jacobian ) - { - memset( J, 0, sizeof(J) ); - if( c > 0 ) - { - J[5] = J[15] = J[19] = -0.5; - J[7] = J[11] = J[21] = 0.5; - } - } - } - else - { - double vth = 1/(2*s); - - if( jacobian ) - { - double t, dtheta_dtr = -1./s; - // var1 = [vth;theta] - // var = [om1;var1] = [om1;vth;theta] - double dvth_dtheta = -vth*c/s; - double d1 = 0.5*dvth_dtheta*dtheta_dtr; - double d2 = 0.5*dtheta_dtr; - // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR - double dvardR[5*9] = - { - 0, 0, 0, 0, 0, 1, 0, -1, 0, - 0, 0, -1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, -1, 0, 0, 0, 0, 0, - d1, 0, 0, 0, d1, 0, 0, 0, d1, - d2, 0, 0, 0, d2, 0, 0, 0, d2 - }; - // var2 = [om;theta] - double dvar2dvar[] = - { - vth, 0, 0, r.x, 0, - 0, vth, 0, r.y, 0, - 0, 0, vth, r.z, 0, - 0, 0, 0, 0, 1 - }; - double domegadvar2[] = - { - theta, 0, 0, r.x*vth, - 0, theta, 0, r.y*vth, - 0, 0, theta, r.z*vth - }; - - CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); - CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); - CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); - double t0[3*5]; - CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); - - cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); - cvMatMul( &_t0, &_dvardR, &matJ ); - - // transpose every row of matJ (treat the rows as 3x3 matrices) - CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); - CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t); - CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t); - } - - vth *= theta; - r *= vth; - } - - if( depth == CV_32F ) - { - dst->data.fl[0] = (float)r.x; - dst->data.fl[step] = (float)r.y; - dst->data.fl[step*2] = (float)r.z; - } - else - { - dst->data.db[0] = r.x; - dst->data.db[step] = r.y; - dst->data.db[step*2] = r.z; - } - } - else - { - CV_Error(cv::Error::StsBadSize, "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); - } - - if( jacobian ) - { - if( depth == CV_32F ) - { - if( jacobian->rows == matJ.rows ) - cvConvert( &matJ, jacobian ); - else - { - float Jf[3*9]; - CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf ); - cvConvert( &matJ, &_Jf ); - cvTranspose( &_Jf, jacobian ); - } - } - else if( jacobian->rows == matJ.rows ) - cvCopy( &matJ, jacobian ); - else - cvTranspose( &matJ, jacobian ); - } - - return 1; -} - static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; -static void cvProjectPoints2Internal( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL), - CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL), - CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL), - CvMat* dpdo CV_DEFAULT(NULL), - double aspectRatio CV_DEFAULT(0) ) -{ - Ptr matM, _m; - Ptr _dpdr, _dpdt, _dpdc, _dpdf, _dpdk; - Ptr _dpdo; - - int i, j, count; - int calc_derivatives; - const CvPoint3D64f* M; - CvPoint2D64f* m; - double r[3], R[9], R_vec[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; - Matx33d matTilt = Matx33d::eye(); - Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0); - Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0); - CvMat _r, _r_vec, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; - CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); - double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; - double* dpdo_p = 0; - int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; - int dpdo_step = 0; - bool fixedAspectRatio = aspectRatio > FLT_EPSILON; - - if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) || - !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) || - /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) - CV_Error( cv::Error::StsBadArg, "One of required arguments is not a valid matrix" ); - - int odepth = CV_MAT_DEPTH(objectPoints->type); - int ochans = CV_MAT_CN(objectPoints->type); - int orows = objectPoints->rows, ocols = objectPoints->cols; - int total = orows * ocols * ochans; - if(total % 3 != 0) - { - //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data - CV_Error( cv::Error::StsBadArg, "Homogeneous coordinates are not supported" ); - } - count = total / 3; - - CV_Assert(CV_IS_CONT_MAT(objectPoints->type)); - CV_Assert(odepth == CV_32F || odepth == CV_64F); - // Homogeneous coordinates are not supported - CV_Assert((orows == 1 && ochans == 3) || - (orows == count && ochans*ocols == 3) || - (orows == 3 && ochans == 1 && ocols == count)); - - int idepth = CV_MAT_DEPTH(imagePoints->type); - int ichans = CV_MAT_CN(imagePoints->type); - int irows = imagePoints->rows, icols = imagePoints->cols; - CV_Assert(CV_IS_CONT_MAT(imagePoints->type)); - CV_Assert(idepth == CV_32F || idepth == CV_64F); - // Homogeneous coordinates are not supported - CV_Assert((irows == 1 && ichans == 2) || - (irows == count && ichans*icols == 2) || - (irows == 2 && ichans == 1 && icols == count)); - - if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || - (((r_vec->rows != 1 && r_vec->cols != 1) || - r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) && - ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1))) - CV_Error( cv::Error::StsBadArg, "Rotation must be represented by 1x3 or 3x1 " - "floating-point rotation vector, or 3x3 rotation matrix" ); - - if( r_vec->rows == 3 && r_vec->cols == 3 ) - { - _r = cvMat( 3, 1, CV_64FC1, r ); - _r_vec = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), R_vec ); - cvConvert( r_vec, &_r_vec ); - cvRodrigues2( &_r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - cvCopy( &_r_vec, &matR ); - } - else - { - _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r ); - cvConvert( r_vec, &_r ); - cvRodrigues2( &_r, &matR, &_dRdr ); - } - - if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) || - (t_vec->rows != 1 && t_vec->cols != 1) || - t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 ) - CV_Error( cv::Error::StsBadArg, - "Translation vector must be 1x3 or 3x1 floating-point vector" ); - - _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t ); - cvConvert( t_vec, &_t ); - - if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) || - A->rows != 3 || A->cols != 3 ) - CV_Error( cv::Error::StsBadArg, "Intrinsic parameters must be 3x3 floating-point matrix" ); - - cvConvert( A, &_a ); - fx = a[0]; fy = a[4]; - cx = a[2]; cy = a[5]; - - if( fixedAspectRatio ) - fx = fy*aspectRatio; - - int delems = 0; - if( distCoeffs ) - { - CV_Assert(CV_IS_MAT(distCoeffs)); - - int ddepth = CV_MAT_DEPTH(distCoeffs->type); - int dchans = CV_MAT_CN(distCoeffs->type); - int drows = distCoeffs->rows, dcols = distCoeffs->cols; - delems = drows * dcols * dchans; - CV_Assert((ddepth == CV_32F || ddepth == CV_64F) && - (drows == 1 || dcols == 1) && - (delems == 4 || delems == 5 || delems == 8 || delems == 12 || delems == 14)); - - _k = cvMat( drows, dcols, CV_MAKETYPE(CV_64F, dchans), k ); - cvConvert( distCoeffs, &_k ); - if(k[12] != 0 || k[13] != 0) - { - detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); - } - } - - if (idepth == CV_32F && odepth == CV_32F) - { - float rtMatrix[12] = { (float)R[0], (float)R[1], (float)R[2], (float)t[0], - (float)R[3], (float)R[4], (float)R[5], (float)t[1], - (float)R[6], (float)R[7], (float)R[8], (float)t[2] }; - - cv_camera_intrinsics_pinhole_32f intr; - intr.fx = (float)fx; intr.fy = (float)fy; - intr.cx = (float)cx; intr.cy = (float)cy; - intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; - - switch (delems) - { - case 0: break; - case 4: // [k_1, k_2, p_1, p_2] - intr.amt_k = 2; intr.amt_p = 2; - break; - case 5: // [k_1, k_2, p_1, p_2, k_3] - intr.amt_k = 3; intr.amt_p = 2; - break; - case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] - intr.amt_k = 6; intr.amt_p = 2; - break; - case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] - intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; - break; - case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] - intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; - break; - default: - CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); - } - - intr.k[0] = (float)k[0]; - intr.k[1] = (float)k[1]; - intr.k[2] = (float)k[4]; - intr.k[3] = (float)k[5]; - intr.k[4] = (float)k[6]; - intr.k[5] = (float)k[7]; - - intr.p[0] = (float)k[2]; - intr.p[1] = (float)k[3]; - - for (int ctr = 0; ctr < 4; ctr++) - { - intr.s[ctr] = (float)k[8+ctr]; - } - - intr.tau_x = (float)k[12]; - intr.tau_y = (float)k[13]; - - CALL_HAL(projectPoints, cv_hal_project_points_pinhole32f, - objectPoints->data.fl, objectPoints->step, count, - imagePoints->data.fl, imagePoints->step, - rtMatrix, &intr); - } - - _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) )); - cvConvert(imagePoints, _m); - - matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) )); - cvConvert(objectPoints, matM); - - M = (CvPoint3D64f*)matM->data.db; - m = (CvPoint2D64f*)_m->data.db; - - if (idepth == CV_64F && odepth == CV_64F) - { - double rtMatrix[12] = { R[0], R[1], R[2], t[0], - R[3], R[4], R[5], t[1], - R[6], R[7], R[8], t[2] }; - - cv_camera_intrinsics_pinhole_64f intr; - intr.fx = fx; intr.fy = fy; - intr.cx = cx; intr.cy = cy; - intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; - - switch (delems) - { - case 0: break; - case 4: // [k_1, k_2, p_1, p_2] - intr.amt_k = 2; intr.amt_p = 2; - break; - case 5: // [k_1, k_2, p_1, p_2, k_3] - intr.amt_k = 3; intr.amt_p = 2; - break; - case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] - intr.amt_k = 6; intr.amt_p = 2; - break; - case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] - intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; - break; - case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] - intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; - break; - default: - CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); - } - - intr.k[0] = k[0]; - intr.k[1] = k[1]; - intr.k[2] = k[4]; - intr.k[3] = k[5]; - intr.k[4] = k[6]; - intr.k[5] = k[7]; - - intr.p[0] = k[2]; - intr.p[1] = k[3]; - - for (int ctr = 0; ctr < 4; ctr++) - { - intr.s[ctr] = k[8+ctr]; - } - - intr.tau_x = k[12]; - intr.tau_y = k[13]; - - CALL_HAL(projectPoints, cv_hal_project_points_pinhole64f, - objectPoints->data.db, objectPoints->step, count, - imagePoints->data.db, imagePoints->step, - rtMatrix, &intr); - } - - if( dpdr ) - { - if( !CV_IS_MAT(dpdr) || - (CV_MAT_TYPE(dpdr->type) != CV_32FC1 && - CV_MAT_TYPE(dpdr->type) != CV_64FC1) || - dpdr->rows != count*2 || dpdr->cols != 3 ) - CV_Error( cv::Error::StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 ) - { - _dpdr.reset(cvCloneMat(dpdr)); - } - else - _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdr_p = _dpdr->data.db; - dpdr_step = _dpdr->step/sizeof(dpdr_p[0]); - } - - if( dpdt ) - { - if( !CV_IS_MAT(dpdt) || - (CV_MAT_TYPE(dpdt->type) != CV_32FC1 && - CV_MAT_TYPE(dpdt->type) != CV_64FC1) || - dpdt->rows != count*2 || dpdt->cols != 3 ) - CV_Error( cv::Error::StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 ) - { - _dpdt.reset(cvCloneMat(dpdt)); - } - else - _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 )); - dpdt_p = _dpdt->data.db; - dpdt_step = _dpdt->step/sizeof(dpdt_p[0]); - } - - if( dpdf ) - { - if( !CV_IS_MAT(dpdf) || - (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) || - dpdf->rows != count*2 || dpdf->cols != 2 ) - CV_Error( cv::Error::StsBadArg, "dp/df must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 ) - { - _dpdf.reset(cvCloneMat(dpdf)); - } - else - _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdf_p = _dpdf->data.db; - dpdf_step = _dpdf->step/sizeof(dpdf_p[0]); - } - - if( dpdc ) - { - if( !CV_IS_MAT(dpdc) || - (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) || - dpdc->rows != count*2 || dpdc->cols != 2 ) - CV_Error( cv::Error::StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" ); - - if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 ) - { - _dpdc.reset(cvCloneMat(dpdc)); - } - else - _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 )); - dpdc_p = _dpdc->data.db; - dpdc_step = _dpdc->step/sizeof(dpdc_p[0]); - } - - if( dpdk ) - { - if( !CV_IS_MAT(dpdk) || - (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || - dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) - CV_Error( cv::Error::StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); - - if( !distCoeffs ) - CV_Error( cv::Error::StsNullPtr, "distCoeffs is NULL while dpdk is not" ); - - if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 ) - { - _dpdk.reset(cvCloneMat(dpdk)); - } - else - _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 )); - dpdk_p = _dpdk->data.db; - dpdk_step = _dpdk->step/sizeof(dpdk_p[0]); - } - - if( dpdo ) - { - if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1 - && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 ) - || dpdo->rows != count * 2 || dpdo->cols != count * 3 ) - CV_Error( cv::Error::StsBadArg, "dp/do must be 2Nx3N floating-point matrix" ); - - if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 ) - { - _dpdo.reset( cvCloneMat( dpdo ) ); - } - else - _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) ); - cvZero(_dpdo); - dpdo_p = _dpdo->data.db; - dpdo_step = _dpdo->step / sizeof( dpdo_p[0] ); - } - - calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo; - - for( i = 0; i < count; i++ ) - { - double X = M[i].x, Y = M[i].y, Z = M[i].z; - double x = R[0]*X + R[1]*Y + R[2]*Z + t[0]; - double y = R[3]*X + R[4]*Y + R[5]*Z + t[1]; - double z = R[6]*X + R[7]*Y + R[8]*Z + t[2]; - double r2, r4, r6, a1, a2, a3, cdist, icdist2; - double xd, yd, xd0, yd0, invProj; - Vec3d vecTilt; - Vec3d dVecTilt; - Matx22d dMatTilt; - Vec2d dXdYd; - - double z0 = z; - z = z ? 1./z : 1; - x *= z; y *= z; - - r2 = x*x + y*y; - r4 = r2*r2; - r6 = r4*r2; - a1 = 2*x*y; - a2 = r2 + 2*x*x; - a3 = r2 + 2*y*y; - cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; - icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6); - xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4; - yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4; - - // additional distortion by projecting onto a tilt plane - vecTilt = matTilt*Vec3d(xd0, yd0, 1); - invProj = vecTilt(2) ? 1./vecTilt(2) : 1; - xd = invProj * vecTilt(0); - yd = invProj * vecTilt(1); - - m[i].x = xd*fx + cx; - m[i].y = yd*fy + cy; - - if( calc_derivatives ) - { - if( dpdc_p ) - { - dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y - dpdc_p[dpdc_step] = 0; - dpdc_p[dpdc_step+1] = 1; - dpdc_p += dpdc_step*2; - } - - if( dpdf_p ) - { - if( fixedAspectRatio ) - { - dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y - dpdf_p[dpdf_step] = 0; - dpdf_p[dpdf_step+1] = yd; - } - else - { - dpdf_p[0] = xd; dpdf_p[1] = 0; - dpdf_p[dpdf_step] = 0; - dpdf_p[dpdf_step+1] = yd; - } - dpdf_p += dpdf_step*2; - } - for (int row = 0; row < 2; ++row) - for (int col = 0; col < 2; ++col) - dMatTilt(row,col) = matTilt(row,col)*vecTilt(2) - - matTilt(2,col)*vecTilt(row); - double invProjSquare = (invProj*invProj); - dMatTilt *= invProjSquare; - if( dpdk_p ) - { - dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2); - dpdk_p[0] = fx*dXdYd(0); - dpdk_p[dpdk_step] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4); - dpdk_p[1] = fx*dXdYd(0); - dpdk_p[dpdk_step+1] = fy*dXdYd(1); - if( _dpdk->cols > 2 ) - { - dXdYd = dMatTilt*Vec2d(a1, a3); - dpdk_p[2] = fx*dXdYd(0); - dpdk_p[dpdk_step+2] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d(a2, a1); - dpdk_p[3] = fx*dXdYd(0); - dpdk_p[dpdk_step+3] = fy*dXdYd(1); - if( _dpdk->cols > 4 ) - { - dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6); - dpdk_p[4] = fx*dXdYd(0); - dpdk_p[dpdk_step+4] = fy*dXdYd(1); - - if( _dpdk->cols > 5 ) - { - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2); - dpdk_p[5] = fx*dXdYd(0); - dpdk_p[dpdk_step+5] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4); - dpdk_p[6] = fx*dXdYd(0); - dpdk_p[dpdk_step+6] = fy*dXdYd(1); - dXdYd = dMatTilt*Vec2d( - x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6); - dpdk_p[7] = fx*dXdYd(0); - dpdk_p[dpdk_step+7] = fy*dXdYd(1); - if( _dpdk->cols > 8 ) - { - dXdYd = dMatTilt*Vec2d(r2, 0); - dpdk_p[8] = fx*dXdYd(0); //s1 - dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1 - dXdYd = dMatTilt*Vec2d(r4, 0); - dpdk_p[9] = fx*dXdYd(0); //s2 - dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2 - dXdYd = dMatTilt*Vec2d(0, r2); - dpdk_p[10] = fx*dXdYd(0);//s3 - dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3 - dXdYd = dMatTilt*Vec2d(0, r4); - dpdk_p[11] = fx*dXdYd(0);//s4 - dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4 - if( _dpdk->cols > 12 ) - { - dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1); - dpdk_p[12] = fx * invProjSquare * ( - dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); - dpdk_p[dpdk_step+12] = fy*invProjSquare * ( - dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); - dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1); - dpdk_p[13] = fx * invProjSquare * ( - dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); - dpdk_p[dpdk_step+13] = fy * invProjSquare * ( - dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); - } - } - } - } - } - dpdk_p += dpdk_step*2; - } - - if( dpdt_p ) - { - double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z }; - for( j = 0; j < 3; j++ ) - { - double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j]; - double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt; - double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt); - double da1dt = 2*(x*dydt[j] + y*dxdt[j]); - double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt + - k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt); - double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt + - k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt); - dXdYd = dMatTilt*Vec2d(dmxdt, dmydt); - dpdt_p[j] = fx*dXdYd(0); - dpdt_p[dpdt_step+j] = fy*dXdYd(1); - } - dpdt_p += dpdt_step*2; - } - - if( dpdr_p ) - { - double dx0dr[] = - { - X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2], - X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11], - X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20] - }; - double dy0dr[] = - { - X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5], - X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14], - X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23] - }; - double dz0dr[] = - { - X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8], - X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17], - X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26] - }; - for( j = 0; j < 3; j++ ) - { - double dxdr = z*(dx0dr[j] - x*dz0dr[j]); - double dydr = z*(dy0dr[j] - y*dz0dr[j]); - double dr2dr = 2*x*dxdr + 2*y*dydr; - double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr; - double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr; - double da1dr = 2*(x*dydr + y*dxdr); - double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr + - k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr); - double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr + - k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr); - dXdYd = dMatTilt*Vec2d(dmxdr, dmydr); - dpdr_p[j] = fx*dXdYd(0); - dpdr_p[dpdr_step+j] = fy*dXdYd(1); - } - dpdr_p += dpdr_step*2; - } - - if( dpdo_p ) - { - double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ), - z * ( R[1] - x * z * z0 * R[7] ), - z * ( R[2] - x * z * z0 * R[8] ) }; - double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ), - z * ( R[4] - y * z * z0 * R[7] ), - z * ( R[5] - y * z * z0 * R[8] ) }; - for( j = 0; j < 3; j++ ) - { - double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j]; - double dr4do = 2 * r2 * dr2do; - double dr6do = 3 * r4 * dr2do; - double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j]; - double da2do = dr2do + 4 * x * dxdo[j]; - double da3do = dr2do + 4 * y * dydo[j]; - double dcdist_do - = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do; - double dicdist2_do = -icdist2 * icdist2 - * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do ); - double dxd0_do = cdist * icdist2 * dxdo[j] - + x * icdist2 * dcdist_do + x * cdist * dicdist2_do - + k[2] * da1do + k[3] * da2do + k[8] * dr2do - + k[9] * dr4do; - double dyd0_do = cdist * icdist2 * dydo[j] - + y * icdist2 * dcdist_do + y * cdist * dicdist2_do - + k[2] * da3do + k[3] * da1do + k[10] * dr2do - + k[11] * dr4do; - dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do ); - dpdo_p[i * 3 + j] = fx * dXdYd( 0 ); - dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 ); - } - dpdo_p += dpdo_step * 2; - } - } - } - - if( _m != imagePoints ) - cvConvert( _m, imagePoints ); - - if( _dpdr != dpdr ) - cvConvert( _dpdr, dpdr ); - - if( _dpdt != dpdt ) - cvConvert( _dpdt, dpdt ); - - if( _dpdf != dpdf ) - cvConvert( _dpdf, dpdf ); - - if( _dpdc != dpdc ) - cvConvert( _dpdc, dpdc ); - - if( _dpdk != dpdk ) - cvConvert( _dpdk, dpdk ); - - if( _dpdo != dpdo ) - cvConvert( _dpdo, dpdo ); -} - -CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, - const CvMat* r_vec, - const CvMat* t_vec, - const CvMat* A, - const CvMat* distCoeffs, - CvMat* imagePoints, CvMat* dpdr, - CvMat* dpdt, CvMat* dpdf, - CvMat* dpdc, CvMat* dpdk, - double aspectRatio ) -{ - cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt, - dpdf, dpdc, dpdk, NULL, aspectRatio ); -} - -CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* A, - const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec, - int useExtrinsicGuess ) -{ - const int max_iter = 20; - Ptr matM, _Mxy, _m, _mn, matL; - - int i, count; - double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9]; - double MM[9] = { 0 }, U[9] = { 0 }, V[9] = { 0 }, W[3] = { 0 }; - cv::Scalar Mc; - double param[6] = { 0 }; - CvMat matA = cvMat( 3, 3, CV_64F, a ); - CvMat _Ar = cvMat( 3, 3, CV_64F, ar ); - CvMat matR = cvMat( 3, 3, CV_64F, R ); - CvMat _r = cvMat( 3, 1, CV_64F, param ); - CvMat _t = cvMat( 3, 1, CV_64F, param + 3 ); - CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val ); - CvMat _MM = cvMat( 3, 3, CV_64F, MM ); - CvMat matU = cvMat( 3, 3, CV_64F, U ); - CvMat matV = cvMat( 3, 3, CV_64F, V ); - CvMat matW = cvMat( 3, 1, CV_64F, W ); - CvMat _param = cvMat( 6, 1, CV_64F, param ); - CvMat _dpdr, _dpdt; - - CV_Assert( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) && - CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) ); - - count = MAX(objectPoints->cols, objectPoints->rows); - matM.reset(cvCreateMat( 1, count, CV_64FC3 )); - _m.reset(cvCreateMat( 1, count, CV_64FC2 )); - - cvConvertPointsHomogeneous( objectPoints, matM ); - cvConvertPointsHomogeneous( imagePoints, _m ); - cvConvert( A, &matA ); - - CV_Assert( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) && - (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 ); - - CV_Assert( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) && - (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 ); - - CV_Assert((count >= 4) || (count == 3 && useExtrinsicGuess)); // it is unsafe to call LM optimisation without an extrinsic guess in the case of 3 points. This is because there is no guarantee that it will converge on the correct solution. - - _mn.reset(cvCreateMat( 1, count, CV_64FC2 )); - _Mxy.reset(cvCreateMat( 1, count, CV_64FC2 )); - - // normalize image points - // (unapply the intrinsic matrix transformation and distortion) - cvUndistortPoints( _m, _mn, &matA, distCoeffs, 0, &_Ar ); - - if( useExtrinsicGuess ) - { - CvMat _r_temp = cvMat(rvec->rows, rvec->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param ); - CvMat _t_temp = cvMat(tvec->rows, tvec->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3); - cvConvert( rvec, &_r_temp ); - cvConvert( tvec, &_t_temp ); - } - else - { - Mc = cvAvg(matM); - cvReshape( matM, matM, 1, count ); - cvMulTransposed( matM, &_MM, 1, &_Mc ); - cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T ); - - // initialize extrinsic parameters - if( W[2]/W[1] < 1e-3) - { - // a planar structure case (all M's lie in the same plane) - double tt[3], h[9], h1_norm, h2_norm; - CvMat* R_transform = &matV; - CvMat T_transform = cvMat( 3, 1, CV_64F, tt ); - CvMat matH = cvMat( 3, 3, CV_64F, h ); - CvMat _h1, _h2, _h3; - - if( V[2]*V[2] + V[5]*V[5] < 1e-10 ) - cvSetIdentity( R_transform ); - - if( cvDet(R_transform) < 0 ) - cvScale( R_transform, R_transform, -1 ); - - cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T ); - - for( i = 0; i < count; i++ ) - { - const double* Rp = R_transform->data.db; - const double* Tp = T_transform.data.db; - const double* src = matM->data.db + i*3; - double* dst = _Mxy->data.db + i*2; - - dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0]; - dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1]; - } - - cvFindHomography( _Mxy, _mn, &matH ); - - if( cvCheckArr(&matH, CV_CHECK_QUIET) ) - { - cvGetCol( &matH, &_h1, 0 ); - _h2 = _h1; _h2.data.db++; - _h3 = _h2; _h3.data.db++; - h1_norm = std::sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]); - h2_norm = std::sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]); - - cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) ); - cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) ); - cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON)); - cvCrossProduct( &_h1, &_h2, &_h3 ); - - cvRodrigues2( &matH, &_r ); - cvRodrigues2( &_r, &matH ); - cvMatMulAdd( &matH, &T_transform, &_t, &_t ); - cvMatMul( &matH, R_transform, &matR ); - } - else - { - cvSetIdentity( &matR ); - cvZero( &_t ); - } - - cvRodrigues2( &matR, &_r ); - } - else - { - // non-planar structure. Use DLT method - CV_CheckGE(count, 6, "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences."); - double* L; - double LL[12*12], LW[12], LV[12*12], sc; - CvMat _LL = cvMat( 12, 12, CV_64F, LL ); - CvMat _LW = cvMat( 12, 1, CV_64F, LW ); - CvMat _LV = cvMat( 12, 12, CV_64F, LV ); - CvMat _RRt, _RR, _tt; - CvPoint3D64f* M = (CvPoint3D64f*)matM->data.db; - CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db; - - matL.reset(cvCreateMat( 2*count, 12, CV_64F )); - L = matL->data.db; - - for( i = 0; i < count; i++, L += 24 ) - { - double x = -mn[i].x, y = -mn[i].y; - L[0] = L[16] = M[i].x; - L[1] = L[17] = M[i].y; - L[2] = L[18] = M[i].z; - L[3] = L[19] = 1.; - L[4] = L[5] = L[6] = L[7] = 0.; - L[12] = L[13] = L[14] = L[15] = 0.; - L[8] = x*M[i].x; - L[9] = x*M[i].y; - L[10] = x*M[i].z; - L[11] = x; - L[20] = y*M[i].x; - L[21] = y*M[i].y; - L[22] = y*M[i].z; - L[23] = y; - } - - cvMulTransposed( matL, &_LL, 1 ); - cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T ); - _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 ); - cvGetCols( &_RRt, &_RR, 0, 3 ); - cvGetCol( &_RRt, &_tt, 3 ); - if( cvDet(&_RR) < 0 ) - cvScale( &_RRt, &_RRt, -1 ); - sc = cvNorm(&_RR); - CV_Assert(fabs(sc) > DBL_EPSILON); - cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); - cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T ); - cvScale( &_tt, &_t, cvNorm(&matR)/sc ); - cvRodrigues2( &matR, &_r ); - } - } - - cvReshape( matM, matM, 3, 1 ); - cvReshape( _mn, _mn, 2, 1 ); - - // refine extrinsic parameters using iterative algorithm - CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true); - cvCopy( &_param, solver.param ); - - for(;;) - { - CvMat *matJ = 0, *_err = 0; - const CvMat *__param = 0; - bool proceed = solver.update( __param, matJ, _err ); - cvCopy( __param, &_param ); - if( !proceed || !_err ) - break; - cvReshape( _err, _err, 2, 1 ); - if( matJ ) - { - cvGetCols( matJ, &_dpdr, 0, 3 ); - cvGetCols( matJ, &_dpdt, 3, 6 ); - cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs, - _err, &_dpdr, &_dpdt, 0, 0, 0 ); - } - else - { - cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs, - _err, 0, 0, 0, 0, 0 ); - } - cvSub(_err, _m, _err); - cvReshape( _err, _err, 1, 2*count ); - } - cvCopy( solver.param, &_param ); - - _r = cvMat( rvec->rows, rvec->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param ); - _t = cvMat( tvec->rows, tvec->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 ); - - cvConvert( &_r, rvec ); - cvConvert( &_t, tvec ); -} - CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints, const CvMat* imagePoints, const CvMat* npoints, CvSize imageSize, CvMat* cameraMatrix, @@ -1797,7 +487,9 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, CvMat _Mi = cvMat(matM.colRange(pos, pos + ni)); CvMat _mi = cvMat(_m.colRange(pos, pos + ni)); - cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti ); + Mat r_mat = cvarrToMat(&_ri), t_mat = cvarrToMat(&_ti); + findExtrinsicCameraParams2( cvarrToMat(&_Mi), cvarrToMat(&_mi), cvarrToMat(&matA), + cvarrToMat(&_k), r_mat, t_mat, /*useExtrinsicGuess=*/0 ); } // 3. run the optimization @@ -1851,21 +543,16 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, if( calcJ ) { - CvMat _dpdr = cvMat(_Je.colRange(0, 3)); - CvMat _dpdt = cvMat(_Je.colRange(3, 6)); - CvMat _dpdf = cvMat(_Ji.colRange(0, 2)); - CvMat _dpdc = cvMat(_Ji.colRange(2, 4)); - CvMat _dpdk = cvMat(_Ji.colRange(4, NINTRINSIC)); - CvMat _dpdo = _Jo.empty() ? CvMat() : cvMat(_Jo.colRange(0, ni * 3)); - - cvProjectPoints2Internal( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt, - (flags & CALIB_FIX_FOCAL_LENGTH) ? nullptr : &_dpdf, - (flags & CALIB_FIX_PRINCIPAL_POINT) ? nullptr : &_dpdc, &_dpdk, - (_Jo.empty()) ? nullptr: &_dpdo, - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); + projectPoints( cvarrToMat(&_Mi), cvarrToMat(&_ri), cvarrToMat(&_ti), cvarrToMat(&matA), + cvarrToMat(&_k), cvarrToMat(&_mp), _Je.colRange(0, 3), _Je.colRange(3, 6), + (flags & CALIB_FIX_FOCAL_LENGTH) ? noArray() : _Ji.colRange(0, 2), + (flags & CALIB_FIX_PRINCIPAL_POINT) ? noArray() : _Ji.colRange(2, 4), + _Ji.colRange(4, 4 + _k.cols * _k.rows), (_Jo.empty()) ? noArray() : _Jo.colRange(0, ni * 3), + (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); } else - cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp ); + projectPoints( cvarrToMat(&_Mi), cvarrToMat(&_ri), cvarrToMat(&_ti), cvarrToMat(&matA), + cvarrToMat(&_k), cvarrToMat(&_mp) ); cvSub( &_mp, &_mi, &_mp ); if (perViewErrors || stdDevs) @@ -1956,7 +643,7 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints, { dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type), rvecs->data.ptr + rvecs->step*i ); - cvRodrigues2( &src, &matA ); + Rodrigues( cvarrToMat(&src), cvarrToMat(&matA) ); cvConvert( &matA, &dst ); } else @@ -2286,8 +973,11 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i R[k] = cvMat(3, 3, CV_64F, r[k]); T[k] = cvMat(3, 1, CV_64F, t[k]); - cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] ); - cvRodrigues2( &om[k], &R[k] ); + Mat r_mat = cvarrToMat(&om[k]), t_mat = cvarrToMat(&T[k]); + findExtrinsicCameraParams2( cvarrToMat(&objpt_i), cvarrToMat(&imgpt_i[k]), + cvarrToMat(&K[k]), cvarrToMat(&Dist[k]), + r_mat, t_mat, /*useExtrinsicGuess=*/0 ); + Rodrigues( cvarrToMat(&om[k]), cvarrToMat(&R[k]) ); if( k == 0 ) { // save initial om_left and T_left @@ -2301,7 +991,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i } cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T ); cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] ); - cvRodrigues2( &R[0], &T[0] ); + Rodrigues( cvarrToMat(&R[0]), cvarrToMat(&T[0]) ); RT0->data.db[i] = t[0][0]; RT0->data.db[i + nimages] = t[0][1]; RT0->data.db[i + nimages*2] = t[0][2]; @@ -2378,7 +1068,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i break; reprojErr = 0; - cvRodrigues2( &om_LR, &R_LR ); + Rodrigues( cvarrToMat(&om_LR), cvarrToMat(&R_LR) ); om[1] = cvMat(3,1,CV_64F,_omR); T[1] = cvMat(3,1,CV_64F,_tR); @@ -2433,31 +1123,33 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3); if( JtJ || JtErr ) - cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0, - &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 ); + composeRT( cvarrToMat(&om[0]), cvarrToMat(&T[0]), cvarrToMat(&om_LR), + cvarrToMat(&T_LR), cvarrToMat(&om[1]), cvarrToMat(&T[1]), + cvarrToMat(&dr3dr1), noArray(), cvarrToMat(&dr3dr2), + noArray(), noArray(), cvarrToMat(&dt3dt1), cvarrToMat(&dt3dr2), + cvarrToMat(&dt3dt2 ) ); else - cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] ); + composeRT( cvarrToMat(&om[0]), cvarrToMat(&T[0]), cvarrToMat(&om_LR), + cvarrToMat(&T_LR), cvarrToMat(&om[1]), cvarrToMat(&T[1]) ); objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2); CvMat tmpimagePoints = cvMat(err.reshape(2, 1)); - CvMat dpdf = cvMat(Ji.colRange(0, 2)); - CvMat dpdc = cvMat(Ji.colRange(2, 4)); - CvMat dpdk = cvMat(Ji.colRange(4, NINTRINSIC)); - CvMat dpdrot = cvMat(Je.colRange(0, 3)); - CvMat dpdt = cvMat(Je.colRange(3, 6)); for( k = 0; k < 2; k++ ) { imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); if( JtJ || JtErr ) - cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], - &tmpimagePoints, &dpdrot, &dpdt, &dpdf, &dpdc, &dpdk, + projectPoints( cvarrToMat(&objpt_i), cvarrToMat(&om[k]), cvarrToMat(&T[k]), + cvarrToMat(&K[k]), cvarrToMat(&Dist[k]), + err.reshape(2, 1), Je.colRange(0, 3), Je.colRange(3, 6), + Ji.colRange(0, 2), Ji.colRange(2, 4), Ji.colRange(4, 4 + Dist[k].cols * Dist[k].rows), noArray(), (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0); else - cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints ); + projectPoints( cvarrToMat(&objpt_i), cvarrToMat(&om[k]), cvarrToMat(&T[k]), + cvarrToMat(&K[k]), cvarrToMat(&Dist[k]), cvarrToMat(&tmpimagePoints) ); cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints ); if( solver.state == CvLevMarq::CALC_J ) @@ -2525,7 +1217,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i *_errNorm = reprojErr; } - cvRodrigues2( &om_LR, &R_LR ); + Rodrigues( cvarrToMat(&om_LR), cvarrToMat(&R_LR) ); if( matR->rows == 1 || matR->cols == 1 ) cvConvert( &om_LR, matR ); else @@ -2586,7 +1278,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i { dst = cvMat(3, 3, CV_MAT_DEPTH(rvecs->type), rvecs->data.ptr + rvecs->step*i); - cvRodrigues2( &src, &tmp ); + Rodrigues( cvarrToMat(&src), cvarrToMat(&tmp) ); cvConvert( &tmp, &dst ); } else @@ -2691,11 +1383,11 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, int i, k; if( matR->rows == 3 && matR->cols == 3 ) - cvRodrigues2(matR, &om); // get vector rotation + Rodrigues(cvarrToMat(matR), cvarrToMat(&om)); // get vector rotation else cvConvert(matR, &om); // it's already a rotation vector cvConvertScale(&om, &om, -0.5); // get average rotation - cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging + Rodrigues(cvarrToMat(&om), cvarrToMat(&r_r)); // rotate cameras to same orientation by averaging cvMatMul(&r_r, matT, &t); int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; @@ -2709,7 +1401,7 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, double nw = cvNorm(&ww, 0, CV_L2); if (nw > 0.0) cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); - cvRodrigues2(&ww, &wR); + Rodrigues(cvarrToMat(&ww), cvarrToMat(&wR)); // apply to both views cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); @@ -2754,7 +1446,8 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, _a_tmp[1][1]=fc_new; _a_tmp[0][2]=0.0; _a_tmp[1][2]=0.0; - cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts ); + projectPoints( cvarrToMat(&pts_3), cvarrToMat(k == 0 ? _R1 : _R2), cvarrToMat(&Z), + cvarrToMat(&A_tmp), noArray(), cvarrToMat(&pts) ); CvScalar avg = cvAvg(&pts); cc_new[k].x = (nx-1)/2 - avg.val[0]; cc_new[k].y = (ny-1)/2 - avg.val[1]; @@ -2878,87 +1571,6 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, } -void cvGetOptimalNewCameraMatrix( const CvMat* cameraMatrix, const CvMat* distCoeffs, - CvSize imgSize, double alpha, - CvMat* newCameraMatrix, CvSize newImgSize, - CvRect* validPixROI, int centerPrincipalPoint ) -{ - cv::Rect_ inner, outer; - newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imgSize; - - double M[3][3]; - CvMat matM = cvMat(3, 3, CV_64F, M); - cvConvert(cameraMatrix, &matM); - - if( centerPrincipalPoint ) - { - double cx0 = M[0][2]; - double cy0 = M[1][2]; - double cx = (newImgSize.width-1)*0.5; - double cy = (newImgSize.height-1)*0.5; - - icvGetRectangles( cameraMatrix, distCoeffs, 0, cameraMatrix, imgSize, inner, outer ); - double s0 = std::max(std::max(std::max((double)cx/(cx0 - inner.x), (double)cy/(cy0 - inner.y)), - (double)cx/(inner.x + inner.width - cx0)), - (double)cy/(inner.y + inner.height - cy0)); - double s1 = std::min(std::min(std::min((double)cx/(cx0 - outer.x), (double)cy/(cy0 - outer.y)), - (double)cx/(outer.x + outer.width - cx0)), - (double)cy/(outer.y + outer.height - cy0)); - double s = s0*(1 - alpha) + s1*alpha; - - M[0][0] *= s; - M[1][1] *= s; - M[0][2] = cx; - M[1][2] = cy; - - if( validPixROI ) - { - inner = cv::Rect_((double)((inner.x - cx0)*s + cx), - (double)((inner.y - cy0)*s + cy), - (double)(inner.width*s), - (double)(inner.height*s)); - cv::Rect r(cvCeil(inner.x), cvCeil(inner.y), cvFloor(inner.width), cvFloor(inner.height)); - r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height); - *validPixROI = cvRect(r); - } - } - else - { - // Get inscribed and circumscribed rectangles in normalized - // (independent of camera matrix) coordinates - icvGetRectangles( cameraMatrix, distCoeffs, 0, 0, imgSize, inner, outer ); - - // Projection mapping inner rectangle to viewport - double fx0 = (newImgSize.width - 1) / inner.width; - double fy0 = (newImgSize.height - 1) / inner.height; - double cx0 = -fx0 * inner.x; - double cy0 = -fy0 * inner.y; - - // Projection mapping outer rectangle to viewport - double fx1 = (newImgSize.width - 1) / outer.width; - double fy1 = (newImgSize.height - 1) / outer.height; - double cx1 = -fx1 * outer.x; - double cy1 = -fy1 * outer.y; - - // Interpolate between the two optimal projections - M[0][0] = fx0*(1 - alpha) + fx1*alpha; - M[1][1] = fy0*(1 - alpha) + fy1*alpha; - M[0][2] = cx0*(1 - alpha) + cx1*alpha; - M[1][2] = cy0*(1 - alpha) + cy1*alpha; - - if( validPixROI ) - { - icvGetRectangles( cameraMatrix, distCoeffs, 0, &matM, imgSize, inner, outer ); - cv::Rect r = inner; - r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height); - *validPixROI = cvRect(r); - } - } - - cvConvert(&matM, newCameraMatrix); -} - - CV_IMPL int cvStereoRectifyUncalibrated( const CvMat* _points1, const CvMat* _points2, const CvMat* F0, CvSize imgSize, @@ -3273,222 +1885,6 @@ void cvReprojectImageTo3D( const CvArr* disparityImage, } -CV_IMPL void -cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, - CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, - CvPoint3D64f *eulerAngles) -{ - double matM[3][3], matR[3][3], matQ[3][3]; - CvMat M = cvMat(3, 3, CV_64F, matM); - CvMat R = cvMat(3, 3, CV_64F, matR); - CvMat Q = cvMat(3, 3, CV_64F, matQ); - double z, c, s; - - /* Validate parameters. */ - CV_Assert( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && - matrixM->cols == 3 && matrixM->rows == 3 && - CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); - - cvConvert(matrixM, &M); - - /* Find Givens rotation Q_x for x axis (left multiplication). */ - /* - ( 1 0 0 ) - Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) - ( 0 -s c ) - */ - s = std::abs(matM[2][1]) > DBL_EPSILON ? matM[2][1] : 0; - c = std::abs(matM[2][1]) > DBL_EPSILON ? matM[2][2] : 1; - z = 1./std::sqrt(c * c + s * s); - c *= z; - s *= z; - - double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} }; - CvMat Qx = cvMat(3, 3, CV_64F, _Qx); - - cvMatMul(&M, &Qx, &R); - CV_DbgAssert(fabs(matR[2][1]) < FLT_EPSILON); - matR[2][1] = 0; - - /* Find Givens rotation for y axis. */ - /* - ( c 0 -s ) - Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2) - ( s 0 c ) - */ - s = std::abs(matR[2][0]) > DBL_EPSILON ? -matR[2][0] : 0; - c = std::abs(matR[2][0]) > DBL_EPSILON ? matR[2][2] : 1; - z = 1./std::sqrt(c * c + s * s); - c *= z; - s *= z; - - double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} }; - CvMat Qy = cvMat(3, 3, CV_64F, _Qy); - cvMatMul(&R, &Qy, &M); - - CV_DbgAssert(fabs(matM[2][0]) < FLT_EPSILON); - matM[2][0] = 0; - - /* Find Givens rotation for z axis. */ - /* - ( c s 0 ) - Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) - ( 0 0 1 ) - */ - - s = std::abs(matM[1][0]) > DBL_EPSILON ? matM[1][0] : 0; - c = std::abs(matM[1][0]) > DBL_EPSILON ? matM[1][1] : 1; - z = 1./std::sqrt(c * c + s * s); - c *= z; - s *= z; - - double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} }; - CvMat Qz = cvMat(3, 3, CV_64F, _Qz); - - cvMatMul(&M, &Qz, &R); - CV_DbgAssert(fabs(matR[1][0]) < FLT_EPSILON); - matR[1][0] = 0; - - // Solve the decomposition ambiguity. - // Diagonal entries of R, except the last one, shall be positive. - // Further rotate R by 180 degree if necessary - if( matR[0][0] < 0 ) - { - if( matR[1][1] < 0 ) - { - // rotate around z for 180 degree, i.e. a rotation matrix of - // [-1, 0, 0], - // [ 0, -1, 0], - // [ 0, 0, 1] - matR[0][0] *= -1; - matR[0][1] *= -1; - matR[1][1] *= -1; - - _Qz[0][0] *= -1; - _Qz[0][1] *= -1; - _Qz[1][0] *= -1; - _Qz[1][1] *= -1; - } - else - { - // rotate around y for 180 degree, i.e. a rotation matrix of - // [-1, 0, 0], - // [ 0, 1, 0], - // [ 0, 0, -1] - matR[0][0] *= -1; - matR[0][2] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; - - cvTranspose( &Qz, &Qz ); - - _Qy[0][0] *= -1; - _Qy[0][2] *= -1; - _Qy[2][0] *= -1; - _Qy[2][2] *= -1; - } - } - else if( matR[1][1] < 0 ) - { - // ??? for some reason, we never get here ??? - - // rotate around x for 180 degree, i.e. a rotation matrix of - // [ 1, 0, 0], - // [ 0, -1, 0], - // [ 0, 0, -1] - matR[0][1] *= -1; - matR[0][2] *= -1; - matR[1][1] *= -1; - matR[1][2] *= -1; - matR[2][2] *= -1; - - cvTranspose( &Qz, &Qz ); - cvTranspose( &Qy, &Qy ); - - _Qx[1][1] *= -1; - _Qx[1][2] *= -1; - _Qx[2][1] *= -1; - _Qx[2][2] *= -1; - } - - // calculate the euler angle - if( eulerAngles ) - { - eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI); - eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); - } - - /* Calculate orthogonal matrix. */ - /* - Q = QzT * QyT * QxT - */ - cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T ); - cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T ); - - /* Save R and Q matrices. */ - cvConvert( &R, matrixR ); - cvConvert( &Q, matrixQ ); - - if( matrixQx ) - cvConvert(&Qx, matrixQx); - if( matrixQy ) - cvConvert(&Qy, matrixQy); - if( matrixQz ) - cvConvert(&Qz, matrixQz); -} - - -CV_IMPL void -cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, - CvMat *rotMatr, CvMat *posVect, - CvMat *rotMatrX, CvMat *rotMatrY, - CvMat *rotMatrZ, CvPoint3D64f *eulerAngles) -{ - double tmpProjMatrData[16], tmpMatrixDData[16], tmpMatrixVData[16]; - CvMat tmpProjMatr = cvMat(4, 4, CV_64F, tmpProjMatrData); - CvMat tmpMatrixD = cvMat(4, 4, CV_64F, tmpMatrixDData); - CvMat tmpMatrixV = cvMat(4, 4, CV_64F, tmpMatrixVData); - CvMat tmpMatrixM; - - /* Validate parameters. */ - if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0) - CV_Error(cv::Error::StsNullPtr, "Some of parameters is a NULL pointer!"); - - if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect)) - CV_Error(cv::Error::StsUnsupportedFormat, "Input parameters must be matrices!"); - - if(projMatr->cols != 4 || projMatr->rows != 3) - CV_Error(cv::Error::StsUnmatchedSizes, "Size of projection matrix must be 3x4!"); - - if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3) - CV_Error(cv::Error::StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!"); - - if(posVect->cols != 1 || posVect->rows != 4) - CV_Error(cv::Error::StsUnmatchedSizes, "Size of position vector must be 4x1!"); - - /* Compute position vector. */ - cvSetZero(&tmpProjMatr); // Add zero row to make matrix square. - int i, k; - for(i = 0; i < 3; i++) - for(k = 0; k < 4; k++) - cvmSet(&tmpProjMatr, i, k, cvmGet(projMatr, i, k)); - - cvSVD(&tmpProjMatr, &tmpMatrixD, NULL, &tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T); - - /* Save position vector. */ - for(i = 0; i < 4; i++) - cvmSet(posVect, i, 0, cvmGet(&tmpMatrixV, 3, i)); // Solution is last row of V. - - /* Compute calibration and rotation matrices via RQ decomposition. */ - cvGetCols(projMatr, &tmpMatrixM, 0, 3); // M is first square matrix of P. - - CV_Assert(cvDet(&tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0]. - - cvRQDecomp3x3(&tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles); -} - - namespace cv { @@ -3633,144 +2029,6 @@ static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize = 14) } // namespace cv - -void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian) -{ - CV_INSTRUMENT_REGION(); - - Mat src = _src.getMat(); - const Size srcSz = src.size(); - CV_Check(srcSz, srcSz == Size(3, 1) || srcSz == Size(1, 3) || - (srcSz == Size(1, 1) && src.channels() == 3) || - srcSz == Size(3, 3), - "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); - - bool v2m = src.cols == 1 || src.rows == 1; - _dst.create(3, v2m ? 3 : 1, src.depth()); - Mat dst = _dst.getMat(); - CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _cjacobian; - if( _jacobian.needed() ) - { - _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth()); - _cjacobian = cvMat(_jacobian.getMat()); - } - bool ok = cvRodrigues2(&_csrc, &_cdst, _jacobian.needed() ? &_cjacobian : 0) > 0; - if( !ok ) - dst = Scalar(0); -} - -void cv::matMulDeriv( InputArray _Amat, InputArray _Bmat, - OutputArray _dABdA, OutputArray _dABdB ) -{ - CV_INSTRUMENT_REGION(); - - Mat A = _Amat.getMat(), B = _Bmat.getMat(); - _dABdA.create(A.rows*B.cols, A.rows*A.cols, A.type()); - _dABdB.create(A.rows*B.cols, B.rows*B.cols, A.type()); - Mat dABdA = _dABdA.getMat(), dABdB = _dABdB.getMat(); - CvMat matA = cvMat(A), matB = cvMat(B), c_dABdA = cvMat(dABdA), c_dABdB = cvMat(dABdB); - cvCalcMatMulDeriv(&matA, &matB, &c_dABdA, &c_dABdB); -} - - -void cv::composeRT( InputArray _rvec1, InputArray _tvec1, - InputArray _rvec2, InputArray _tvec2, - OutputArray _rvec3, OutputArray _tvec3, - OutputArray _dr3dr1, OutputArray _dr3dt1, - OutputArray _dr3dr2, OutputArray _dr3dt2, - OutputArray _dt3dr1, OutputArray _dt3dt1, - OutputArray _dt3dr2, OutputArray _dt3dt2 ) -{ - Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat(); - Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat(); - int rtype = rvec1.type(); - _rvec3.create(rvec1.size(), rtype); - _tvec3.create(tvec1.size(), rtype); - Mat rvec3 = _rvec3.getMat(), tvec3 = _tvec3.getMat(); - - CvMat c_rvec1 = cvMat(rvec1), c_tvec1 = cvMat(tvec1), c_rvec2 = cvMat(rvec2), - c_tvec2 = cvMat(tvec2), c_rvec3 = cvMat(rvec3), c_tvec3 = cvMat(tvec3); - CvMat c_dr3dr1, c_dr3dt1, c_dr3dr2, c_dr3dt2, c_dt3dr1, c_dt3dt1, c_dt3dr2, c_dt3dt2; - CvMat *p_dr3dr1=0, *p_dr3dt1=0, *p_dr3dr2=0, *p_dr3dt2=0, *p_dt3dr1=0, *p_dt3dt1=0, *p_dt3dr2=0, *p_dt3dt2=0; -#define CV_COMPOSE_RT_PARAM(name) \ - Mat name; \ - if (_ ## name.needed())\ - { \ - _ ## name.create(3, 3, rtype); \ - name = _ ## name.getMat(); \ - p_ ## name = &(c_ ## name = cvMat(name)); \ - } - - CV_COMPOSE_RT_PARAM(dr3dr1); CV_COMPOSE_RT_PARAM(dr3dt1); - CV_COMPOSE_RT_PARAM(dr3dr2); CV_COMPOSE_RT_PARAM(dr3dt2); - CV_COMPOSE_RT_PARAM(dt3dr1); CV_COMPOSE_RT_PARAM(dt3dt1); - CV_COMPOSE_RT_PARAM(dt3dr2); CV_COMPOSE_RT_PARAM(dt3dt2); -#undef CV_COMPOSE_RT_PARAM - - cvComposeRT(&c_rvec1, &c_tvec1, &c_rvec2, &c_tvec2, &c_rvec3, &c_tvec3, - p_dr3dr1, p_dr3dt1, p_dr3dr2, p_dr3dt2, - p_dt3dr1, p_dt3dt1, p_dt3dr2, p_dt3dt2); -} - - -void cv::projectPoints( InputArray _opoints, - InputArray _rvec, - InputArray _tvec, - InputArray _cameraMatrix, - InputArray _distCoeffs, - OutputArray _ipoints, - OutputArray _jacobian, - double aspectRatio ) -{ - Mat opoints = _opoints.getMat(); - int npoints = opoints.checkVector(3), depth = opoints.depth(); - if (npoints < 0) - opoints = opoints.t(); - npoints = opoints.checkVector(3); - CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F)); - - if (opoints.cols == 3) - opoints = opoints.reshape(3); - - CvMat dpdrot, dpdt, dpdf, dpdc, dpddist; - CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0; - - CV_Assert( _ipoints.needed() ); - - _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true); - Mat imagePoints = _ipoints.getMat(); - CvMat c_imagePoints = cvMat(imagePoints); - CvMat c_objectPoints = cvMat(opoints); - Mat cameraMatrix = _cameraMatrix.getMat(); - - Mat rvec = _rvec.getMat(), tvec = _tvec.getMat(); - CvMat c_cameraMatrix = cvMat(cameraMatrix); - CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec); - - double dc0buf[5]={0}; - Mat dc0(5,1,CV_64F,dc0buf); - Mat distCoeffs = _distCoeffs.getMat(); - if( distCoeffs.empty() ) - distCoeffs = dc0; - CvMat c_distCoeffs = cvMat(distCoeffs); - int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1; - - Mat jacobian; - if( _jacobian.needed() ) - { - _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F); - jacobian = _jacobian.getMat(); - pdpdrot = &(dpdrot = cvMat(jacobian.colRange(0, 3))); - pdpdt = &(dpdt = cvMat(jacobian.colRange(3, 6))); - pdpdf = &(dpdf = cvMat(jacobian.colRange(6, 8))); - pdpdc = &(dpdc = cvMat(jacobian.colRange(8, 10))); - pdpddist = &(dpddist = cvMat(jacobian.colRange(10, 10+ndistCoeffs))); - } - - cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs, - &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio ); -} - cv::Mat cv::initCameraMatrix2D( InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, Size imageSize, double aspectRatio ) @@ -4244,108 +2502,6 @@ bool cv::stereoRectifyUncalibrated( InputArray _points1, InputArray _points2, return cvStereoRectifyUncalibrated(&c_pt1, &c_pt2, p_F, cvSize(imgSize), &c_H1, &c_H2, threshold) > 0; } -cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix, - InputArray _distCoeffs, - Size imgSize, double alpha, Size newImgSize, - Rect* validPixROI, bool centerPrincipalPoint ) -{ - CV_INSTRUMENT_REGION(); - - Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); - CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs); - - Mat newCameraMatrix(3, 3, CV_MAT_TYPE(c_cameraMatrix.type)); - CvMat c_newCameraMatrix = cvMat(newCameraMatrix); - - cvGetOptimalNewCameraMatrix(&c_cameraMatrix, &c_distCoeffs, cvSize(imgSize), - alpha, &c_newCameraMatrix, - cvSize(newImgSize), (CvRect*)validPixROI, (int)centerPrincipalPoint); - return newCameraMatrix; -} - - -cv::Vec3d cv::RQDecomp3x3( InputArray _Mmat, - OutputArray _Rmat, - OutputArray _Qmat, - OutputArray _Qx, - OutputArray _Qy, - OutputArray _Qz ) -{ - CV_INSTRUMENT_REGION(); - - Mat M = _Mmat.getMat(); - _Rmat.create(3, 3, M.type()); - _Qmat.create(3, 3, M.type()); - Mat Rmat = _Rmat.getMat(); - Mat Qmat = _Qmat.getMat(); - Vec3d eulerAngles; - - CvMat matM = cvMat(M), matR = cvMat(Rmat), matQ = cvMat(Qmat); -#define CV_RQDecomp3x3_PARAM(name) \ - Mat name; \ - CvMat c_ ## name, *p ## name = NULL; \ - if( _ ## name.needed() ) \ - { \ - _ ## name.create(3, 3, M.type()); \ - name = _ ## name.getMat(); \ - c_ ## name = cvMat(name); p ## name = &c_ ## name; \ - } - - CV_RQDecomp3x3_PARAM(Qx); - CV_RQDecomp3x3_PARAM(Qy); - CV_RQDecomp3x3_PARAM(Qz); -#undef CV_RQDecomp3x3_PARAM - cvRQDecomp3x3(&matM, &matR, &matQ, pQx, pQy, pQz, (CvPoint3D64f*)&eulerAngles[0]); - return eulerAngles; -} - - -void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix, - OutputArray _rotMatrix, OutputArray _transVect, - OutputArray _rotMatrixX, OutputArray _rotMatrixY, - OutputArray _rotMatrixZ, OutputArray _eulerAngles ) -{ - CV_INSTRUMENT_REGION(); - - Mat projMatrix = _projMatrix.getMat(); - int type = projMatrix.type(); - _cameraMatrix.create(3, 3, type); - _rotMatrix.create(3, 3, type); - _transVect.create(4, 1, type); - Mat cameraMatrix = _cameraMatrix.getMat(); - Mat rotMatrix = _rotMatrix.getMat(); - Mat transVect = _transVect.getMat(); - CvMat c_projMatrix = cvMat(projMatrix), c_cameraMatrix = cvMat(cameraMatrix); - CvMat c_rotMatrix = cvMat(rotMatrix), c_transVect = cvMat(transVect); - CvPoint3D64f *p_eulerAngles = 0; - -#define CV_decomposeProjectionMatrix_PARAM(name) \ - Mat name; \ - CvMat c_ ## name, *p_ ## name = NULL; \ - if( _ ## name.needed() ) \ - { \ - _ ## name.create(3, 3, type); \ - name = _ ## name.getMat(); \ - c_ ## name = cvMat(name); p_ ## name = &c_ ## name; \ - } - - CV_decomposeProjectionMatrix_PARAM(rotMatrixX); - CV_decomposeProjectionMatrix_PARAM(rotMatrixY); - CV_decomposeProjectionMatrix_PARAM(rotMatrixZ); -#undef CV_decomposeProjectionMatrix_PARAM - - if( _eulerAngles.needed() ) - { - _eulerAngles.create(3, 1, CV_64F, -1, true); - p_eulerAngles = _eulerAngles.getMat().ptr(); - } - - cvDecomposeProjectionMatrix(&c_projMatrix, &c_cameraMatrix, &c_rotMatrix, - &c_transVect, p_rotMatrixX, p_rotMatrixY, - p_rotMatrixZ, p_eulerAngles); -} - - namespace cv { diff --git a/modules/calib3d/src/calibration_base.cpp b/modules/calib3d/src/calibration_base.cpp new file mode 100644 index 0000000000..c047a45e8e --- /dev/null +++ b/modules/calib3d/src/calibration_base.cpp @@ -0,0 +1,1644 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "precomp.hpp" +#include "hal_replacement.hpp" +#include "distortion_model.hpp" +#include "calib3d_c_api.h" +#include +#include + +using namespace cv; + +/* + This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet + that is (in a large extent) based on the paper: + Z. Zhang. "A flexible new technique for camera calibration". + IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. + The 1st initial port was done by Valery Mosyagin. +*/ + +// reimplementation of dAB.m +void cv::matMulDeriv( InputArray A_, InputArray B_, OutputArray dABdA_, OutputArray dABdB_ ) +{ + CV_INSTRUMENT_REGION(); + + Mat A = A_.getMat(), B = B_.getMat(); + int type = A.type(); + CV_Assert(type == B.type()); + CV_Assert(type == CV_32F || type == CV_64F); + CV_Assert(A.cols == B.rows); + + dABdA_.create(A.rows*B.cols, A.rows*A.cols, type); + dABdB_.create(A.rows*B.cols, B.rows*B.cols, type); + Mat dABdA = dABdA_.getMat(), dABdB = dABdB_.getMat(); + + int M = A.rows, L = A.cols, N = B.cols; + int bstep = (int)(B.step/B.elemSize()); + + if( type == CV_32F ) + { + for( int i = 0; i < M*N; i++ ) + { + int j, i1 = i / N, i2 = i % N; + + const float* a = A.ptr(i1); + const float* b = B.ptr() + i2; + float* dcda = dABdA.ptr(i); + float* dcdb = dABdB.ptr(i); + + memset(dcda, 0, M*L*sizeof(dcda[0])); + memset(dcdb, 0, L*N*sizeof(dcdb[0])); + + for( j = 0; j < L; j++ ) + { + dcda[i1*L + j] = b[j*bstep]; + dcdb[j*N + i2] = a[j]; + } + } + } + else + { + for( int i = 0; i < M*N; i++ ) + { + int j, i1 = i / N, i2 = i % N; + + const double* a = A.ptr(i1); + const double* b = B.ptr() + i2; + double* dcda = dABdA.ptr(i); + double* dcdb = dABdB.ptr(i); + + memset(dcda, 0, M*L*sizeof(dcda[0])); + memset(dcdb, 0, L*N*sizeof(dcdb[0])); + + for( j = 0; j < L; j++ ) + { + dcda[i1*L + j] = b[j*bstep]; + dcdb[j*N + i2] = a[j]; + } + } + } +} + +void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian) +{ + CV_INSTRUMENT_REGION(); + + Mat src = _src.getMat(); + const Size srcSz = src.size(); + int srccn = src.channels(); + int depth = src.depth(); + CV_Check(srcSz, ((srcSz == Size(3, 1) || srcSz == Size(1, 3)) && srccn == 1) || + (srcSz == Size(1, 1) && srccn == 3) || + (srcSz == Size(3, 3) && srccn == 1), + "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); + + bool v2m = src.cols == 1 || src.rows == 1; + _dst.create(3, v2m ? 3 : 1, depth); + Mat dst = _dst.getMat(), jacobian; + if( _jacobian.needed() ) + { + _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth()); + jacobian = _jacobian.getMat(); + } + + double J[27] = {0}; + Mat matJ( 3, 9, CV_64F, J); + + dst.setTo(0); + + if( depth != CV_32F && depth != CV_64F ) + CV_Error( cv::Error::StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); + + if( v2m ) + { + int sstep = src.rows > 1 ? (int)src.step1() : 1; + + Point3d r; + if( depth == CV_32F ) + { + const float* sptr = src.ptr(); + r.x = sptr[0]; + r.y = sptr[sstep]; + r.z = sptr[sstep*2]; + } + else + { + const double* sptr = src.ptr(); + r.x = sptr[0]; + r.y = sptr[sstep]; + r.z = sptr[sstep*2]; + } + + double theta = norm(r); + if( theta < DBL_EPSILON ) + { + dst = Mat::eye(3, 3, depth); + if( jacobian.data ) + { + memset( J, 0, sizeof(J) ); + J[5] = J[15] = J[19] = -1; + J[7] = J[11] = J[21] = 1; + } + } + else + { + double c = std::cos(theta); + double s = std::sin(theta); + double c1 = 1. - c; + double itheta = theta ? 1./theta : 0.; + + r *= itheta; + + Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z ); + Matx33d r_x( 0, -r.z, r.y, + r.z, 0, -r.x, + -r.y, r.x, 0 ); + + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; + Mat(R).convertTo(dst, depth); + + if( jacobian.data ) + { + const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0, + 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0, + 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z }; + double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, + 0, 0, 1, 0, 0, 0, -1, 0, 0, + 0, -1, 0, 1, 0, 0, 0, 0, 0 }; + for( int i = 0; i < 3; i++ ) + { + double ri = i == 0 ? r.x : i == 1 ? r.y : r.z; + double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; + double a3 = (c - s*itheta)*ri, a4 = s*itheta; + for( int k = 0; k < 9; k++ ) + J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] + + a3*r_x.val[k] + a4*d_r_x_[i*9+k]; + } + } + } + } + else + { + Matx33d U, Vt; + Vec3d W; + double theta, s, c; + int dstep = dst.rows > 1 ? (int)dst.step1() : 1; + + Matx33d R; + src.convertTo(R, CV_64F); + + if( !checkRange(R, true, NULL, -100, 100) ) + { + dst.setTo(0); + if (jacobian.data) + jacobian.setTo(0); + return; + } + + SVD::compute(R, W, U, Vt); + R = U*Vt; + + Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + + s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25); + c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5; + c = c > 1. ? 1. : c < -1. ? -1. : c; + theta = std::acos(c); + + if( s < 1e-5 ) + { + double t; + + if( c > 0 ) + r = Point3d(0, 0, 0); + else + { + t = (R(0, 0) + 1)*0.5; + r.x = std::sqrt(MAX(t,0.)); + t = (R(1, 1) + 1)*0.5; + r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.); + t = (R(2, 2) + 1)*0.5; + r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.); + if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) ) + r.z = -r.z; + theta /= norm(r); + r *= theta; + } + + if( jacobian.data ) + { + memset( J, 0, sizeof(J) ); + if( c > 0 ) + { + J[5] = J[15] = J[19] = -0.5; + J[7] = J[11] = J[21] = 0.5; + } + } + } + else + { + double vth = 1/(2*s); + + if( jacobian.data ) + { + double t, dtheta_dtr = -1./s; + // var1 = [vth;theta] + // var = [om1;var1] = [om1;vth;theta] + double dvth_dtheta = -vth*c/s; + double d1 = 0.5*dvth_dtheta*dtheta_dtr; + double d2 = 0.5*dtheta_dtr; + // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR + double dvardR[5*9] = + { + 0, 0, 0, 0, 0, 1, 0, -1, 0, + 0, 0, -1, 0, 0, 0, 1, 0, 0, + 0, 1, 0, -1, 0, 0, 0, 0, 0, + d1, 0, 0, 0, d1, 0, 0, 0, d1, + d2, 0, 0, 0, d2, 0, 0, 0, d2 + }; + // var2 = [om;theta] + double dvar2dvar[] = + { + vth, 0, 0, r.x, 0, + 0, vth, 0, r.y, 0, + 0, 0, vth, r.z, 0, + 0, 0, 0, 0, 1 + }; + double domegadvar2[] = + { + theta, 0, 0, r.x*vth, + 0, theta, 0, r.y*vth, + 0, 0, theta, r.z*vth + }; + + Mat _dvardR( 5, 9, CV_64FC1, dvardR ); + Mat _dvar2dvar( 4, 5, CV_64FC1, dvar2dvar ); + Mat _domegadvar2( 3, 4, CV_64FC1, domegadvar2 ); + double t0[3*5]; + Mat _t0( 3, 5, CV_64FC1, t0 ); + + gemm(_domegadvar2, _dvar2dvar, 1, noArray(), 0, _t0); + gemm(_t0, _dvardR, 1, noArray(), 0, matJ); + CV_Assert(matJ.ptr() == J); + + // transpose every row of matJ (treat the rows as 3x3 matrices) + CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); + CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t); + CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t); + } + + vth *= theta; + r *= vth; + } + + if( depth == CV_32F ) + { + float* dptr = dst.ptr(); + dptr[0] = (float)r.x; + dptr[dstep] = (float)r.y; + dptr[dstep*2] = (float)r.z; + } + else + { + double* dptr = dst.ptr(); + dptr[0] = r.x; + dptr[dstep] = r.y; + dptr[dstep*2] = r.z; + } + } + + if( jacobian.data ) + { + if( depth == CV_32F ) + { + if( jacobian.rows == matJ.rows ) + matJ.convertTo(jacobian, CV_32F); + else + { + float Jf[3*9]; + Mat _Jf( matJ.rows, matJ.cols, CV_32FC1, Jf ); + matJ.convertTo(_Jf, CV_32F); + transpose(_Jf, jacobian); + } + } + else if( jacobian.rows == matJ.rows ) + matJ.copyTo(jacobian); + else + transpose(matJ, jacobian); + } +} + +// reimplementation of compose_motion.m +void cv::composeRT( InputArray _rvec1, InputArray _tvec1, + InputArray _rvec2, InputArray _tvec2, + OutputArray _rvec3, OutputArray _tvec3, + OutputArray _dr3dr1, OutputArray _dr3dt1, + OutputArray _dr3dr2, OutputArray _dr3dt2, + OutputArray _dt3dr1, OutputArray _dt3dt1, + OutputArray _dt3dr2, OutputArray _dt3dt2 ) +{ + Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat(); + Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat(); + int rtype = rvec1.type(); + + CV_Assert(rtype == CV_32F || rtype == CV_64F); + Size rsz = rvec1.size(); + CV_Assert(rsz == Size(3, 1) || rsz == Size(1, 3)); + CV_Assert(rsz == rvec2.size() && rsz == tvec1.size() && rsz == tvec2.size()); + + Mat dr3dr1, dr3dt1, dr3dr2, dr3dt2; + Mat dt3dr1, dt3dt1, dt3dr2, dt3dt2; + if(_dr3dr1.needed()) { + _dr3dr1.create(3, 3, rtype); + dr3dr1 = _dr3dr1.getMat(); + } + if(_dr3dt1.needed()) { + _dr3dt1.create(3, 3, rtype); + dr3dt1 = _dr3dt1.getMat(); + } + if(_dr3dr2.needed()) { + _dr3dr2.create(3, 3, rtype); + dr3dr2 = _dr3dr2.getMat(); + } + if(_dr3dt2.needed()) { + _dr3dt2.create(3, 3, rtype); + dr3dt2 = _dr3dt2.getMat(); + } + if(_dt3dr1.needed()) { + _dt3dr1.create(3, 3, rtype); + dt3dr1 = _dt3dr1.getMat(); + } + if(_dt3dt1.needed()) { + _dt3dt1.create(3, 3, rtype); + dt3dt1 = _dt3dt1.getMat(); + } + if(_dt3dr2.needed()) { + _dt3dr2.create(3, 3, rtype); + dt3dr2 = _dt3dr2.getMat(); + } + if(_dt3dt2.needed()) { + _dt3dt2.create(3, 3, rtype); + dt3dt2 = _dt3dt2.getMat(); + } + + double _r1[3], _r2[3]; + double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; + Mat r1(rsz,CV_64F,_r1), r2(rsz,CV_64F,_r2); + Mat R1(3,3,CV_64F,_R1), R2(3,3,CV_64F,_R2); + Mat dR1dr1(3,9,CV_64F,_d1), dR2dr2(3,9,CV_64F,_d2); + + rvec1.convertTo(r1, CV_64F); + rvec2.convertTo(r2, CV_64F); + + Rodrigues(r1, R1, dR1dr1); + Rodrigues(r2, R2, dR2dr2); + CV_Assert(dR1dr1.ptr() == _d1); + CV_Assert(dR2dr2.ptr() == _d2); + + double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; + double _W1[9*3], _W2[3*3]; + Mat r3(3,1,CV_64F,_r3), R3(3,3,CV_64F,_R3); + Mat dR3dR1(9,9,CV_64F,_dR3dR1), dR3dR2(9,9,CV_64F,_dR3dR2); + Mat dr3dR3(9,3,CV_64F,_dr3dR3); + Mat W1(3,9,CV_64F,_W1), W2(3,3,CV_64F,_W2); + + R3 = R2*R1; + matMulDeriv(R2, R1, dR3dR2, dR3dR1); + Rodrigues(R3, r3, dr3dR3); + CV_Assert(dr3dR3.ptr() == _dr3dR3); + + r3.convertTo(_rvec3, rtype); + + if( dr3dr1.data ) + { + gemm(dr3dR3, dR3dR1, 1, noArray(), 0, W1, GEMM_1_T); + gemm(W1, dR1dr1, 1, noArray(), 0, W2, GEMM_2_T); + W2.convertTo(dr3dr1, rtype); + } + + if( dr3dr2.data ) + { + gemm(dr3dR3, dR3dR2, 1, noArray(), 0, W1, GEMM_1_T); + gemm(W1, dR2dr2, 1, noArray(), 0, W2, GEMM_2_T); + W2.convertTo(dr3dr2, rtype); + } + + if( dr3dt1.data ) + dr3dt1.setTo(0); + if( dr3dt2.data ) + dr3dt2.setTo(0); + + double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; + Mat t1(3,1,CV_64F,_t1), t2(3,1,CV_64F,_t2); + Mat t3(3,1,CV_64F,_t3); + Mat dxdR2(3, 9, CV_64F, _dxdR2); + Mat dxdt1(3, 3, CV_64F, _dxdt1); + Mat W3(3, 3, CV_64F, _W3); + + tvec1.convertTo(t1, CV_64F); + tvec2.convertTo(t2, CV_64F); + gemm(R2, t1, 1, t2, 1, t3); + t3.convertTo(_tvec3, rtype); + + if( dt3dr2.data || dt3dt1.data ) + { + matMulDeriv(R2, t1, dxdR2, dxdt1); + if( dt3dr2.data ) + { + gemm(dxdR2, dR2dr2, 1, noArray(), 0, W3, GEMM_2_T); + W3.convertTo(dt3dr2, rtype); + } + if( dt3dt1.data ) + dxdt1.convertTo(dt3dt1, rtype); + } + + if( dt3dt2.data ) + setIdentity(dt3dt2); + if( dt3dr1.data ) + dt3dr1.setTo(0); +} + +static const char* cvDistCoeffErr = + "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; + +void cv::projectPoints( InputArray _objectPoints, + InputArray _rvec, InputArray _tvec, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _imagePoints, OutputArray _dpdr, + OutputArray _dpdt, OutputArray _dpdf, + OutputArray _dpdc, OutputArray _dpdk, + OutputArray _dpdo, double aspectRatio) +{ + Mat _m, objectPoints = _objectPoints.getMat(); + Mat dpdr, dpdt, dpdc, dpdf, dpdk, dpdo; + + int i, j; + double R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; + Matx33d matTilt = Matx33d::eye(); + Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0); + Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0); + Mat matR( 3, 3, CV_64F, R ), _dRdr( 3, 9, CV_64F, dRdr ); + double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; + double* dpdo_p = 0; + int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; + int dpdo_step = 0; + bool fixedAspectRatio = aspectRatio > FLT_EPSILON; + + int objpt_depth = objectPoints.depth(); + int objpt_cn = objectPoints.channels(); + int total = (int)(objectPoints.total()*objectPoints.channels()); + int count = total / 3; + if(total % 3 != 0) + { + //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data + CV_Error( cv::Error::StsBadArg, "Homogeneous coordinates are not supported" ); + } + count = total / 3; + CV_Assert(objpt_depth == CV_32F || objpt_depth == CV_64F); + CV_Assert((objectPoints.rows == 1 && objpt_cn == 3) || + (objectPoints.rows == count && objpt_cn*objectPoints.cols == 3) || + (objectPoints.rows == 3 && objpt_cn == 1 && objectPoints.cols == count)); + + if (objectPoints.rows == 3 && objectPoints.cols == count) { + Mat temp; + transpose(objectPoints, temp); + objectPoints = temp; + } + + CV_Assert( _imagePoints.needed() ); + _imagePoints.create(count, 1, CV_MAKETYPE(objpt_depth, 2), -1, true); + Mat ipoints = _imagePoints.getMat(); + + Mat rvec = _rvec.getMat(), tvec = _tvec.getMat(); + if(!((rvec.depth() == CV_32F || rvec.depth() == CV_64F) && + (rvec.size() == Size(3, 3) || + (rvec.rows == 1 && rvec.cols*rvec.channels() == 3) || + (rvec.rows == 3 && rvec.cols*rvec.channels() == 1)))) { + CV_Error(cv::Error::StsBadArg, "rvec must be 3x3 or 1x3 or 3x1 floating-point array"); + } + + if( rvec.size() == Size(3, 3) ) + { + rvec.convertTo(matR, CV_64F); + Vec3d rvec_d; + Rodrigues(matR, rvec_d); + Rodrigues(rvec_d, matR, _dRdr); + rvec.convertTo(matR, CV_64F); + } + else + { + double r[3]; + Mat _r(rvec.size(), CV_64FC(rvec.channels()), r); + rvec.convertTo(_r, CV_64F); + Rodrigues(_r, matR, _dRdr); + } + + if(!((tvec.depth() == CV_32F || tvec.depth() == CV_64F) && + ((tvec.rows == 1 && tvec.cols*tvec.channels() == 3) || + (tvec.rows == 3 && tvec.cols*tvec.channels() == 1)))) { + CV_Error(cv::Error::StsBadArg, "tvec must be 1x3 or 3x1 floating-point array"); + } + + Mat _t(tvec.size(), CV_64FC(tvec.channels()), t); + tvec.convertTo(_t, CV_64F); + + Mat cameraMatrix = _cameraMatrix.getMat(); + + if(cameraMatrix.size() != Size(3, 3) || cameraMatrix.channels() != 1) + CV_Error( cv::Error::StsBadArg, "Intrinsic parameters must be 3x3 floating-point matrix" ); + Mat _a(3, 3, CV_64F, a); + cameraMatrix.convertTo(_a, CV_64F); + + fx = a[0]; fy = a[4]; + cx = a[2]; cy = a[5]; + + if( fixedAspectRatio ) + fx = fy*aspectRatio; + + Mat distCoeffs = _distCoeffs.getMat(); + int ktotal = 0; + if( distCoeffs.data ) + { + int kcn = distCoeffs.channels(); + ktotal = (int)distCoeffs.total()*kcn; + if( (distCoeffs.rows != 1 && distCoeffs.cols != 1) || + (ktotal != 4 && ktotal != 5 && ktotal != 8 && ktotal != 12 && ktotal != 14)) + CV_Error( cv::Error::StsBadArg, cvDistCoeffErr ); + + Mat _k(distCoeffs.size(), CV_64FC(kcn), k); + distCoeffs.convertTo(_k, CV_64F); + if(k[12] != 0 || k[13] != 0) + detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); + } + + if( _dpdr.needed() ) + { + dpdr.create(count*2, 3, CV_64F); + dpdr_p = dpdr.ptr(); + dpdr_step = (int)dpdr.step1(); + } + if( _dpdt.needed() ) + { + dpdt.create(count*2, 3, CV_64F); + dpdt_p = dpdt.ptr(); + dpdt_step = (int)dpdt.step1(); + } + if( _dpdf.needed() ) + { + dpdf.create(count*2, 2, CV_64F); + dpdf_p = dpdf.ptr(); + dpdf_step = (int)dpdf.step1(); + } + if( _dpdc.needed() ) + { + dpdc.create(count*2, 2, CV_64F); + dpdc_p = dpdc.ptr(); + dpdc_step = (int)dpdc.step1(); + } + if( _dpdk.needed() ) + { + dpdk.create(count*2, ktotal, CV_64F); + dpdk_p = dpdk.ptr(); + dpdk_step = (int)dpdk.step1(); + } + if( _dpdo.needed() ) + { + dpdo = Mat::zeros(count*2, count*3, CV_64F); + dpdo_p = dpdo.ptr(); + dpdo_step = (int)dpdo.step1(); + } + + bool calc_derivatives = dpdr.data || dpdt.data || dpdf.data || + dpdc.data || dpdk.data || dpdo.data; + + if (!calc_derivatives) + { + if (objpt_depth == CV_32F && ipoints.type() == CV_32F) + { + float rtMatrix[12] = { (float)R[0], (float)R[1], (float)R[2], (float)t[0], + (float)R[3], (float)R[4], (float)R[5], (float)t[1], + (float)R[6], (float)R[7], (float)R[8], (float)t[2] }; + + cv_camera_intrinsics_pinhole_32f intr; + intr.fx = (float)fx; intr.fy = (float)fy; + intr.cx = (float)cx; intr.cy = (float)cy; + intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; + + switch (ktotal) + { + case 0: break; + case 4: // [k_1, k_2, p_1, p_2] + intr.amt_k = 2; intr.amt_p = 2; + break; + case 5: // [k_1, k_2, p_1, p_2, k_3] + intr.amt_k = 3; intr.amt_p = 2; + break; + case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] + intr.amt_k = 6; intr.amt_p = 2; + break; + case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; + break; + case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; + break; + default: + CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); + } + + intr.k[0] = (float)k[0]; + intr.k[1] = (float)k[1]; + intr.k[2] = (float)k[4]; + intr.k[3] = (float)k[5]; + intr.k[4] = (float)k[6]; + intr.k[5] = (float)k[7]; + + intr.p[0] = (float)k[2]; + intr.p[1] = (float)k[3]; + + for (int ctr = 0; ctr < 4; ctr++) + { + intr.s[ctr] = (float)k[8+ctr]; + } + + intr.tau_x = (float)k[12]; + intr.tau_y = (float)k[13]; + + CALL_HAL(projectPoints, cv_hal_project_points_pinhole32f, + (float*)objectPoints.data, objectPoints.step, count, + (float*)ipoints.data, ipoints.step, rtMatrix, &intr); + } + + if (objpt_depth == CV_64F && ipoints.type() == CV_64F) + { + double rtMatrix[12] = { R[0], R[1], R[2], t[0], + R[3], R[4], R[5], t[1], + R[6], R[7], R[8], t[2] }; + + cv_camera_intrinsics_pinhole_64f intr; + intr.fx = fx; intr.fy = fy; + intr.cx = cx; intr.cy = cy; + intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; + + switch (ktotal) + { + case 0: break; + case 4: // [k_1, k_2, p_1, p_2] + intr.amt_k = 2; intr.amt_p = 2; + break; + case 5: // [k_1, k_2, p_1, p_2, k_3] + intr.amt_k = 3; intr.amt_p = 2; + break; + case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] + intr.amt_k = 6; intr.amt_p = 2; + break; + case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; + break; + case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; + break; + default: + CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); + } + + intr.k[0] = k[0]; + intr.k[1] = k[1]; + intr.k[2] = k[4]; + intr.k[3] = k[5]; + intr.k[4] = k[6]; + intr.k[5] = k[7]; + + intr.p[0] = k[2]; + intr.p[1] = k[3]; + + for (int ctr = 0; ctr < 4; ctr++) + { + intr.s[ctr] = k[8+ctr]; + } + + intr.tau_x = k[12]; + intr.tau_y = k[13]; + + CALL_HAL(projectPoints, cv_hal_project_points_pinhole64f, + (double*)objectPoints.data, objectPoints.step, count, + (double*)ipoints.data, ipoints.step, rtMatrix, &intr); + } + } + + Mat matM(objectPoints.size(), CV_64FC(objpt_cn)); + objectPoints.convertTo(matM, CV_64F); + ipoints.convertTo(_m, CV_64F); + const Point3d* M = matM.ptr(); + Point2d* m = _m.ptr(); + + for( i = 0; i < count; i++ ) + { + double X = M[i].x, Y = M[i].y, Z = M[i].z; + double x = R[0]*X + R[1]*Y + R[2]*Z + t[0]; + double y = R[3]*X + R[4]*Y + R[5]*Z + t[1]; + double z = R[6]*X + R[7]*Y + R[8]*Z + t[2]; + double r2, r4, r6, a1, a2, a3, cdist, icdist2; + double xd, yd, xd0, yd0, invProj; + Vec3d vecTilt; + Vec3d dVecTilt; + Matx22d dMatTilt; + Vec2d dXdYd; + + double z0 = z; + z = z ? 1./z : 1; + x *= z; y *= z; + + r2 = x*x + y*y; + r4 = r2*r2; + r6 = r4*r2; + a1 = 2*x*y; + a2 = r2 + 2*x*x; + a3 = r2 + 2*y*y; + cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; + icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6); + xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4; + yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4; + + // additional distortion by projecting onto a tilt plane + vecTilt = matTilt*Vec3d(xd0, yd0, 1); + invProj = vecTilt(2) ? 1./vecTilt(2) : 1; + xd = invProj * vecTilt(0); + yd = invProj * vecTilt(1); + + m[i].x = xd*fx + cx; + m[i].y = yd*fy + cy; + + if( calc_derivatives ) + { + if( dpdc.data ) + { + dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y + dpdc_p[dpdc_step] = 0; + dpdc_p[dpdc_step+1] = 1; + dpdc_p += dpdc_step*2; + } + + if( dpdf_p ) + { + if( fixedAspectRatio ) + { + dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y + dpdf_p[dpdf_step] = 0; + dpdf_p[dpdf_step+1] = yd; + } + else + { + dpdf_p[0] = xd; dpdf_p[1] = 0; + dpdf_p[dpdf_step] = 0; + dpdf_p[dpdf_step+1] = yd; + } + dpdf_p += dpdf_step*2; + } + for (int row = 0; row < 2; ++row) + for (int col = 0; col < 2; ++col) + dMatTilt(row,col) = matTilt(row,col)*vecTilt(2) - matTilt(2,col)*vecTilt(row); + double invProjSquare = (invProj*invProj); + dMatTilt *= invProjSquare; + if( dpdk_p ) + { + dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2); + dpdk_p[0] = fx*dXdYd(0); + dpdk_p[dpdk_step] = fy*dXdYd(1); + dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4); + dpdk_p[1] = fx*dXdYd(0); + dpdk_p[dpdk_step+1] = fy*dXdYd(1); + if( dpdk.cols > 2 ) + { + dXdYd = dMatTilt*Vec2d(a1, a3); + dpdk_p[2] = fx*dXdYd(0); + dpdk_p[dpdk_step+2] = fy*dXdYd(1); + dXdYd = dMatTilt*Vec2d(a2, a1); + dpdk_p[3] = fx*dXdYd(0); + dpdk_p[dpdk_step+3] = fy*dXdYd(1); + if( dpdk.cols > 4 ) + { + dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6); + dpdk_p[4] = fx*dXdYd(0); + dpdk_p[dpdk_step+4] = fy*dXdYd(1); + + if( dpdk.cols > 5 ) + { + dXdYd = dMatTilt*Vec2d( + x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2); + dpdk_p[5] = fx*dXdYd(0); + dpdk_p[dpdk_step+5] = fy*dXdYd(1); + dXdYd = dMatTilt*Vec2d( + x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4); + dpdk_p[6] = fx*dXdYd(0); + dpdk_p[dpdk_step+6] = fy*dXdYd(1); + dXdYd = dMatTilt*Vec2d( + x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6); + dpdk_p[7] = fx*dXdYd(0); + dpdk_p[dpdk_step+7] = fy*dXdYd(1); + if( dpdk.cols > 8 ) + { + dXdYd = dMatTilt*Vec2d(r2, 0); + dpdk_p[8] = fx*dXdYd(0); //s1 + dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1 + dXdYd = dMatTilt*Vec2d(r4, 0); + dpdk_p[9] = fx*dXdYd(0); //s2 + dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2 + dXdYd = dMatTilt*Vec2d(0, r2); + dpdk_p[10] = fx*dXdYd(0);//s3 + dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3 + dXdYd = dMatTilt*Vec2d(0, r4); + dpdk_p[11] = fx*dXdYd(0);//s4 + dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4 + if( dpdk.cols > 12 ) + { + dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1); + dpdk_p[12] = fx * invProjSquare * ( + dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); + dpdk_p[dpdk_step+12] = fy*invProjSquare * ( + dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); + dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1); + dpdk_p[13] = fx * invProjSquare * ( + dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0)); + dpdk_p[dpdk_step+13] = fy * invProjSquare * ( + dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1)); + } + } + } + } + } + dpdk_p += dpdk_step*2; + } + + if( dpdt_p ) + { + double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z }; + for( j = 0; j < 3; j++ ) + { + double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j]; + double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt; + double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt); + double da1dt = 2*(x*dydt[j] + y*dxdt[j]); + double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt + + k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt); + double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt + + k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt); + dXdYd = dMatTilt*Vec2d(dmxdt, dmydt); + dpdt_p[j] = fx*dXdYd(0); + dpdt_p[dpdt_step+j] = fy*dXdYd(1); + } + dpdt_p += dpdt_step*2; + } + + if( dpdr_p ) + { + double dx0dr[] = + { + X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2], + X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11], + X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20] + }; + double dy0dr[] = + { + X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5], + X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14], + X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23] + }; + double dz0dr[] = + { + X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8], + X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17], + X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26] + }; + for( j = 0; j < 3; j++ ) + { + double dxdr = z*(dx0dr[j] - x*dz0dr[j]); + double dydr = z*(dy0dr[j] - y*dz0dr[j]); + double dr2dr = 2*x*dxdr + 2*y*dydr; + double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr; + double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr; + double da1dr = 2*(x*dydr + y*dxdr); + double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr + + k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr); + double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr + + k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr); + dXdYd = dMatTilt*Vec2d(dmxdr, dmydr); + dpdr_p[j] = fx*dXdYd(0); + dpdr_p[dpdr_step+j] = fy*dXdYd(1); + } + dpdr_p += dpdr_step*2; + } + + if( dpdo_p ) + { + double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ), + z * ( R[1] - x * z * z0 * R[7] ), + z * ( R[2] - x * z * z0 * R[8] ) }; + double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ), + z * ( R[4] - y * z * z0 * R[7] ), + z * ( R[5] - y * z * z0 * R[8] ) }; + for( j = 0; j < 3; j++ ) + { + double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j]; + double dr4do = 2 * r2 * dr2do; + double dr6do = 3 * r4 * dr2do; + double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j]; + double da2do = dr2do + 4 * x * dxdo[j]; + double da3do = dr2do + 4 * y * dydo[j]; + double dcdist_do + = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do; + double dicdist2_do = -icdist2 * icdist2 + * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do ); + double dxd0_do = cdist * icdist2 * dxdo[j] + + x * icdist2 * dcdist_do + x * cdist * dicdist2_do + + k[2] * da1do + k[3] * da2do + k[8] * dr2do + + k[9] * dr4do; + double dyd0_do = cdist * icdist2 * dydo[j] + + y * icdist2 * dcdist_do + y * cdist * dicdist2_do + + k[2] * da3do + k[3] * da1do + k[10] * dr2do + + k[11] * dr4do; + dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do ); + dpdo_p[i * 3 + j] = fx * dXdYd( 0 ); + dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 ); + } + dpdo_p += dpdo_step * 2; + } + } + } + + _m.convertTo(_imagePoints, objpt_depth); + + int depth = CV_64F;//cameraMatrix.depth(); + if( _dpdr.needed() ) + dpdr.convertTo(_dpdr, depth); + + if( _dpdt.needed() ) + dpdt.convertTo(_dpdt, depth); + + if( _dpdf.needed() ) + dpdf.convertTo(_dpdf, depth); + + if( _dpdc.needed() ) + dpdc.convertTo(_dpdc, depth); + + if( _dpdk.needed() ) + dpdk.convertTo(_dpdk, depth); + + if( _dpdo.needed() ) + dpdo.convertTo(_dpdo, depth); +} + +cv::Vec3d cv::RQDecomp3x3( InputArray _Marr, + OutputArray _Rarr, + OutputArray _Qarr, + OutputArray _Qx, + OutputArray _Qy, + OutputArray _Qz ) +{ + CV_INSTRUMENT_REGION(); + + Matx33d M, Q; + double z, c, s; + Mat Mmat = _Marr.getMat(); + int depth = Mmat.depth(); + Mmat.convertTo(M, CV_64F); + + /* Find Givens rotation Q_x for x axis (left multiplication). */ + /* + ( 1 0 0 ) + Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) + ( 0 -s c ) + */ + s = std::abs(M(2, 1)) > DBL_EPSILON ? M(2, 1): 0.; + c = std::abs(M(2, 1)) > DBL_EPSILON ? M(2, 2): 1.; + z = 1./std::sqrt(c * c + s * s); + c *= z; + s *= z; + + Matx33d Qx(1, 0, 0, 0, c, s, 0, -s, c); + Matx33d R = M*Qx; + + assert(fabs(R(2, 1)) < FLT_EPSILON); + R(2, 1) = 0; + + /* Find Givens rotation for y axis. */ + /* + ( c 0 -s ) + Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2) + ( s 0 c ) + */ + s = std::abs(R(2, 0)) > DBL_EPSILON ? -R(2, 0): 0.; + c = std::abs(R(2, 0)) > DBL_EPSILON ? R(2, 2): 1.; + z = 1./std::sqrt(c * c + s * s); + c *= z; + s *= z; + + Matx33d Qy(c, 0, -s, 0, 1, 0, s, 0, c); + M = R*Qy; + + CV_Assert(fabs(M(2, 0)) < FLT_EPSILON); + M(2, 0) = 0; + + /* Find Givens rotation for z axis. */ + /* + ( c s 0 ) + Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) + ( 0 0 1 ) + */ + + s = std::abs(M(1, 0)) > DBL_EPSILON ? M(1, 0): 0.; + c = std::abs(M(1, 0)) > DBL_EPSILON ? M(1, 1): 1.; + z = 1./std::sqrt(c * c + s * s); + c *= z; + s *= z; + + Matx33d Qz(c, s, 0, -s, c, 0, 0, 0, 1); + R = M*Qz; + + CV_Assert(fabs(R(1, 0)) < FLT_EPSILON); + R(1, 0) = 0; + + // Solve the decomposition ambiguity. + // Diagonal entries of R, except the last one, shall be positive. + // Further rotate R by 180 degree if necessary + if( R(0, 0) < 0 ) + { + if( R(1, 1) < 0 ) + { + // rotate around z for 180 degree, i.e. a rotation matrix of + // [-1, 0, 0], + // [ 0, -1, 0], + // [ 0, 0, 1] + R(0, 0) *= -1; + R(0, 1) *= -1; + R(1, 1) *= -1; + + Qz(0, 0) *= -1; + Qz(0, 1) *= -1; + Qz(1, 0) *= -1; + Qz(1, 1) *= -1; + } + else + { + // rotate around y for 180 degree, i.e. a rotation matrix of + // [-1, 0, 0], + // [ 0, 1, 0], + // [ 0, 0, -1] + R(0, 0) *= -1; + R(0, 2) *= -1; + R(1, 2) *= -1; + R(2, 2) *= -1; + + Qz = Qz.t(); + + Qy(0, 0) *= -1; + Qy(0, 2) *= -1; + Qy(2, 0) *= -1; + Qy(2, 2) *= -1; + } + } + else if( R(1, 1) < 0 ) + { + // ??? for some reason, we never get here ??? + + // rotate around x for 180 degree, i.e. a rotation matrix of + // [ 1, 0, 0], + // [ 0, -1, 0], + // [ 0, 0, -1] + R(0, 1) *= -1; + R(0, 2) *= -1; + R(1, 1) *= -1; + R(1, 2) *= -1; + R(2, 2) *= -1; + + Qz = Qz.t(); + Qy = Qy.t(); + + Qx(1, 1) *= -1; + Qx(1, 2) *= -1; + Qx(2, 1) *= -1; + Qx(2, 2) *= -1; + } + + // calculate the euler angle + Vec3d eulerAngles( + std::acos(Qx(1, 1)) * (Qx(1, 2) >= 0 ? 1 : -1) * (180.0 / CV_PI), + std::acos(Qy(0, 0)) * (Qy(2, 0) >= 0 ? 1 : -1) * (180.0 / CV_PI), + std::acos(Qz(0, 0)) * (Qz(0, 1) >= 0 ? 1 : -1) * (180.0 / CV_PI)); + + /* Calculate orthogonal matrix. */ + /* + Q = QzT * QyT * QxT + */ + M = Qz.t()*Qy.t(); + Q = M*Qx.t(); + + /* Save R and Q matrices. */ + Mat(R).convertTo(_Rarr, depth); + Mat(Q).convertTo(_Qarr, depth); + + if(_Qx.needed()) + Mat(Qx).convertTo(_Qx, depth); + if(_Qy.needed()) + Mat(Qy).convertTo(_Qy, depth); + if(_Qz.needed()) + Mat(Qz).convertTo(_Qz, depth); + return eulerAngles; +} + +void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix, + OutputArray _rotMatrix, OutputArray _transVect, + OutputArray _rotMatrixX, OutputArray _rotMatrixY, + OutputArray _rotMatrixZ, OutputArray _eulerAngles ) +{ + CV_INSTRUMENT_REGION(); + + Mat projMatrix = _projMatrix.getMat(); + int depth = projMatrix.depth(); + Matx34d P; + projMatrix.convertTo(P, CV_64F); + Matx44d Px(P(0, 0), P(0, 1), P(0, 2), P(0, 3), + P(1, 0), P(1, 1), P(1, 2), P(1, 3), + P(2, 0), P(2, 1), P(2, 2), P(2, 3), + 0, 0, 0, 0), U, Vt; + Matx41d W; + SVDecomp(Px, W, U, Vt, SVD::MODIFY_A); + Vec4d t(Vt(3, 0), Vt(3, 1), Vt(3, 2), Vt(3, 3)); + Matx33d M(P(0, 0), P(0, 1), P(0, 2), + P(1, 0), P(1, 1), P(1, 2), + P(2, 0), P(2, 1), P(2, 2)); + Mat(t).convertTo(_transVect, depth); + Vec3d eulerAngles = RQDecomp3x3(M, _cameraMatrix, _rotMatrix, _rotMatrixX, _rotMatrixY, _rotMatrixZ); + if (_eulerAngles.needed()) + Mat(eulerAngles).convertTo(_eulerAngles, depth); +} + +void cv::findExtrinsicCameraParams2( const Mat& objectPoints, + const Mat& imagePoints, const Mat& A, + const Mat& distCoeffs, Mat& rvec, Mat& tvec, + int useExtrinsicGuess ) +{ + const int max_iter = 20; + Mat matM, _m, _mn; + + int i, count; + double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9]; + double MM[9] = { 0 }, U[9] = { 0 }, V[9] = { 0 }, W[3] = { 0 }; + double param[6] = { 0 }; + Mat matA( 3, 3, CV_64F, a ); + Mat _Ar( 3, 3, CV_64F, ar ); + Mat matR( 3, 3, CV_64F, R ); + Mat _r( 3, 1, CV_64F, param ); + Mat _t( 3, 1, CV_64F, param + 3 ); + Mat _MM( 3, 3, CV_64F, MM ); + Mat matU( 3, 3, CV_64F, U ); + Mat matV( 3, 3, CV_64F, V ); + Mat matW( 3, 1, CV_64F, W ); + Mat _param( 6, 1, CV_64F, param ); + Mat _dpdr, _dpdt; + + count = MAX(objectPoints.cols, objectPoints.rows); + if (objectPoints.checkVector(3) > 0) + objectPoints.convertTo(matM, CV_64F); + else { + convertPointsFromHomogeneous(objectPoints, matM); + matM.convertTo(matM, CV_64F); + } + if (imagePoints.checkVector(2) > 0) + imagePoints.convertTo(_m, CV_64F); + else { + convertPointsFromHomogeneous(imagePoints, _m); + _m.convertTo(_m, CV_64F); + } + A.convertTo(matA, CV_64F); + + CV_Assert((count >= 4) || (count == 3 && useExtrinsicGuess)); // it is unsafe to call LM optimisation without an extrinsic guess in the case of 3 points. This is because there is no guarantee that it will converge on the correct solution. + + // normalize image points + // (unapply the intrinsic matrix transformation and distortion) + undistortPoints(_m, _mn, matA, distCoeffs, Mat(), _Ar); + + if( useExtrinsicGuess ) + { + CV_Assert((rvec.rows == 1 || rvec.cols == 1) && rvec.total()*rvec.channels() == 3); + CV_Assert((tvec.rows == 1 || tvec.cols == 1) && tvec.total()*tvec.channels() == 3); + Mat _r_temp(rvec.rows, rvec.cols, CV_MAKETYPE(CV_64F,rvec.channels()), param); + Mat _t_temp(tvec.rows, tvec.cols, CV_MAKETYPE(CV_64F,tvec.channels()), param + 3); + rvec.convertTo(_r_temp, CV_64F); + tvec.convertTo(_t_temp, CV_64F); + } + else + { + Scalar Mc = mean(matM); + Mat _Mc( 1, 3, CV_64F, Mc.val ); + + matM = matM.reshape(1, count); + mulTransposed(matM, _MM, true, _Mc); + SVDecomp(_MM, matW, noArray(), matV, SVD::MODIFY_A); + CV_Assert(matW.ptr() == W); + CV_Assert(matV.ptr() == V); + + // initialize extrinsic parameters + if( W[2]/W[1] < 1e-3) + { + // a planar structure case (all M's lie in the same plane) + double tt[3]; + Mat R_transform = matV; + Mat T_transform( 3, 1, CV_64F, tt ); + + if( V[2]*V[2] + V[5]*V[5] < 1e-10 ) + R_transform = Mat::eye(3, 3, CV_64F); + + if( determinant(R_transform) < 0 ) + R_transform *= -1.; + + gemm( R_transform, _Mc, -1, Mat(), 0, T_transform, GEMM_2_T ); + + const double* Rp = R_transform.ptr(); + const double* Tp = T_transform.ptr(); + + Mat _Mxy(count, 1, CV_64FC2); + const double* src = (double*)matM.data; + double* dst = (double*)_Mxy.data; + + for( i = 0; i < count; i++, src += 3, dst += 2 ) + { + dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0]; + dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1]; + } + + Mat matH = findHomography(_Mxy, _mn); + + if( checkRange(matH, true)) + { + Mat _h1 = matH.col(0); + Mat _h2 = matH.col(1); + Mat _h3 = matH.col(2); + double* h = matH.ptr(); + CV_Assert(matH.isContinuous()); + double h1_norm = std::sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]); + double h2_norm = std::sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]); + + _h1 *= 1./MAX(h1_norm, DBL_EPSILON); + _h2 *= 1./MAX(h2_norm, DBL_EPSILON); + _t = _h3 * (2./MAX(h1_norm + h2_norm, DBL_EPSILON)); + _h1.cross(_h2).copyTo(_h3); + + Rodrigues( matH, _r ); + Rodrigues( _r, matH ); + _t += matH*T_transform; + matR = matH * R_transform; + } + else + { + setIdentity(matR); + _t.setTo(0); + } + + Rodrigues( matR, _r ); + } + else + { + // non-planar structure. Use DLT method + CV_CheckGE(count, 6, "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences."); + double LL[12*12], LW[12], LV[12*12], sc; + Mat _LL( 12, 12, CV_64F, LL ); + Mat _LW( 12, 1, CV_64F, LW ); + Mat _LV( 12, 12, CV_64F, LV ); + Point3d* M = (Point3d*)matM.data; + Point2d* mn = (Point2d*)_mn.data; + + Mat matL(2*count, 12, CV_64F); + double* L = matL.ptr(); + + for( i = 0; i < count; i++, L += 24 ) + { + double x = -mn[i].x, y = -mn[i].y; + L[0] = L[16] = M[i].x; + L[1] = L[17] = M[i].y; + L[2] = L[18] = M[i].z; + L[3] = L[19] = 1.; + L[4] = L[5] = L[6] = L[7] = 0.; + L[12] = L[13] = L[14] = L[15] = 0.; + L[8] = x*M[i].x; + L[9] = x*M[i].y; + L[10] = x*M[i].z; + L[11] = x; + L[20] = y*M[i].x; + L[21] = y*M[i].y; + L[22] = y*M[i].z; + L[23] = y; + } + + mulTransposed( matL, _LL, true ); + SVDecomp( _LL, _LW, noArray(), _LV, SVD::MODIFY_A ); + Mat _RRt( 3, 4, CV_64F, LV + 11*12 ); + Mat _RR = _RRt.colRange(0, 3); + Mat _tt = _RRt.col(3); + if( determinant(_RR) < 0 ) + _RRt *= -1.0; + sc = norm(_RR, NORM_L2); + CV_Assert(fabs(sc) > DBL_EPSILON); + SVDecomp(_RR, matW, matU, matV, SVD::MODIFY_A); + matR = matU*matV; + _tt.convertTo(_t, CV_64F, norm(matR, NORM_L2)/sc); + Rodrigues(matR, _r); + } + } + + matM = matM.reshape(3, 1); + _mn = _mn.reshape(2, 1); + + // refine extrinsic parameters using iterative algorithm +#if 0 + // The C++ LMSolver is not as good as CvLevMarq to pass the tests, maybe due to _completeSymmFlag in CvLevMarq. + class RefineLMCallback CV_FINAL : public LMSolver::Callback + { + public: + RefineLMCallback(const Mat &matM, const Mat &_m, const Mat &matA, const Mat& distCoeffs) : matM_(matM), _m_(_m), matA_(matA), distCoeffs_(distCoeffs) { + } + bool compute(InputArray param_, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE + { + const Mat& objpt = matM_; + const Mat& imgpt = _m_; + const Mat& cameraMatrix = matA_; + Mat x = param_.getMat(); + CV_Assert((x.cols == 1 || x.rows == 1) && x.total() == 6 && x.type() == CV_64F); + double* pdata = x.ptr(); + Mat rv(3, 1, CV_64F, pdata); + Mat tv(3, 1, CV_64F, pdata + 3); + int errCount = objpt.rows + objpt.cols - 1; + _err.create(errCount * 2, 1, CV_64F); + Mat err = _err.getMat(); + err = err.reshape(2, errCount); + if (_Jac.needed()) + { + _Jac.create(errCount * 2, 6, CV_64F); + Mat Jac = _Jac.getMat(); + Mat dpdr = Jac.colRange(0, 3); + Mat dpdt = Jac.colRange(3, 6); + projectPoints(objpt, rv, tv, cameraMatrix, distCoeffs_, + err, dpdr, dpdt, noArray(), noArray(), noArray(), noArray()); + } + else + { + projectPoints(objpt, rv, tv, cameraMatrix, distCoeffs_, err); + } + err = err - (imgpt.rows == 1 ? imgpt.t() : imgpt); + err = err.reshape(1, 2 * errCount); + return true; + }; + private: + const Mat &matM_, &_m_, &matA_, &distCoeffs_; + }; + + LMSolver::create(makePtr(matM, _m, matA, distCoeffs), max_iter, FLT_EPSILON)->run(_param); +#else + CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true); + _param.copyTo(cvarrToMat(solver.param)); + + for(;;) + { + CvMat *matJ = 0, *_err = 0; + const CvMat *__param = 0; + bool proceed = solver.update( __param, matJ, _err ); + cvarrToMat(__param).copyTo(_param ); + if( !proceed || !_err ) + break; + int errCount = matM.rows + matM.cols - 1; + Mat err = cvarrToMat(_err); + err = err.reshape(2, errCount); + if( matJ ) + { + Mat Jac = cvarrToMat(matJ); + Mat dpdr = Jac.colRange(0, 3); + Mat dpdt = Jac.colRange(3, 6); + projectPoints(matM, _r, _t, matA, distCoeffs, + err, dpdr, dpdt, noArray(), noArray(), noArray(), noArray()); + } + else + { + projectPoints(matM, _r, _t, matA, distCoeffs, err); + } + subtract(err, _m.rows == 1 ? _m.t() : _m, err); + cvReshape( _err, _err, 1, 2*count ); + } + cvarrToMat(solver.param).copyTo(_param ); +#endif + + _param.rowRange(0, 3).convertTo(rvec, rvec.depth()); + _param.rowRange(3, 6).convertTo(tvec, tvec.depth()); +} + +void cv::projectPoints( InputArray _opoints, + InputArray _rvec, + InputArray _tvec, + InputArray _cameraMatrix, + InputArray _distCoeffs, + OutputArray _ipoints, + OutputArray _jacobian, + double aspectRatio ) +{ + Mat opoints = _opoints.getMat(); + int npoints = opoints.checkVector(3), depth = opoints.depth(); + if (npoints < 0) + opoints = opoints.t(); + npoints = opoints.checkVector(3); + CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F)); + + if (opoints.cols == 3) + opoints = opoints.reshape(3); + + CV_Assert( _ipoints.needed() ); + + double dc0buf[5]={0}; + Mat dc0(5,1,CV_64F,dc0buf); + Mat distCoeffs = _distCoeffs.getMat(); + if( distCoeffs.empty() ) + distCoeffs = dc0; + int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1; + + if( _jacobian.needed() ) + { + _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F); + Mat jacobian = _jacobian.getMat(); + Mat dpdr = jacobian.colRange(0, 3); + Mat dpdt = jacobian.colRange(3, 6); + Mat dpdf = jacobian.colRange(6, 8); + Mat dpdc = jacobian.colRange(8, 10); + Mat dpdk = jacobian.colRange(10, 10+ndistCoeffs); + + projectPoints(opoints, _rvec, _tvec, _cameraMatrix, distCoeffs, _ipoints, + dpdr, dpdt, dpdf, dpdc, dpdk, noArray(), aspectRatio); + } + else + { + projectPoints(opoints, _rvec, _tvec, _cameraMatrix, distCoeffs, _ipoints, + noArray(), noArray(), noArray(), noArray(), noArray(), noArray(), aspectRatio); + } +} + +static void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, + InputArray R, InputArray newCameraMatrix, Size imgSize, + Rect_& inner, Rect_& outer ) +{ + const int N = 9; + int x, y, k; + Mat _pts(1, N*N, CV_64FC2); + Point2d* pts = _pts.ptr(); + + for( y = k = 0; y < N; y++ ) + for( x = 0; x < N; x++ ) + pts[k++] = Point2d((double)x*(imgSize.width-1)/(N-1), (double)y*(imgSize.height-1)/(N-1)); + + undistortPoints(_pts, _pts, _cameraMatrix, _distCoeffs, R, newCameraMatrix); + + double iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX; + double oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX; + // find the inscribed rectangle. + // the code will likely not work with extreme rotation matrices (R) (>45%) + for( y = k = 0; y < N; y++ ) + for( x = 0; x < N; x++ ) + { + Point2d p = pts[k++]; + oX0 = MIN(oX0, p.x); + oX1 = MAX(oX1, p.x); + oY0 = MIN(oY0, p.y); + oY1 = MAX(oY1, p.y); + + if( x == 0 ) + iX0 = MAX(iX0, p.x); + if( x == N-1 ) + iX1 = MIN(iX1, p.x); + if( y == 0 ) + iY0 = MAX(iY0, p.y); + if( y == N-1 ) + iY1 = MIN(iY1, p.y); + } + inner = Rect_(iX0, iY0, iX1-iX0, iY1-iY0); + outer = Rect_(oX0, oY0, oX1-oX0, oY1-oY0); +} + +cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix, InputArray _distCoeffs, + Size imgSize, double alpha, Size newImgSize, + Rect* validPixROI, bool centerPrincipalPoint ) +{ + Rect_ inner, outer; + newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imgSize; + + Mat cameraMatrix = _cameraMatrix.getMat(), M; + cameraMatrix.convertTo(M, CV_64F); + CV_Assert(M.isContinuous()); + + if( centerPrincipalPoint ) + { + double cx0 = M.at(0, 2); + double cy0 = M.at(1, 2); + double cx = (newImgSize.width-1)*0.5; + double cy = (newImgSize.height-1)*0.5; + + getUndistortRectangles( _cameraMatrix, _distCoeffs, Mat(), cameraMatrix, imgSize, inner, outer ); + double s0 = std::max(std::max(std::max((double)cx/(cx0 - inner.x), (double)cy/(cy0 - inner.y)), + (double)cx/(inner.x + inner.width - cx0)), + (double)cy/(inner.y + inner.height - cy0)); + double s1 = std::min(std::min(std::min((double)cx/(cx0 - outer.x), (double)cy/(cy0 - outer.y)), + (double)cx/(outer.x + outer.width - cx0)), + (double)cy/(outer.y + outer.height - cy0)); + double s = s0*(1 - alpha) + s1*alpha; + + M.at(0, 0) *= s; + M.at(1, 1) *= s; + M.at(0, 2) = cx; + M.at(1, 2) = cy; + + if( validPixROI ) + { + inner = cv::Rect_((double)((inner.x - cx0)*s + cx), + (double)((inner.y - cy0)*s + cy), + (double)(inner.width*s), + (double)(inner.height*s)); + Rect r(cvCeil(inner.x), cvCeil(inner.y), cvFloor(inner.width), cvFloor(inner.height)); + r &= Rect(0, 0, newImgSize.width, newImgSize.height); + *validPixROI = r; + } + } + else + { + // Get inscribed and circumscribed rectangles in normalized + // (independent of camera matrix) coordinates + getUndistortRectangles( _cameraMatrix, _distCoeffs, Mat(), Mat(), imgSize, inner, outer ); + + // Projection mapping inner rectangle to viewport + double fx0 = (newImgSize.width - 1) / inner.width; + double fy0 = (newImgSize.height - 1) / inner.height; + double cx0 = -fx0 * inner.x; + double cy0 = -fy0 * inner.y; + + // Projection mapping outer rectangle to viewport + double fx1 = (newImgSize.width - 1) / outer.width; + double fy1 = (newImgSize.height - 1) / outer.height; + double cx1 = -fx1 * outer.x; + double cy1 = -fy1 * outer.y; + + // Interpolate between the two optimal projections + M.at(0, 0) = fx0*(1 - alpha) + fx1*alpha; + M.at(1, 1) = fy0*(1 - alpha) + fy1*alpha; + M.at(0, 2) = cx0*(1 - alpha) + cx1*alpha; + M.at(1, 2) = cy0*(1 - alpha) + cy1*alpha; + + if( validPixROI ) + { + getUndistortRectangles( _cameraMatrix, _distCoeffs, Mat(), M, imgSize, inner, outer ); + Rect r = inner; + r &= Rect(0, 0, newImgSize.width, newImgSize.height); + *validPixROI = r; + } + } + M.convertTo(M, cameraMatrix.type()); + + return M; +} + +/* End of file. */ diff --git a/modules/calib3d/src/checkchessboard.cpp b/modules/calib3d/src/checkchessboard.cpp index 350614e78f..2e867303cc 100644 --- a/modules/calib3d/src/checkchessboard.cpp +++ b/modules/calib3d/src/checkchessboard.cpp @@ -40,7 +40,6 @@ //M*/ #include "precomp.hpp" -#include "opencv2/imgproc/imgproc_c.h" #include "calib3d_c_api.h" #include diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index 8f598d6709..05811e0907 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -137,6 +137,19 @@ static inline bool haveCollinearPoints( const Mat& m, int count ) return false; } +void findExtrinsicCameraParams2( const Mat& objectPoints, + const Mat& imagePoints, const Mat& A, + const Mat& distCoeffs, Mat& rvec, Mat& tvec, + int useExtrinsicGuess ); + +void projectPoints( InputArray objectPoints, + InputArray rvec, InputArray tvec, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray imagePoints, OutputArray dpdr, + OutputArray dpdt, OutputArray dpdf=noArray(), + OutputArray dpdc=noArray(), OutputArray dpdk=noArray(), + OutputArray dpdo=noArray(), double aspectRatio=0.); + } // namespace cv int checkChessboardBinary(const cv::Mat & img, const cv::Size & size); diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 771abff483..37cf0bb2db 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -48,7 +48,6 @@ #include "ap3p.h" #include "ippe.hpp" #include "sqpnp.hpp" -#include "calib3d_c_api.h" #include "usac.hpp" @@ -892,12 +891,7 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, tvec.create(3, 1, CV_64FC1); } - CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints); - CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs); - CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec); - cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, - (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0, - &c_rvec, &c_tvec, useExtrinsicGuess ); + findExtrinsicCameraParams2(opoints, ipoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); vec_rvecs.push_back(rvec); vec_tvecs.push_back(tvec); From 3d89824423820a0cd94186d7da9a341b38b3cd65 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Fri, 8 Nov 2024 10:27:02 +0100 Subject: [PATCH 04/10] Remove unused internal C functions --- modules/calib3d/src/calib3d_c_api.h | 205 ------------ modules/calib3d/src/calibration.cpp | 91 ------ modules/calib3d/src/checkchessboard.cpp | 13 - modules/calib3d/src/compat_ptsetreg.cpp | 35 -- modules/calib3d/src/posit.cpp | 360 --------------------- modules/calib3d/src/undistort.dispatch.cpp | 48 --- 6 files changed, 752 deletions(-) delete mode 100644 modules/calib3d/src/posit.cpp diff --git a/modules/calib3d/src/calib3d_c_api.h b/modules/calib3d/src/calib3d_c_api.h index 46cdbc824a..76d5f79450 100644 --- a/modules/calib3d/src/calib3d_c_api.h +++ b/modules/calib3d/src/calib3d_c_api.h @@ -55,45 +55,8 @@ extern "C" { * Camera Calibration, Pose Estimation and Stereo * \****************************************************************************************/ -typedef struct CvPOSITObject CvPOSITObject; - -/* Allocates and initializes CvPOSITObject structure before doing cvPOSIT */ -CvPOSITObject* cvCreatePOSITObject( CvPoint3D32f* points, int point_count ); - - -/* Runs POSIT (POSe from ITeration) algorithm for determining 3d position of - an object given its model and projection in a weak-perspective case */ -void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points, - double focal_length, CvTermCriteria criteria, - float* rotation_matrix, float* translation_vector); - -/* Releases CvPOSITObject structure */ -void cvReleasePOSITObject( CvPOSITObject** posit_object ); - -/* updates the number of RANSAC iterations */ -int cvRANSACUpdateNumIters( double p, double err_prob, - int model_points, int max_iters ); - void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ); -/* Calculates fundamental matrix given a set of corresponding points */ -/*#define CV_FM_7POINT 1 -#define CV_FM_8POINT 2 - -#define CV_LMEDS 4 -#define CV_RANSAC 8 - -#define CV_FM_LMEDS_ONLY CV_LMEDS -#define CV_FM_RANSAC_ONLY CV_RANSAC -#define CV_FM_LMEDS CV_LMEDS -#define CV_FM_RANSAC CV_RANSAC*/ - -int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2, - CvMat* fundamental_matrix, - int method CV_DEFAULT(CV_FM_RANSAC), - double param1 CV_DEFAULT(3.), double param2 CV_DEFAULT(0.99), - CvMat* status CV_DEFAULT(NULL) ); - /* For each input point on one of images computes parameters of the corresponding epipolar line on the other image */ @@ -102,15 +65,6 @@ void cvComputeCorrespondEpilines( const CvMat* points, const CvMat* fundamental_matrix, CvMat* correspondent_lines ); -/* Triangulation functions */ - -void cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, - CvMat* projPoints1, CvMat* projPoints2, - CvMat* points4D); - -void cvCorrectMatches(CvMat* F, CvMat* points1, CvMat* points2, - CvMat* new_points1, CvMat* new_points2); - /* Finds perspective transformation between the object plane and image (view) plane */ int cvFindHomography( const CvMat* src_points, const CvMat* dst_points, @@ -129,45 +83,6 @@ void cvInitIntrinsicParams2D( const CvMat* object_points, CvMat* camera_matrix, double aspect_ratio CV_DEFAULT(1.) ); -// Performs a fast check if a chessboard is in the input image. This is a workaround to -// a problem of cvFindChessboardCorners being slow on images with no chessboard -// - src: input image -// - size: chessboard size -// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called, -// 0 if there is no chessboard, -1 in case of error -int cvCheckChessboard(IplImage* src, CvSize size); - - /* Detects corners on a chessboard calibration pattern */ -/*int cvFindChessboardCorners( const void* image, CvSize pattern_size, - CvPoint2D32f* corners, - int* corner_count CV_DEFAULT(NULL), - int flags CV_DEFAULT(CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE) );*/ - -/* Draws individual chessboard corners or the whole chessboard detected */ -/*void cvDrawChessboardCorners( CvArr* image, CvSize pattern_size, - CvPoint2D32f* corners, - int count, int pattern_was_found );*/ - -/*#define CV_CALIB_USE_INTRINSIC_GUESS 1 -#define CV_CALIB_FIX_ASPECT_RATIO 2 -#define CV_CALIB_FIX_PRINCIPAL_POINT 4 -#define CV_CALIB_ZERO_TANGENT_DIST 8 -#define CV_CALIB_FIX_FOCAL_LENGTH 16 -#define CV_CALIB_FIX_K1 32 -#define CV_CALIB_FIX_K2 64 -#define CV_CALIB_FIX_K3 128 -#define CV_CALIB_FIX_K4 2048 -#define CV_CALIB_FIX_K5 4096 -#define CV_CALIB_FIX_K6 8192 -#define CV_CALIB_RATIONAL_MODEL 16384 -#define CV_CALIB_THIN_PRISM_MODEL 32768 -#define CV_CALIB_FIX_S1_S2_S3_S4 65536 -#define CV_CALIB_TILTED_MODEL 262144 -#define CV_CALIB_FIX_TAUX_TAUY 524288 -#define CV_CALIB_FIX_TANGENT_DIST 2097152 - -#define CV_CALIB_NINTRINSIC 18*/ - /* Finds intrinsic and extrinsic camera parameters from a few views of known calibration pattern */ double cvCalibrateCamera2( const CvMat* object_points, @@ -182,37 +97,6 @@ double cvCalibrateCamera2( const CvMat* object_points, CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) ); -/* Finds intrinsic and extrinsic camera parameters - from a few views of known calibration pattern */ -double cvCalibrateCamera4( const CvMat* object_points, - const CvMat* image_points, - const CvMat* point_counts, - CvSize image_size, - int iFixedPoint, - CvMat* camera_matrix, - CvMat* distortion_coeffs, - CvMat* rotation_vectors CV_DEFAULT(NULL), - CvMat* translation_vectors CV_DEFAULT(NULL), - CvMat* newObjPoints CV_DEFAULT(NULL), - int flags CV_DEFAULT(0), - CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( - CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) ); - -/* Computes various useful characteristics of the camera from the data computed by - cvCalibrateCamera2 */ -void cvCalibrationMatrixValues( const CvMat *camera_matrix, - CvSize image_size, - double aperture_width CV_DEFAULT(0), - double aperture_height CV_DEFAULT(0), - double *fovx CV_DEFAULT(NULL), - double *fovy CV_DEFAULT(NULL), - double *focal_length CV_DEFAULT(NULL), - CvPoint2D64f *principal_point CV_DEFAULT(NULL), - double *pixel_aspect_ratio CV_DEFAULT(NULL)); - -/*#define CV_CALIB_FIX_INTRINSIC 256 -#define CV_CALIB_SAME_FOCAL_LENGTH 512*/ - /* Computes the transformation from one camera coordinate system to another one from a few correspondent views of the same calibration target. Optionally, calibrates both cameras */ @@ -248,95 +132,6 @@ int cvStereoRectifyUncalibrated( const CvMat* points1, const CvMat* points2, CvMat* H1, CvMat* H2, double threshold CV_DEFAULT(5)); - - -/* stereo correspondence parameters and functions */ - -#define CV_STEREO_BM_NORMALIZED_RESPONSE 0 -#define CV_STEREO_BM_XSOBEL 1 - -/* Block matching algorithm structure */ -typedef struct CvStereoBMState -{ - // pre-filtering (normalization of input images) - int preFilterType; // =CV_STEREO_BM_NORMALIZED_RESPONSE now - int preFilterSize; // averaging window size: ~5x5..21x21 - int preFilterCap; // the output of pre-filtering is clipped by [-preFilterCap,preFilterCap] - - // correspondence using Sum of Absolute Difference (SAD) - int SADWindowSize; // ~5x5..21x21 - int minDisparity; // minimum disparity (can be negative) - int numberOfDisparities; // maximum disparity - minimum disparity (> 0) - - // post-filtering - int textureThreshold; // the disparity is only computed for pixels - // with textured enough neighborhood - int uniquenessRatio; // accept the computed disparity d* only if - // SAD(d) >= SAD(d*)*(1 + uniquenessRatio/100.) - // for any d != d*+/-1 within the search range. - int speckleWindowSize; // disparity variation window - int speckleRange; // acceptable range of variation in window - - int trySmallerWindows; // if 1, the results may be more accurate, - // at the expense of slower processing - CvRect roi1, roi2; - int disp12MaxDiff; - - // temporary buffers - CvMat* preFilteredImg0; - CvMat* preFilteredImg1; - CvMat* slidingSumBuf; - CvMat* cost; - CvMat* disp; -} CvStereoBMState; - -#define CV_STEREO_BM_BASIC 0 -#define CV_STEREO_BM_FISH_EYE 1 -#define CV_STEREO_BM_NARROW 2 - -CvStereoBMState* cvCreateStereoBMState(int preset CV_DEFAULT(CV_STEREO_BM_BASIC), - int numberOfDisparities CV_DEFAULT(0)); - -void cvReleaseStereoBMState( CvStereoBMState** state ); - -void cvFindStereoCorrespondenceBM( const CvArr* left, const CvArr* right, - CvArr* disparity, CvStereoBMState* state ); - -CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity, - int numberOfDisparities, int SADWindowSize ); - -void cvValidateDisparity( CvArr* disparity, const CvArr* cost, - int minDisparity, int numberOfDisparities, - int disp12MaxDiff CV_DEFAULT(1) ); - -/* Reprojects the computed disparity image to the 3D space using the specified 4x4 matrix */ -void cvReprojectImageTo3D( const CvArr* disparityImage, - CvArr* _3dImage, const CvMat* Q, - int handleMissingValues CV_DEFAULT(0) ); - -/** @brief Transforms the input image to compensate lens distortion -@see cv::undistort -*/ -void cvUndistort2( const CvArr* src, CvArr* dst, - const CvMat* camera_matrix, - const CvMat* distortion_coeffs, - const CvMat* new_camera_matrix CV_DEFAULT(0) ); - -/** @brief Computes transformation map from intrinsic camera parameters - that can used by cvRemap -*/ -void cvInitUndistortMap( const CvMat* camera_matrix, - const CvMat* distortion_coeffs, - CvArr* mapx, CvArr* mapy ); - -/** @brief Computes undistortion+rectification map for a head of stereo camera -@see cv::initUndistortRectifyMap -*/ -void cvInitUndistortRectifyMap( const CvMat* camera_matrix, - const CvMat* dist_coeffs, - const CvMat *R, const CvMat* new_camera_matrix, - CvArr* mapx, CvArr* mapy ); - /** @brief Computes the original (undistorted) feature coordinates from the observed (distorted) coordinates @see cv::undistortPoints diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index c791d64f00..a10d83b7eb 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -679,82 +679,6 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, distCoeffs, rvecs, tvecs, NULL, NULL, NULL, flags, termCrit); } -CV_IMPL double cvCalibrateCamera4( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* npoints, - CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs, - CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, int flags, CvTermCriteria termCrit ) -{ - if( !CV_IS_MAT(npoints) ) - CV_Error( cv::Error::StsBadArg, "npoints is not a valid matrix" ); - if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || - (npoints->rows != 1 && npoints->cols != 1) ) - CV_Error( cv::Error::StsUnsupportedFormat, - "the array of point counters must be 1-dimensional integer vector" ); - - bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints->data.i[0] - 1; - int nimages = npoints->rows * npoints->cols; - int npstep = npoints->rows == 1 ? 1 : npoints->step / CV_ELEM_SIZE(npoints->type); - int i, ni; - // check object points. If not qualified, report errors. - if( releaseObject ) - { - if( !CV_IS_MAT(objectPoints) ) - CV_Error( cv::Error::StsBadArg, "objectPoints is not a valid matrix" ); - Mat matM; - if(CV_MAT_CN(objectPoints->type) == 3) { - matM = cvarrToMat(objectPoints); - } else { - convertPointsHomogeneous(cvarrToMat(objectPoints), matM); - } - - matM = matM.reshape(3, 1); - ni = npoints->data.i[0]; - for( i = 1; i < nimages; i++ ) - { - if( npoints->data.i[i * npstep] != ni ) - { - CV_Error( cv::Error::StsBadArg, "All objectPoints[i].size() should be equal when " - "object-releasing method is requested." ); - } - Mat ocmp = matM.colRange(ni * i, ni * i + ni) != matM.colRange(0, ni); - ocmp = ocmp.reshape(1); - if( countNonZero(ocmp) ) - { - CV_Error( cv::Error::StsBadArg, "All objectPoints[i] should be identical when object-releasing" - " method is requested." ); - } - } - } - - return cvCalibrateCamera2Internal(objectPoints, imagePoints, npoints, imageSize, iFixedPoint, - cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints, NULL, - NULL, flags, termCrit); -} - -void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize, - double apertureWidth, double apertureHeight, double *fovx, double *fovy, - double *focalLength, CvPoint2D64f *principalPoint, double *pasp ) -{ - /* Validate parameters. */ - if(calibMatr == 0) - CV_Error(cv::Error::StsNullPtr, "Some of parameters is a NULL pointer!"); - - if(!CV_IS_MAT(calibMatr)) - CV_Error(cv::Error::StsUnsupportedFormat, "Input parameters must be matrices!"); - - double dummy = .0; - Point2d pp; - cv::calibrationMatrixValues(cvarrToMat(calibMatr), imgSize, apertureWidth, apertureHeight, - fovx ? *fovx : dummy, - fovy ? *fovy : dummy, - focalLength ? *focalLength : dummy, - pp, - pasp ? *pasp : dummy); - - if(principalPoint) - *principalPoint = cvPoint2D64f(pp.x, pp.y); -} - //////////////////////////////// Stereo Calibration /////////////////////////////////// @@ -1870,21 +1794,6 @@ void cv::reprojectImageTo3D( InputArray _disparity, } -void cvReprojectImageTo3D( const CvArr* disparityImage, - CvArr* _3dImage, const CvMat* matQ, - int handleMissingValues ) -{ - cv::Mat disp = cv::cvarrToMat(disparityImage); - cv::Mat _3dimg = cv::cvarrToMat(_3dImage); - cv::Mat mq = cv::cvarrToMat(matQ); - CV_Assert( disp.size() == _3dimg.size() ); - int dtype = _3dimg.type(); - CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 ); - - cv::reprojectImageTo3D(disp, _3dimg, mq, handleMissingValues != 0, dtype ); -} - - namespace cv { diff --git a/modules/calib3d/src/checkchessboard.cpp b/modules/calib3d/src/checkchessboard.cpp index 2e867303cc..c7d66d351d 100644 --- a/modules/calib3d/src/checkchessboard.cpp +++ b/modules/calib3d/src/checkchessboard.cpp @@ -40,7 +40,6 @@ //M*/ #include "precomp.hpp" -#include "calib3d_c_api.h" #include #include @@ -150,18 +149,6 @@ static bool checkQuads(vector > & quads, const cv::Size & size) return false; } -// does a fast check if a chessboard is in the input image. This is a workaround to -// a problem of cvFindChessboardCorners being slow on images with no chessboard -// - src: input image -// - size: chessboard size -// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called, -// 0 if there is no chessboard, -1 in case of error -int cvCheckChessboard(IplImage* src, CvSize size) -{ - cv::Mat img = cv::cvarrToMat(src); - return (int)cv::checkChessboard(img, size); -} - bool cv::checkChessboard(InputArray _img, Size size) { Mat img = _img.getMat(); diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index 8c51ceb220..ba103d7b8d 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -322,12 +322,6 @@ void CvLevMarq::step() param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0); } -CV_IMPL int cvRANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters ) -{ - return cv::RANSACUpdateNumIters(p, ep, modelPoints, maxIters); -} - - CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, int method, double ransacReprojThreshold, CvMat* _mask, int maxIters, double confidence) @@ -365,35 +359,6 @@ CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, } -CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2, - CvMat* fmatrix, int method, - double param1, double param2, CvMat* _mask ) -{ - cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2); - - if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 ) - cv::transpose(m1, m1); - if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 ) - cv::transpose(m2, m2); - - const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask); - cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2, - _mask ? cv::_OutputArray(mask) : cv::_OutputArray()); - - if( FM0.empty() ) - { - cv::Mat FM0z = cv::cvarrToMat(fmatrix); - FM0z.setTo(cv::Scalar::all(0)); - return 0; - } - - CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 ); - cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows)); - FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type()); - return FM1.rows / 3; -} - - CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID, const CvMat* fmatrix, CvMat* _lines ) { diff --git a/modules/calib3d/src/posit.cpp b/modules/calib3d/src/posit.cpp deleted file mode 100644 index 22043121d0..0000000000 --- a/modules/calib3d/src/posit.cpp +++ /dev/null @@ -1,360 +0,0 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// Intel License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000, Intel Corporation, all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of Intel Corporation may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ -#include "precomp.hpp" -#include "calib3d_c_api.h" - -/* POSIT structure */ -struct CvPOSITObject -{ - int N; - float* inv_matr; - float* obj_vecs; - float* img_vecs; -}; - -static void icvPseudoInverse3D( float *a, float *b, int n, int method ); - -static CvStatus icvCreatePOSITObject( CvPoint3D32f *points, - int numPoints, - CvPOSITObject **ppObject ) -{ - int i; - - /* Compute size of required memory */ - /* buffer for inverse matrix = N*3*float */ - /* buffer for storing weakImagePoints = numPoints * 2 * float */ - /* buffer for storing object vectors = N*3*float */ - /* buffer for storing image vectors = N*2*float */ - - int N = numPoints - 1; - int inv_matr_size = N * 3 * sizeof( float ); - int obj_vec_size = inv_matr_size; - int img_vec_size = N * 2 * sizeof( float ); - CvPOSITObject *pObject; - - /* check bad arguments */ - if( points == NULL ) - return CV_NULLPTR_ERR; - if( numPoints < 4 ) - return CV_BADSIZE_ERR; - if( ppObject == NULL ) - return CV_NULLPTR_ERR; - - /* memory allocation */ - pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) + - inv_matr_size + obj_vec_size + img_vec_size ); - - if( !pObject ) - return CV_OUTOFMEM_ERR; - - /* part the memory between all structures */ - pObject->N = N; - pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject )); - pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size); - pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size); - -/****************************************************************************************\ -* Construct object vectors from object points * -\****************************************************************************************/ - for( i = 0; i < numPoints - 1; i++ ) - { - pObject->obj_vecs[i] = points[i + 1].x - points[0].x; - pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y; - pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z; - } -/****************************************************************************************\ -* Compute pseudoinverse matrix * -\****************************************************************************************/ - icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 ); - - *ppObject = pObject; - return CV_NO_ERR; -} - - -static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints, - float focalLength, CvTermCriteria criteria, - float* rotation, float* translation ) -{ - int i, j, k; - int count = 0; - bool converged = false; - float scale = 0, inv_Z = 0; - float diff = (float)criteria.epsilon; - - /* Check bad arguments */ - if( imagePoints == NULL ) - return CV_NULLPTR_ERR; - if( pObject == NULL ) - return CV_NULLPTR_ERR; - if( focalLength <= 0 ) - return CV_BADFACTOR_ERR; - if( !rotation ) - return CV_NULLPTR_ERR; - if( !translation ) - return CV_NULLPTR_ERR; - if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS))) - return CV_BADFLAG_ERR; - if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 ) - return CV_BADFACTOR_ERR; - if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 ) - return CV_BADFACTOR_ERR; - - /* init variables */ - float inv_focalLength = 1 / focalLength; - int N = pObject->N; - float *objectVectors = pObject->obj_vecs; - float *invMatrix = pObject->inv_matr; - float *imgVectors = pObject->img_vecs; - - while( !converged ) - { - if( count == 0 ) - { - /* subtract out origin to get image vectors */ - for( i = 0; i < N; i++ ) - { - imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x; - imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y; - } - } - else - { - diff = 0; - /* Compute new SOP (scaled orthograthic projection) image from pose */ - for( i = 0; i < N; i++ ) - { - /* objectVector * k */ - float old; - float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ + - objectVectors[N + i] * rotation[7] /*[2][1]*/ + - objectVectors[2 * N + i] * rotation[8] /*[2][2]*/; - - tmp *= inv_Z; - tmp += 1; - - old = imgVectors[i]; - imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x; - - diff = MAX( diff, (float) fabs( imgVectors[i] - old )); - - old = imgVectors[N + i]; - imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y; - - diff = MAX( diff, (float) fabs( imgVectors[N + i] - old )); - } - } - - /* calculate I and J vectors */ - for( i = 0; i < 2; i++ ) - { - for( j = 0; j < 3; j++ ) - { - rotation[3*i+j] /*[i][j]*/ = 0; - for( k = 0; k < N; k++ ) - { - rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k]; - } - } - } - - float inorm = - rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ + - rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ + - rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/; - - float jnorm = - rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ + - rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ + - rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/; - - const float invInorm = cvInvSqrt( inorm ); - const float invJnorm = cvInvSqrt( jnorm ); - - inorm *= invInorm; - jnorm *= invJnorm; - - rotation[0] /*[0][0]*/ *= invInorm; - rotation[1] /*[0][1]*/ *= invInorm; - rotation[2] /*[0][2]*/ *= invInorm; - - rotation[3] /*[1][0]*/ *= invJnorm; - rotation[4] /*[1][1]*/ *= invJnorm; - rotation[5] /*[1][2]*/ *= invJnorm; - - /* row2 = row0 x row1 (cross product) */ - rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ - - rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/; - - rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ - - rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/; - - rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ - - rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/; - - scale = (inorm + jnorm) / 2.0f; - inv_Z = scale * inv_focalLength; - - count++; - converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon)) - || ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter)); - } - const float invScale = 1 / scale; - translation[0] = imagePoints[0].x * invScale; - translation[1] = imagePoints[0].y * invScale; - translation[2] = 1 / inv_Z; - - return CV_NO_ERR; -} - - -static CvStatus icvReleasePOSITObject( CvPOSITObject ** ppObject ) -{ - cvFree( ppObject ); - return CV_NO_ERR; -} - -/*F/////////////////////////////////////////////////////////////////////////////////////// -// Name: icvPseudoInverse3D -// Purpose: Pseudoinverse N x 3 matrix N >= 3 -// Context: -// Parameters: -// a - input matrix -// b - pseudoinversed a -// n - number of rows in a -// method - if 0, then b = inv(transpose(a)*a) * transpose(a) -// if 1, then SVD used. -// Returns: -// Notes: Both matrix are stored by n-dimensional vectors. -// Now only method == 0 supported. -//F*/ -void -icvPseudoInverse3D( float *a, float *b, int n, int method ) -{ - if( method == 0 ) - { - float ata00 = 0; - float ata11 = 0; - float ata22 = 0; - float ata01 = 0; - float ata02 = 0; - float ata12 = 0; - - int k; - /* compute matrix ata = transpose(a) * a */ - for( k = 0; k < n; k++ ) - { - float a0 = a[k]; - float a1 = a[n + k]; - float a2 = a[2 * n + k]; - - ata00 += a0 * a0; - ata11 += a1 * a1; - ata22 += a2 * a2; - - ata01 += a0 * a1; - ata02 += a0 * a2; - ata12 += a1 * a2; - } - /* inverse matrix ata */ - { - float p00 = ata11 * ata22 - ata12 * ata12; - float p01 = -(ata01 * ata22 - ata12 * ata02); - float p02 = ata12 * ata01 - ata11 * ata02; - - float p11 = ata00 * ata22 - ata02 * ata02; - float p12 = -(ata00 * ata12 - ata01 * ata02); - float p22 = ata00 * ata11 - ata01 * ata01; - - float det = 0; - det += ata00 * p00; - det += ata01 * p01; - det += ata02 * p02; - - const float inv_det = 1 / det; - - /* compute resultant matrix */ - for( k = 0; k < n; k++ ) - { - float a0 = a[k]; - float a1 = a[n + k]; - float a2 = a[2 * n + k]; - - b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det; - b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det; - b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det; - } - } - } - - /*if ( method == 1 ) - { - } - */ - - return; -} - -CV_IMPL CvPOSITObject * -cvCreatePOSITObject( CvPoint3D32f * points, int numPoints ) -{ - CvPOSITObject *pObject = 0; - IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject )); - return pObject; -} - - -CV_IMPL void -cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints, - double focalLength, CvTermCriteria criteria, - float* rotation, float* translation ) -{ - IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria, - rotation, translation )); -} - -CV_IMPL void -cvReleasePOSITObject( CvPOSITObject ** ppObject ) -{ - IPPI_CALL( icvReleasePOSITObject( ppObject )); -} - -/* End of file. */ diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index 6c3d32941c..830dc63aa6 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -334,54 +334,6 @@ void undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix, } -CV_IMPL void -cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr ) -{ - cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst; - cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA; - if( newAarr ) - newA = cv::cvarrToMat(newAarr); - - CV_Assert( src.size() == dst.size() && src.type() == dst.type() ); - cv::undistort( src, dst, A, distCoeffs, newA ); -} - - -CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs, - CvArr* mapxarr, CvArr* mapyarr ) -{ - cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs); - cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0; - - if( mapyarr ) - mapy0 = mapy = cv::cvarrToMat(mapyarr); - - cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A, - mapx.size(), mapx.type(), mapx, mapy ); - CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data ); -} - -void -cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs, - const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr ) -{ - cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar; - cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0; - - if( mapyarr ) - mapy0 = mapy = cv::cvarrToMat(mapyarr); - - if( dist_coeffs ) - distCoeffs = cv::cvarrToMat(dist_coeffs); - if( Rarr ) - R = cv::cvarrToMat(Rarr); - if( ArArr ) - Ar = cv::cvarrToMat(ArArr); - - cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy ); - CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data ); -} - static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, const CvMat* _distCoeffs, const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria) From 6f8c3b13d8c2a4f79c9fc207b416095bb07f317f Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Mon, 11 Nov 2024 08:22:56 +0100 Subject: [PATCH 05/10] Merge pull request #26437 from vrabaud:4x_calibration_base Backport C++ stereo/stereo_geom.cpp:5.x to calib3d/stereo_geom.cpp:4.x #26437 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/calib3d/src/calib3d_c_api.h | 55 -- modules/calib3d/src/calibration.cpp | 760 --------------------- modules/calib3d/src/calibration_base.cpp | 5 +- modules/calib3d/src/compat_ptsetreg.cpp | 77 --- modules/calib3d/src/fisheye.cpp | 84 --- modules/calib3d/src/precomp.hpp | 4 + modules/calib3d/src/stereo_geom.cpp | 713 +++++++++++++++++++ modules/calib3d/src/undistort.dispatch.cpp | 11 +- 8 files changed, 721 insertions(+), 988 deletions(-) create mode 100644 modules/calib3d/src/stereo_geom.cpp diff --git a/modules/calib3d/src/calib3d_c_api.h b/modules/calib3d/src/calib3d_c_api.h index 76d5f79450..68d3c20b78 100644 --- a/modules/calib3d/src/calib3d_c_api.h +++ b/modules/calib3d/src/calib3d_c_api.h @@ -55,16 +55,6 @@ extern "C" { * Camera Calibration, Pose Estimation and Stereo * \****************************************************************************************/ -void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst ); - -/* For each input point on one of images - computes parameters of the corresponding - epipolar line on the other image */ -void cvComputeCorrespondEpilines( const CvMat* points, - int which_image, - const CvMat* fundamental_matrix, - CvMat* correspondent_lines ); - /* Finds perspective transformation between the object plane and image (view) plane */ int cvFindHomography( const CvMat* src_points, const CvMat* dst_points, @@ -97,51 +87,6 @@ double cvCalibrateCamera2( const CvMat* object_points, CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) ); -/* Computes the transformation from one camera coordinate system to another one - from a few correspondent views of the same calibration target. Optionally, calibrates - both cameras */ -double cvStereoCalibrate( const CvMat* object_points, const CvMat* image_points1, - const CvMat* image_points2, const CvMat* npoints, - CvMat* camera_matrix1, CvMat* dist_coeffs1, - CvMat* camera_matrix2, CvMat* dist_coeffs2, - CvSize image_size, CvMat* R, CvMat* T, - CvMat* E CV_DEFAULT(0), CvMat* F CV_DEFAULT(0), - int flags CV_DEFAULT(CV_CALIB_FIX_INTRINSIC), - CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( - CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,1e-6)) ); - -#define CV_CALIB_ZERO_DISPARITY 1024 - -/* Computes 3D rotations (+ optional shift) for each camera coordinate system to make both - views parallel (=> to make all the epipolar lines horizontal or vertical) */ -void cvStereoRectify( const CvMat* camera_matrix1, const CvMat* camera_matrix2, - const CvMat* dist_coeffs1, const CvMat* dist_coeffs2, - CvSize image_size, const CvMat* R, const CvMat* T, - CvMat* R1, CvMat* R2, CvMat* P1, CvMat* P2, - CvMat* Q CV_DEFAULT(0), - int flags CV_DEFAULT(CV_CALIB_ZERO_DISPARITY), - double alpha CV_DEFAULT(-1), - CvSize new_image_size CV_DEFAULT(cvSize(0,0)), - CvRect* valid_pix_ROI1 CV_DEFAULT(0), - CvRect* valid_pix_ROI2 CV_DEFAULT(0)); - -/* Computes rectification transformations for uncalibrated pair of images using a set - of point correspondences */ -int cvStereoRectifyUncalibrated( const CvMat* points1, const CvMat* points2, - const CvMat* F, CvSize img_size, - CvMat* H1, CvMat* H2, - double threshold CV_DEFAULT(5)); - -/** @brief Computes the original (undistorted) feature coordinates - from the observed (distorted) coordinates -@see cv::undistortPoints -*/ -void cvUndistortPoints( const CvMat* src, CvMat* dst, - const CvMat* camera_matrix, - const CvMat* dist_coeffs, - const CvMat* R CV_DEFAULT(0), - const CvMat* P CV_DEFAULT(0)); - #ifdef __cplusplus } // extern "C" #endif diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index a10d83b7eb..58544909a2 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1225,575 +1225,6 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i return std::sqrt(reprojErr/(pointsTotal*2)); } -double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, - const CvMat* _imagePoints2, const CvMat* _npoints, - CvMat* _cameraMatrix1, CvMat* _distCoeffs1, - CvMat* _cameraMatrix2, CvMat* _distCoeffs2, - CvSize imageSize, CvMat* matR, CvMat* matT, - CvMat* matE, CvMat* matF, - int flags, - CvTermCriteria termCrit ) -{ - return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1, - _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE, - matF, NULL, NULL, NULL, flags, termCrit); -} - -static void -icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs, - const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize, - cv::Rect_& inner, cv::Rect_& outer ) -{ - const int N = 9; - int x, y, k; - cv::Ptr _pts(cvCreateMat(1, N*N, CV_64FC2)); - CvPoint2D64f* pts = (CvPoint2D64f*)(_pts->data.ptr); - - for( y = k = 0; y < N; y++ ) - for( x = 0; x < N; x++ ) - pts[k++] = cvPoint2D64f((double)x*(imgSize.width-1)/(N-1), - (double)y*(imgSize.height-1)/(N-1)); - - cvUndistortPoints(_pts, _pts, cameraMatrix, distCoeffs, R, newCameraMatrix); - - double iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX; - double oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX; - // find the inscribed rectangle. - // the code will likely not work with extreme rotation matrices (R) (>45%) - for( y = k = 0; y < N; y++ ) - for( x = 0; x < N; x++ ) - { - CvPoint2D64f p = pts[k++]; - oX0 = MIN(oX0, p.x); - oX1 = MAX(oX1, p.x); - oY0 = MIN(oY0, p.y); - oY1 = MAX(oY1, p.y); - - if( x == 0 ) - iX0 = MAX(iX0, p.x); - if( x == N-1 ) - iX1 = MIN(iX1, p.x); - if( y == 0 ) - iY0 = MAX(iY0, p.y); - if( y == N-1 ) - iY1 = MIN(iY1, p.y); - } - inner = cv::Rect_(iX0, iY0, iX1-iX0, iY1-iY0); - outer = cv::Rect_(oX0, oY0, oX1-oX0, oY1-oY0); -} - - -void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, - const CvMat* _distCoeffs1, const CvMat* _distCoeffs2, - CvSize imageSize, const CvMat* matR, const CvMat* matT, - CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, - CvMat* matQ, int flags, double alpha, CvSize newImgSize, - CvRect* roi1, CvRect* roi2 ) -{ - double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4]; - double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; - cv::Rect_ inner1, inner2, outer1, outer2; - - CvMat om = cvMat(3, 1, CV_64F, _om); - CvMat t = cvMat(3, 1, CV_64F, _t); - CvMat uu = cvMat(3, 1, CV_64F, _uu); - CvMat r_r = cvMat(3, 3, CV_64F, _r_r); - CvMat pp = cvMat(3, 4, CV_64F, _pp); - CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps - CvMat wR = cvMat(3, 3, CV_64F, _wr); - CvMat Z = cvMat(3, 1, CV_64F, _z); - CvMat Ri = cvMat(3, 3, CV_64F, _ri); - double nx = imageSize.width, ny = imageSize.height; - int i, k; - - if( matR->rows == 3 && matR->cols == 3 ) - Rodrigues(cvarrToMat(matR), cvarrToMat(&om)); // get vector rotation - else - cvConvert(matR, &om); // it's already a rotation vector - cvConvertScale(&om, &om, -0.5); // get average rotation - Rodrigues(cvarrToMat(&om), cvarrToMat(&r_r)); // rotate cameras to same orientation by averaging - cvMatMul(&r_r, matT, &t); - - int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; - double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); - _uu[idx] = c > 0 ? 1 : -1; - - CV_Assert(nt > 0.0); - - // calculate global Z rotation - cvCrossProduct(&t,&uu,&ww); - double nw = cvNorm(&ww, 0, CV_L2); - if (nw > 0.0) - cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); - Rodrigues(cvarrToMat(&ww), cvarrToMat(&wR)); - - // apply to both views - cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); - cvConvert( &Ri, _R1 ); - cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0); - cvConvert( &Ri, _R2 ); - cvMatMul(&Ri, matT, &t); - - // calculate projection/camera matrices - // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) - double fc_new = DBL_MAX; - CvPoint2D64f cc_new[2] = {}; - - newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; - const double ratio_x = (double)newImgSize.width / imageSize.width / 2; - const double ratio_y = (double)newImgSize.height / imageSize.height / 2; - const double ratio = idx == 1 ? ratio_x : ratio_y; - fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio; - - for( k = 0; k < 2; k++ ) - { - const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2; - const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2; - CvPoint2D32f _pts[4] = {}; - CvPoint3D32f _pts_3[4] = {}; - CvMat pts = cvMat(1, 4, CV_32FC2, _pts); - CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); - - for( i = 0; i < 4; i++ ) - { - int j = (i<2) ? 0 : 1; - _pts[i].x = (float)((i % 2)*(nx-1)); - _pts[i].y = (float)(j*(ny-1)); - } - cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 ); - cvConvertPointsHomogeneous( &pts, &pts_3 ); - - //Change camera matrix to have cc=[0,0] and fc = fc_new - double _a_tmp[3][3]; - CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp); - _a_tmp[0][0]=fc_new; - _a_tmp[1][1]=fc_new; - _a_tmp[0][2]=0.0; - _a_tmp[1][2]=0.0; - projectPoints( cvarrToMat(&pts_3), cvarrToMat(k == 0 ? _R1 : _R2), cvarrToMat(&Z), - cvarrToMat(&A_tmp), noArray(), cvarrToMat(&pts) ); - CvScalar avg = cvAvg(&pts); - cc_new[k].x = (nx-1)/2 - avg.val[0]; - cc_new[k].y = (ny-1)/2 - avg.val[1]; - } - - // vertical focal length must be the same for both images to keep the epipolar constraint - // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) - // use fy for fx also, for simplicity - - // For simplicity, set the principal points for both cameras to be the average - // of the two principal points (either one of or both x- and y- coordinates) - if( flags & CALIB_ZERO_DISPARITY ) - { - cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; - cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; - } - else if( idx == 0 ) // horizontal stereo - cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; - else // vertical stereo - cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; - - cvZero( &pp ); - _pp[0][0] = _pp[1][1] = fc_new; - _pp[0][2] = cc_new[0].x; - _pp[1][2] = cc_new[0].y; - _pp[2][2] = 1; - cvConvert(&pp, _P1); - - _pp[0][2] = cc_new[1].x; - _pp[1][2] = cc_new[1].y; - _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length - cvConvert(&pp, _P2); - - alpha = MIN(alpha, 1.); - - icvGetRectangles( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 ); - icvGetRectangles( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 ); - - { - newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; - double cx1_0 = cc_new[0].x; - double cy1_0 = cc_new[0].y; - double cx2_0 = cc_new[1].x; - double cy2_0 = cc_new[1].y; - double cx1 = newImgSize.width*cx1_0/imageSize.width; - double cy1 = newImgSize.height*cy1_0/imageSize.height; - double cx2 = newImgSize.width*cx2_0/imageSize.width; - double cy2 = newImgSize.height*cy2_0/imageSize.height; - double s = 1.; - - if( alpha >= 0 ) - { - double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), - (double)(newImgSize.width - 1 - cx1)/(inner1.x + inner1.width - cx1_0)), - (double)(newImgSize.height - 1 - cy1)/(inner1.y + inner1.height - cy1_0)); - s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)), - (double)(newImgSize.width - 1 - cx2)/(inner2.x + inner2.width - cx2_0)), - (double)(newImgSize.height - 1 - cy2)/(inner2.y + inner2.height - cy2_0)), - s0); - - double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), - (double)(newImgSize.width - 1 - cx1)/(outer1.x + outer1.width - cx1_0)), - (double)(newImgSize.height - 1 - cy1)/(outer1.y + outer1.height - cy1_0)); - s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), - (double)(newImgSize.width - 1 - cx2)/(outer2.x + outer2.width - cx2_0)), - (double)(newImgSize.height - 1 - cy2)/(outer2.y + outer2.height - cy2_0)), - s1); - - s = s0*(1 - alpha) + s1*alpha; - } - - fc_new *= s; - cc_new[0] = cvPoint2D64f(cx1, cy1); - cc_new[1] = cvPoint2D64f(cx2, cy2); - - cvmSet(_P1, 0, 0, fc_new); - cvmSet(_P1, 1, 1, fc_new); - cvmSet(_P1, 0, 2, cx1); - cvmSet(_P1, 1, 2, cy1); - - cvmSet(_P2, 0, 0, fc_new); - cvmSet(_P2, 1, 1, fc_new); - cvmSet(_P2, 0, 2, cx2); - cvmSet(_P2, 1, 2, cy2); - cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3)); - - if(roi1) - { - *roi1 = cvRect( - cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1), - cvCeil((inner1.y - cy1_0)*s + cy1), - cvFloor(inner1.width*s), cvFloor(inner1.height*s)) - & cv::Rect(0, 0, newImgSize.width, newImgSize.height) - ); - } - - if(roi2) - { - *roi2 = cvRect( - cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2), - cvCeil((inner2.y - cy2_0)*s + cy2), - cvFloor(inner2.width*s), cvFloor(inner2.height*s)) - & cv::Rect(0, 0, newImgSize.width, newImgSize.height) - ); - } - } - - if( matQ ) - { - double q[] = - { - 1, 0, 0, -cc_new[0].x, - 0, 1, 0, -cc_new[0].y, - 0, 0, 0, fc_new, - 0, 0, -1./_t[idx], - (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx] - }; - CvMat Q = cvMat(4, 4, CV_64F, q); - cvConvert( &Q, matQ ); - } -} - - -CV_IMPL int cvStereoRectifyUncalibrated( - const CvMat* _points1, const CvMat* _points2, - const CvMat* F0, CvSize imgSize, - CvMat* _H1, CvMat* _H2, double threshold ) -{ - Ptr _m1, _m2, _lines1, _lines2; - - int i, j, npoints; - double cx, cy; - double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3] = {0}; - CvMat E2 = cvMat( 3, 1, CV_64F, e2 ); - CvMat U = cvMat( 3, 3, CV_64F, u ); - CvMat V = cvMat( 3, 3, CV_64F, v ); - CvMat W = cvMat( 3, 3, CV_64F, w ); - CvMat F = cvMat( 3, 3, CV_64F, f ); - CvMat H1 = cvMat( 3, 3, CV_64F, h1 ); - CvMat H2 = cvMat( 3, 3, CV_64F, h2 ); - CvMat H0 = cvMat( 3, 3, CV_64F, h0 ); - - CvPoint2D64f* m1; - CvPoint2D64f* m2; - CvPoint3D64f* lines1; - CvPoint3D64f* lines2; - - CV_Assert( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) && - CV_ARE_SIZES_EQ(_points1, _points2) ); - - npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2; - - _m1.reset(cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) )); - _m2.reset(cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) )); - _lines1.reset(cvCreateMat( 1, npoints, CV_64FC3 )); - _lines2.reset(cvCreateMat( 1, npoints, CV_64FC3 )); - - cvConvert( F0, &F ); - - cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T ); - W.data.db[8] = 0.; - cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T ); - cvMatMul( &W, &V, &F ); - - cx = cvRound( (imgSize.width-1)*0.5 ); - cy = cvRound( (imgSize.height-1)*0.5 ); - - cvZero( _H1 ); - cvZero( _H2 ); - - cvConvert( _points1, _m1 ); - cvConvert( _points2, _m2 ); - cvReshape( _m1, _m1, 2, 1 ); - cvReshape( _m2, _m2, 2, 1 ); - - m1 = (CvPoint2D64f*)_m1->data.ptr; - m2 = (CvPoint2D64f*)_m2->data.ptr; - lines1 = (CvPoint3D64f*)_lines1->data.ptr; - lines2 = (CvPoint3D64f*)_lines2->data.ptr; - - if( threshold > 0 ) - { - cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 ); - cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 ); - - // measure distance from points to the corresponding epilines, mark outliers - for( i = j = 0; i < npoints; i++ ) - { - if( fabs(m1[i].x*lines2[i].x + - m1[i].y*lines2[i].y + - lines2[i].z) <= threshold && - fabs(m2[i].x*lines1[i].x + - m2[i].y*lines1[i].y + - lines1[i].z) <= threshold ) - { - if( j < i ) - { - m1[j] = m1[i]; - m2[j] = m2[i]; - } - j++; - } - } - - npoints = j; - if( npoints == 0 ) - return 0; - } - - _m1->cols = _m2->cols = npoints; - memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); - cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); - - double t[] = - { - 1, 0, -cx, - 0, 1, -cy, - 0, 0, 1 - }; - CvMat T = cvMat(3, 3, CV_64F, t); - cvMatMul( &T, &E2, &E2 ); - - int mirror = e2[0] < 0; - double d = MAX(std::sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON); - double alpha = e2[0]/d; - double beta = e2[1]/d; - double r[] = - { - alpha, beta, 0, - -beta, alpha, 0, - 0, 0, 1 - }; - CvMat R = cvMat(3, 3, CV_64F, r); - cvMatMul( &R, &T, &T ); - cvMatMul( &R, &E2, &E2 ); - double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; - double k[] = - { - 1, 0, 0, - 0, 1, 0, - invf, 0, 1 - }; - CvMat K = cvMat(3, 3, CV_64F, k); - cvMatMul( &K, &T, &H2 ); - cvMatMul( &K, &E2, &E2 ); - - double it[] = - { - 1, 0, cx, - 0, 1, cy, - 0, 0, 1 - }; - CvMat iT = cvMat( 3, 3, CV_64F, it ); - cvMatMul( &iT, &H2, &H2 ); - - memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); - cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); - - double e2_x[] = - { - 0, -e2[2], e2[1], - e2[2], 0, -e2[0], - -e2[1], e2[0], 0 - }; - double e2_111[] = - { - e2[0], e2[0], e2[0], - e2[1], e2[1], e2[1], - e2[2], e2[2], e2[2], - }; - CvMat E2_x = cvMat(3, 3, CV_64F, e2_x); - CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111); - cvMatMulAdd(&E2_x, &F, &E2_111, &H0 ); - cvMatMul(&H2, &H0, &H0); - CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6); - cvMatMul(&H0, &E1, &E1); - - cvPerspectiveTransform( _m1, _m1, &H0 ); - cvPerspectiveTransform( _m2, _m2, &H2 ); - CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B; - double x[3] = {0}; - CvMat X = cvMat( 3, 1, CV_64F, x ); - cvConvertPointsHomogeneous( _m1, &A ); - cvReshape( &A, &A, 1, npoints ); - cvReshape( _m2, &BxBy, 1, npoints ); - cvGetCol( &BxBy, &B, 0 ); - cvSolve( &A, &B, &X, CV_SVD ); - - double ha[] = - { - x[0], x[1], x[2], - 0, 1, 0, - 0, 0, 1 - }; - CvMat Ha = cvMat(3, 3, CV_64F, ha); - cvMatMul( &Ha, &H0, &H1 ); - cvPerspectiveTransform( _m1, _m1, &Ha ); - - if( mirror ) - { - double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; - CvMat MM = cvMat(3, 3, CV_64F, mm); - cvMatMul( &MM, &H1, &H1 ); - cvMatMul( &MM, &H2, &H2 ); - } - - cvConvert( &H1, _H1 ); - cvConvert( &H2, _H2 ); - - return 1; -} - - -void cv::reprojectImageTo3D( InputArray _disparity, - OutputArray __3dImage, InputArray _Qmat, - bool handleMissingValues, int dtype ) -{ - CV_INSTRUMENT_REGION(); - - Mat disparity = _disparity.getMat(), Q = _Qmat.getMat(); - int stype = disparity.type(); - - CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 || - stype == CV_32SC1 || stype == CV_32FC1 ); - CV_Assert( Q.size() == Size(4,4) ); - - if( dtype >= 0 ) - dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3); - - if( __3dImage.fixedType() ) - { - int dtype_ = __3dImage.type(); - CV_Assert( dtype == -1 || dtype == dtype_ ); - dtype = dtype_; - } - - if( dtype < 0 ) - dtype = CV_32FC3; - else - CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 ); - - __3dImage.create(disparity.size(), dtype); - Mat _3dImage = __3dImage.getMat(); - - const float bigZ = 10000.f; - Matx44d _Q; - Q.convertTo(_Q, CV_64F); - - int x, cols = disparity.cols; - CV_Assert( cols >= 0 ); - - std::vector _sbuf(cols); - std::vector _dbuf(cols); - float* sbuf = &_sbuf[0]; - Vec3f* dbuf = &_dbuf[0]; - double minDisparity = FLT_MAX; - - // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined. - // and we set the corresponding Z's to some fixed big value. - if( handleMissingValues ) - cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 ); - - for( int y = 0; y < disparity.rows; y++ ) - { - float* sptr = sbuf; - Vec3f* dptr = dbuf; - - if( stype == CV_8UC1 ) - { - const uchar* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else if( stype == CV_16SC1 ) - { - const short* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else if( stype == CV_32SC1 ) - { - const int* sptr0 = disparity.ptr(y); - for( x = 0; x < cols; x++ ) - sptr[x] = (float)sptr0[x]; - } - else - sptr = disparity.ptr(y); - - if( dtype == CV_32FC3 ) - dptr = _3dImage.ptr(y); - - for( x = 0; x < cols; x++) - { - double d = sptr[x]; - Vec4d homg_pt = _Q*Vec4d(x, y, d, 1.0); - dptr[x] = Vec3d(homg_pt.val); - dptr[x] /= homg_pt[3]; - - if( fabs(d-minDisparity) <= FLT_EPSILON ) - dptr[x][2] = bigZ; - } - - if( dtype == CV_16SC3 ) - { - Vec3s* dptr0 = _3dImage.ptr(y); - for( x = 0; x < cols; x++ ) - { - dptr0[x] = dptr[x]; - } - } - else if( dtype == CV_32SC3 ) - { - Vec3i* dptr0 = _3dImage.ptr(y); - for( x = 0; x < cols; x++ ) - { - dptr0[x] = dptr[x]; - } - } - } -} - - namespace cv { @@ -2351,195 +1782,4 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, return err; } - -void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, - InputArray _cameraMatrix2, InputArray _distCoeffs2, - Size imageSize, InputArray _Rmat, InputArray _Tmat, - OutputArray _Rmat1, OutputArray _Rmat2, - OutputArray _Pmat1, OutputArray _Pmat2, - OutputArray _Qmat, int flags, - double alpha, Size newImageSize, - Rect* validPixROI1, Rect* validPixROI2 ) -{ - Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat(); - Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat(); - Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat(); - CvMat c_cameraMatrix1 = cvMat(cameraMatrix1); - CvMat c_cameraMatrix2 = cvMat(cameraMatrix2); - CvMat c_distCoeffs1 = cvMat(distCoeffs1); - CvMat c_distCoeffs2 = cvMat(distCoeffs2); - CvMat c_R = cvMat(Rmat), c_T = cvMat(Tmat); - - int rtype = CV_64F; - _Rmat1.create(3, 3, rtype); - _Rmat2.create(3, 3, rtype); - _Pmat1.create(3, 4, rtype); - _Pmat2.create(3, 4, rtype); - Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q; - CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2); - CvMat c_Q, *p_Q = 0; - - if( _Qmat.needed() ) - { - _Qmat.create(4, 4, rtype); - p_Q = &(c_Q = cvMat(Q = _Qmat.getMat())); - } - - CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1; - CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2; - cvStereoRectify( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2, - cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha, - cvSize(newImageSize), (CvRect*)validPixROI1, (CvRect*)validPixROI2); -} - -bool cv::stereoRectifyUncalibrated( InputArray _points1, InputArray _points2, - InputArray _Fmat, Size imgSize, - OutputArray _Hmat1, OutputArray _Hmat2, double threshold ) -{ - CV_INSTRUMENT_REGION(); - - int rtype = CV_64F; - _Hmat1.create(3, 3, rtype); - _Hmat2.create(3, 3, rtype); - Mat F = _Fmat.getMat(); - Mat points1 = _points1.getMat(), points2 = _points2.getMat(); - CvMat c_pt1 = cvMat(points1), c_pt2 = cvMat(points2); - Mat H1 = _Hmat1.getMat(), H2 = _Hmat2.getMat(); - CvMat c_F, *p_F=0, c_H1 = cvMat(H1), c_H2 = cvMat(H2); - if( F.size() == Size(3, 3) ) - p_F = &(c_F = cvMat(F)); - return cvStereoRectifyUncalibrated(&c_pt1, &c_pt2, p_F, cvSize(imgSize), &c_H1, &c_H2, threshold) > 0; -} - -namespace cv -{ - -static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, - InputArrayOfArrays _imgpt3_0, - const Mat& cameraMatrix1, const Mat& distCoeffs1, - const Mat& cameraMatrix3, const Mat& distCoeffs3, - const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 ) -{ - size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total(); - std::vector imgpt1, imgpt3; - - for( int i = 0; i < (int)std::min(n1, n3); i++ ) - { - Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i); - int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F); - CV_Assert( ni1 > 0 && ni1 == ni3 ); - const Point2f* pt1data = pt1.ptr(); - const Point2f* pt3data = pt3.ptr(); - std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1)); - std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3)); - } - - undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1); - undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3); - - double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0; - size_t n = imgpt1.size(); - CV_DbgAssert(n > 0); - - for( size_t i = 0; i < n; i++ ) - { - double y1 = imgpt3[i].y, y2 = imgpt1[i].y; - - y1_ += y1; y2_ += y2; - y1y1_ += y1*y1; y1y2_ += y1*y2; - } - - y1_ /= n; - y2_ /= n; - y1y1_ /= n; - y1y2_ /= n; - - double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_); - double b = y2_ - a*y1_; - - P3.at(0,0) *= a; - P3.at(1,1) *= a; - P3.at(0,2) = P3.at(0,2)*a; - P3.at(1,2) = P3.at(1,2)*a + b; - P3.at(0,3) *= a; - P3.at(1,3) *= a; -} - -} - -float cv::rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1, - InputArray _cameraMatrix2, InputArray _distCoeffs2, - InputArray _cameraMatrix3, InputArray _distCoeffs3, - InputArrayOfArrays _imgpt1, - InputArrayOfArrays _imgpt3, - Size imageSize, InputArray _Rmat12, InputArray _Tmat12, - InputArray _Rmat13, InputArray _Tmat13, - OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3, - OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3, - OutputArray _Qmat, - double alpha, Size newImgSize, - Rect* roi1, Rect* roi2, int flags ) -{ - // first, rectify the 1-2 stereo pair - stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, - imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat, - flags, alpha, newImgSize, roi1, roi2 ); - - Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat(); - - _Rmat3.create(3, 3, CV_64F); - _Pmat3.create(3, 4, CV_64F); - - Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(); - Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat(); - - // recompute rectification transforms for cameras 1 & 2. - Mat om, r_r, r_r13; - - if( R13.size() != Size(3,3) ) - Rodrigues(R13, r_r13); - else - R13.copyTo(r_r13); - - if( R12.size() == Size(3,3) ) - Rodrigues(R12, om); - else - R12.copyTo(om); - - om *= -0.5; - Rodrigues(om, r_r); // rotate cameras to same orientation by averaging - Mat_ t12 = r_r * T12; - - int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1; - double c = t12(idx,0), nt = norm(t12, CV_L2); - CV_Assert(fabs(nt) > 0); - Mat_ uu = Mat_::zeros(3,1); - uu(idx, 0) = c > 0 ? 1 : -1; - - // calculate global Z rotation - Mat_ ww = t12.cross(uu), wR; - double nw = norm(ww, CV_L2); - CV_Assert(fabs(nw) > 0); - ww *= acos(fabs(c)/nt)/nw; - Rodrigues(ww, wR); - - // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2. - R3 = wR*r_r.t()*r_r13.t(); - Mat_ t13 = R3 * T13; - - P2.copyTo(P3); - Mat t = P3.col(3); - t13.copyTo(t); - P3.at(0,3) *= P3.at(0,0); - P3.at(1,3) *= P3.at(1,1); - - if( !_imgpt1.empty() && !_imgpt3.empty() ) - adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(), - _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3); - - return (float)((P3.at(idx,3)/P3.at(idx,idx))/ - (P2.at(idx,3)/P2.at(idx,idx))); -} - - /* End of file. */ diff --git a/modules/calib3d/src/calibration_base.cpp b/modules/calib3d/src/calibration_base.cpp index c047a45e8e..166f297917 100644 --- a/modules/calib3d/src/calibration_base.cpp +++ b/modules/calib3d/src/calibration_base.cpp @@ -43,7 +43,8 @@ #include "precomp.hpp" #include "hal_replacement.hpp" #include "distortion_model.hpp" -#include "calib3d_c_api.h" +#include "opencv2/calib3d/calib3d_c.h" +#include "opencv2/core/core_c.h" #include #include @@ -1520,7 +1521,7 @@ void cv::projectPoints( InputArray _opoints, } } -static void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, +void cv::getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray R, InputArray newCameraMatrix, Size imgSize, Rect_& inner, Rect_& outer ) { diff --git a/modules/calib3d/src/compat_ptsetreg.cpp b/modules/calib3d/src/compat_ptsetreg.cpp index ba103d7b8d..fec3f3a420 100644 --- a/modules/calib3d/src/compat_ptsetreg.cpp +++ b/modules/calib3d/src/compat_ptsetreg.cpp @@ -357,80 +357,3 @@ CV_IMPL int cvFindHomography( const CvMat* _src, const CvMat* _dst, CvMat* __H, H0.convertTo(H, H.type()); return 1; } - - -CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID, - const CvMat* fmatrix, CvMat* _lines ) -{ - cv::Mat pt = cv::cvarrToMat(points), fm = cv::cvarrToMat(fmatrix); - cv::Mat lines = cv::cvarrToMat(_lines); - const cv::Mat lines0 = lines; - - if( pt.channels() == 1 && (pt.rows == 2 || pt.rows == 3) && pt.cols > 3 ) - cv::transpose(pt, pt); - - cv::computeCorrespondEpilines(pt, pointImageID, fm, lines); - - bool tflag = lines0.channels() == 1 && lines0.rows == 3 && lines0.cols > 3; - lines = lines.reshape(lines0.channels(), (tflag ? lines0.cols : lines0.rows)); - - if( tflag ) - { - CV_Assert( lines.rows == lines0.cols && lines.cols == lines0.rows ); - if( lines0.type() == lines.type() ) - transpose( lines, lines0 ); - else - { - transpose( lines, lines ); - lines.convertTo( lines0, lines0.type() ); - } - } - else - { - CV_Assert( lines.size() == lines0.size() ); - if( lines.data != lines0.data ) - lines.convertTo(lines0, lines0.type()); - } -} - - -CV_IMPL void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst ) -{ - cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst); - const cv::Mat dst0 = dst; - - int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows); - - if( src.channels() == 1 && src.cols > d0 ) - cv::transpose(src, src); - - int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows); - - if( d0 == d1 ) - src.copyTo(dst); - else if( d0 < d1 ) - cv::convertPointsToHomogeneous(src, dst); - else - cv::convertPointsFromHomogeneous(src, dst); - - bool tflag = dst0.channels() == 1 && dst0.cols > d1; - dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows)); - - if( tflag ) - { - CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows ); - if( dst0.type() == dst.type() ) - transpose( dst, dst0 ); - else - { - transpose( dst, dst ); - dst.convertTo( dst0, dst0.type() ); - } - } - else - { - CV_Assert( dst.size() == dst0.size() ); - if( dst.data != dst0.data ) - dst.convertTo(dst0, dst0.type()); - } -} diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 4aec4324e0..7bbddf2838 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -698,90 +698,6 @@ void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, Input } -////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// cv::fisheye::stereoRectify - -void cv::fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size& imageSize, - InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, - OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale) -{ - CV_INSTRUMENT_REGION(); - - CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F)); - CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); - - - cv::Mat aaa = _tvec.getMat().reshape(3, 1); - - Vec3d rvec; // Rodrigues vector - if (_R.size() == Size(3, 3)) - { - cv::Matx33d rmat; - _R.getMat().convertTo(rmat, CV_64F); - rvec = Affine3d(rmat).rvec(); - } - else if (_R.total() * _R.channels() == 3) - _R.getMat().convertTo(rvec, CV_64F); - - Vec3d tvec; - _tvec.getMat().convertTo(tvec, CV_64F); - - // rectification algorithm - rvec *= -0.5; // get average rotation - - Matx33d r_r; - Rodrigues(rvec, r_r); // rotate cameras to same orientation by averaging - - Vec3d t = r_r * tvec; - Vec3d uu(t[0] > 0 ? 1 : -1, 0, 0); - - // calculate global Z rotation - Vec3d ww = t.cross(uu); - double nw = norm(ww); - if (nw > 0.0) - ww *= acos(fabs(t[0])/cv::norm(t))/nw; - - Matx33d wr; - Rodrigues(ww, wr); - - // apply to both views - Matx33d ri1 = wr * r_r.t(); - Mat(ri1, false).convertTo(R1, R1.empty() ? CV_64F : R1.type()); - Matx33d ri2 = wr * r_r; - Mat(ri2, false).convertTo(R2, R2.empty() ? CV_64F : R2.type()); - Vec3d tnew = ri2 * tvec; - - // calculate projection/camera matrices. these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) - Matx33d newK1, newK2; - estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale); - estimateNewCameraMatrixForUndistortRectify(K2, D2, imageSize, R2, newK2, balance, newImageSize, fov_scale); - - double fc_new = std::min(newK1(1,1), newK2(1,1)); - Point2d cc_new[2] = { Vec2d(newK1(0, 2), newK1(1, 2)), Vec2d(newK2(0, 2), newK2(1, 2)) }; - - // Vertical focal length must be the same for both images to keep the epipolar constraint use fy for fx also. - // For simplicity, set the principal points for both cameras to be the average - // of the two principal points (either one of or both x- and y- coordinates) - if( flags & cv::CALIB_ZERO_DISPARITY ) - cc_new[0] = cc_new[1] = (cc_new[0] + cc_new[1]) * 0.5; - else - cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; - - Mat(Matx34d(fc_new, 0, cc_new[0].x, 0, - 0, fc_new, cc_new[0].y, 0, - 0, 0, 1, 0), false).convertTo(P1, P1.empty() ? CV_64F : P1.type()); - - Mat(Matx34d(fc_new, 0, cc_new[1].x, tnew[0]*fc_new, // baseline * focal length;, - 0, fc_new, cc_new[1].y, 0, - 0, 0, 1, 0), false).convertTo(P2, P2.empty() ? CV_64F : P2.type()); - - if (Q.needed()) - Mat(Matx44d(1, 0, 0, -cc_new[0].x, - 0, 1, 0, -cc_new[0].y, - 0, 0, 0, fc_new, - 0, 0, -1./tnew[0], (cc_new[0].x - cc_new[1].x)/tnew[0]), false).convertTo(Q, Q.empty() ? CV_64F : Q.depth()); -} - ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// cv::fisheye::calibrate diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index 05811e0907..118e9dc974 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -150,6 +150,10 @@ void projectPoints( InputArray objectPoints, OutputArray dpdc=noArray(), OutputArray dpdk=noArray(), OutputArray dpdo=noArray(), double aspectRatio=0.); +void getUndistortRectangles(InputArray _cameraMatrix, InputArray _distCoeffs, + InputArray R, InputArray newCameraMatrix, Size imgSize, + Rect_& inner, Rect_& outer ); + } // namespace cv int checkChessboardBinary(const cv::Mat & img, const cv::Size & size); diff --git a/modules/calib3d/src/stereo_geom.cpp b/modules/calib3d/src/stereo_geom.cpp new file mode 100644 index 0000000000..4c91522bdc --- /dev/null +++ b/modules/calib3d/src/stereo_geom.cpp @@ -0,0 +1,713 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "precomp.hpp" + +namespace cv { + +void reprojectImageTo3D( InputArray _disparity, OutputArray __3dImage, + InputArray _Qmat, bool handleMissingValues, int dtype ) +{ + CV_INSTRUMENT_REGION(); + + Mat disparity = _disparity.getMat(), Q = _Qmat.getMat(); + int stype = disparity.type(); + + CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 || + stype == CV_32SC1 || stype == CV_32FC1 ); + CV_Assert( Q.size() == Size(4,4) ); + + if( dtype >= 0 ) + dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3); + + if( __3dImage.fixedType() ) + { + int dtype_ = __3dImage.type(); + CV_Assert( dtype == -1 || dtype == dtype_ ); + dtype = dtype_; + } + + if( dtype < 0 ) + dtype = CV_32FC3; + else + CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 ); + + __3dImage.create(disparity.size(), dtype); + Mat _3dImage = __3dImage.getMat(); + + const float bigZ = 10000.f; + Matx44d _Q; + Q.convertTo(_Q, CV_64F); + + int x, cols = disparity.cols; + CV_Assert( cols >= 0 ); + + std::vector _sbuf(cols); + std::vector _dbuf(cols); + float* sbuf = &_sbuf[0]; + Vec3f* dbuf = &_dbuf[0]; + double minDisparity = FLT_MAX; + + // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined. + // and we set the corresponding Z's to some fixed big value. + if( handleMissingValues ) + cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 ); + + for( int y = 0; y < disparity.rows; y++ ) + { + float* sptr = sbuf; + Vec3f* dptr = dbuf; + + if( stype == CV_8UC1 ) + { + const uchar* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else if( stype == CV_16SC1 ) + { + const short* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else if( stype == CV_32SC1 ) + { + const int* sptr0 = disparity.ptr(y); + for( x = 0; x < cols; x++ ) + sptr[x] = (float)sptr0[x]; + } + else + sptr = disparity.ptr(y); + + if( dtype == CV_32FC3 ) + dptr = _3dImage.ptr(y); + + for( x = 0; x < cols; x++) + { + double d = sptr[x]; + Vec4d homg_pt = _Q*Vec4d(x, y, d, 1.0); + dptr[x] = Vec3d(homg_pt.val); + dptr[x] /= homg_pt[3]; + + if( fabs(d-minDisparity) <= FLT_EPSILON ) + dptr[x][2] = bigZ; + } + + if( dtype == CV_16SC3 ) + { + Vec3s* dptr0 = _3dImage.ptr(y); + for( x = 0; x < cols; x++ ) + { + dptr0[x] = dptr[x]; + } + } + else if( dtype == CV_32SC3 ) + { + Vec3i* dptr0 = _3dImage.ptr(y); + for( x = 0; x < cols; x++ ) + { + dptr0[x] = dptr[x]; + } + } + } +} + +void stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, + Size imageSize, InputArray R, InputArray T, + OutputArray _R1, OutputArray _R2, + OutputArray _P1, OutputArray _P2, + OutputArray _Qmat, int flags, + double alpha, Size newImgSize, + Rect* roi1, Rect* roi2 ) +{ + Mat matR = Mat_(R.getMat()), matT = Mat_(T.getMat()); + + Mat om, r_r; + Mat Z = Mat::zeros(3, 1, CV_64F); + double nx = imageSize.width, ny = imageSize.height; + + if( matR.rows == 3 && matR.cols == 3 ) + Rodrigues(matR, om); // get vector rotation + else + matR.copyTo(om); + om *= -0.5; // get average rotation + Rodrigues(om, r_r); + Mat t = r_r * matT; // rotate cameras to same orientation by averaging + + int idx = fabs(t.at(0)) > fabs(t.at(1)) ? 0 : 1; + double c = t.at(idx), nt = norm(t, NORM_L2); + double _uu[3]={0, 0, 0}; + _uu[idx] = c > 0 ? 1 : -1; + + CV_Assert(nt > 0.0); + + // calculate global Z rotation + Mat ww = t.cross(Mat(3, 1, CV_64F, _uu)), wR; + double nw = norm(ww, NORM_L2); + if (nw > 0.0) + ww *= std::acos(fabs(c)/nt)/nw; + Rodrigues(ww, wR); + + Mat Ri; + // apply to both views + gemm(wR, r_r, 1, Mat(), 0, Ri, GEMM_2_T); + Ri.copyTo(_R1); + gemm(wR, r_r, 1, Mat(), 0, Ri, 0); + Ri.copyTo(_R2); + t = Ri * matT; + + // calculate projection/camera matrices + // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) + Point2d cc_new[2]={}; + + newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; + const double ratio_x = (double)newImgSize.width / imageSize.width / 2; + const double ratio_y = (double)newImgSize.height / imageSize.height / 2; + const double ratio = idx == 1 ? ratio_x : ratio_y; + + Mat cameraMatrix1 = Mat_(_cameraMatrix1.getMat()); + Mat cameraMatrix2 = Mat_(_cameraMatrix2.getMat()); + Mat distCoeffs1, distCoeffs2; + if (!_distCoeffs1.empty()) + distCoeffs1 = Mat_(_distCoeffs1.getMat()); + if (!_distCoeffs2.empty()) + distCoeffs2 = Mat_(_distCoeffs2.getMat()); + + double fc_new = (cameraMatrix1.at(idx ^ 1, idx ^ 1) + cameraMatrix2.at(idx ^ 1, idx ^ 1)) * ratio; + + for( int k = 0; k < 2; k++ ) + { + const Mat& A = k == 0 ? cameraMatrix1 : cameraMatrix2; + const Mat& Dk = k == 0 ? distCoeffs1 : distCoeffs2; + Point2f _pts[4] = {}; + Point3f _pts_3[4] = {}; + Mat pts(1, 4, CV_32FC2, _pts); + Mat pts_3(1, 4, CV_32FC3, _pts_3); + + for( int i = 0; i < 4; i++ ) + { + int j = (i<2) ? 0 : 1; + _pts[i].x = (float)((i % 2)*(nx-1)); + _pts[i].y = (float)(j*(ny-1)); + } + undistortPoints(pts, pts, A, Dk, Mat(), Mat()); + convertPointsToHomogeneous(pts, pts_3); + + // Change the camera matrix to have cc=[0,0] and fc = fc_new + double _a_tmp[3][3] = {{fc_new, 0, 0}, {0, fc_new, 0}, {0, 0, 1}}; + Mat A_tmp(3, 3, CV_64F, _a_tmp); + projectPoints(pts_3, (k == 0 ? _R1 : _R2), Z, A_tmp, Mat(), pts); + Scalar avg = mean(pts); + cc_new[k].x = (nx-1)/2 - avg.val[0]; + cc_new[k].y = (ny-1)/2 - avg.val[1]; + } + + // vertical focal length must be the same for both images to keep the epipolar constraint + // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) + // use fy for fx also, for simplicity + + // For simplicity, set the principal points for both cameras to be the average + // of the two principal points (either one of or both x- and y- coordinates) + if( flags & CALIB_ZERO_DISPARITY ) + { + cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + } + else if( idx == 0 ) // horizontal stereo + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + else // vertical stereo + cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; + + double t_idx = t.at(idx); + + Mat pp = Mat::zeros(3, 4, CV_64F); + pp.at(0, 0) = pp.at(1, 1) = fc_new; + pp.at(0, 2) = cc_new[0].x; + pp.at(1, 2) = cc_new[0].y; + pp.at(2, 2) = 1.; + pp.copyTo(_P1); + + pp.at(0, 2) = cc_new[1].x; + pp.at(1, 2) = cc_new[1].y; + pp.at(idx, 3) = t_idx*fc_new; // baseline * focal length + pp.copyTo(_P2); + + alpha = MIN(alpha, 1.); + + cv::Rect_ inner1, inner2, outer1, outer2; + getUndistortRectangles(cameraMatrix1, distCoeffs1, _R1, _P1, imageSize, inner1, outer1); + getUndistortRectangles(cameraMatrix2, distCoeffs2, _R2, _P2, imageSize, inner2, outer2); + + { + newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize; + double cx1_0 = cc_new[0].x; + double cy1_0 = cc_new[0].y; + double cx2_0 = cc_new[1].x; + double cy2_0 = cc_new[1].y; + double cx1 = newImgSize.width*cx1_0/imageSize.width; + double cy1 = newImgSize.height*cy1_0/imageSize.height; + double cx2 = newImgSize.width*cx2_0/imageSize.width; + double cy2 = newImgSize.height*cy2_0/imageSize.height; + double s = 1.; + + if( alpha >= 0 ) + { + double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)), + (double)(newImgSize.width - 1 - cx1)/(inner1.x + inner1.width - cx1_0)), + (double)(newImgSize.height - 1 - cy1)/(inner1.y + inner1.height - cy1_0)); + s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)), + (double)(newImgSize.width - 1 - cx2)/(inner2.x + inner2.width - cx2_0)), + (double)(newImgSize.height - 1 - cy2)/(inner2.y + inner2.height - cy2_0)), + s0); + + double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)), + (double)(newImgSize.width - 1 - cx1)/(outer1.x + outer1.width - cx1_0)), + (double)(newImgSize.height - 1 - cy1)/(outer1.y + outer1.height - cy1_0)); + s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)), + (double)(newImgSize.width - 1 - cx2)/(outer2.x + outer2.width - cx2_0)), + (double)(newImgSize.height - 1 - cy2)/(outer2.y + outer2.height - cy2_0)), + s1); + + s = s0*(1 - alpha) + s1*alpha; + } + + fc_new *= s; + cc_new[0] = Point2d(cx1, cy1); + cc_new[1] = Point2d(cx2, cy2); + + pp.at(0, 0) = pp.at(1, 1) = fc_new; + pp.at(0, 2) = cx2; + pp.at(1, 2) = cy2; + pp.at(idx, 3) *= s; + pp.copyTo(_P2); + + pp.at(0, 2) = cx1; + pp.at(1, 2) = cy1; + pp.at(idx, 3) = 0.; + pp.copyTo(_P1); + + if(roi1) + { + *roi1 = + cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1), + cvCeil((inner1.y - cy1_0)*s + cy1), + cvFloor(inner1.width*s), cvFloor(inner1.height*s)) + & cv::Rect(0, 0, newImgSize.width, newImgSize.height) + ; + } + + if(roi2) + { + *roi2 = + cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2), + cvCeil((inner2.y - cy2_0)*s + cy2), + cvFloor(inner2.width*s), cvFloor(inner2.height*s)) + & cv::Rect(0, 0, newImgSize.width, newImgSize.height) + ; + } + } + + if( _Qmat.needed() ) + { + double q[] = + { + 1, 0, 0, -cc_new[0].x, + 0, 1, 0, -cc_new[0].y, + 0, 0, 0, fc_new, + 0, 0, -1./t_idx, + (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/t_idx + }; + Mat Q(4, 4, CV_64F, q); + Q.copyTo(_Qmat); + } +} + +/* +CV_IMPL int cvStereoRectifyUncalibrated( + const CvMat* _points1, const CvMat* _points2, + const CvMat* F0, CvSize imgSize, + CvMat* _H1, CvMat* _H2, double threshold ) +*/ +bool stereoRectifyUncalibrated( InputArray _points1, InputArray _points2, + InputArray _Fmat, Size imgSize, + OutputArray _Hmat1, OutputArray _Hmat2, double threshold ) +{ + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + CV_Assert( points1.size() == points2.size() ); + + int npoints = points1.checkVector(2); + CV_Assert(npoints > 0); + + Mat _m1, _m2; + + points1.convertTo(_m1, CV_64F); + points2.convertTo(_m2, CV_64F); + _m1 = _m1.reshape(2, 1); + _m2 = _m2.reshape(2, 1); + + Mat F0 = _Fmat.getMat(), F, Wdiag, U, Vt; + F0.convertTo(F, CV_64F); + + SVDecomp(F, Wdiag, U, Vt, 0); + Wdiag.at(2) = 0.; + Mat W = Mat::diag(Wdiag), UW; + gemm(U, W, 1, Mat(), 0, UW); + gemm(UW, Vt, 1, Mat(), 0, F); + + double cx = cvRound( (imgSize.width-1)*0.5 ); + double cy = cvRound( (imgSize.height-1)*0.5 ); + + if( threshold > 0 ) + { + Mat _lines1, _lines2; + computeCorrespondEpilines(_m1, 1, F, _lines1); + computeCorrespondEpilines(_m2, 2, F, _lines2); + CV_Assert(_m1.isContinuous() && _m2.isContinuous() && + _lines1.isContinuous() && _lines2.isContinuous()); + Point2d* m1 = (Point2d*)_m1.data; + Point2d* m2 = (Point2d*)_m2.data; + Point3d* lines1 = (Point3d*)_lines1.data; + Point3d* lines2 = (Point3d*)_lines2.data; + + // measure distance from points to the corresponding epilines, mark outliers + int i, j; + for( i = j = 0; i < npoints; i++ ) + { + if( fabs(m1[i].x*lines2[i].x + + m1[i].y*lines2[i].y + + lines2[i].z) <= threshold && + fabs(m2[i].x*lines1[i].x + + m2[i].y*lines1[i].y + + lines1[i].z) <= threshold ) + { + if( j < i ) + { + m1[j] = m1[i]; + m2[j] = m2[i]; + } + j++; + } + } + + npoints = j; + if( npoints == 0 ) + return false; + _m1.cols = _m2.cols = npoints; + } + + Mat E2 = U.col(2).clone(); + if (E2.at(2) < 0) + E2 *= -1.0; + + double t[] = + { + 1, 0, -cx, + 0, 1, -cy, + 0, 0, 1 + }; + Mat T(3, 3, CV_64F, t); + E2 = T*E2; + + double* e2 = (double*)E2.data; + int mirror = e2[0] < 0; + double d = std::sqrt(e2[0]*e2[0] + e2[1]*e2[1]); + d = MAX(d, DBL_EPSILON); + double alpha = e2[0]/d; + double beta = e2[1]/d; + double r[] = + { + alpha, beta, 0, + -beta, alpha, 0, + 0, 0, 1 + }; + Mat R(3, 3, CV_64F, r); + T = R*T; + E2 = R*E2; + double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; + double k[] = + { + 1, 0, 0, + 0, 1, 0, + invf, 0, 1 + }; + Mat K(3, 3, CV_64F, k); + Mat H2 = K*T; + E2 = K*E2; + + double it[] = + { + 1, 0, cx, + 0, 1, cy, + 0, 0, 1 + }; + Mat iT( 3, 3, CV_64F, it ); + H2 = iT*H2; + + U.col(2).copyTo(E2); + if (E2.at(2) < 0) + E2 *= -1.0; + + double e2_x[] = + { + 0, -e2[2], e2[1], + e2[2], 0, -e2[0], + -e2[1], e2[0], 0 + }; + double e2_111[] = + { + e2[0], e2[0], e2[0], + e2[1], e2[1], e2[1], + e2[2], e2[2], e2[2], + }; + Mat E2_x(3, 3, CV_64F, e2_x); + Mat E2_111(3, 3, CV_64F, e2_111); + Mat H0 = E2_x*F + E2_111; + H0 = H2*H0; + Mat E1(3, 1, CV_64F, (double*)Vt.data+6); + E1 = H0*E1; + + perspectiveTransform( _m1, _m1, H0 ); + perspectiveTransform( _m2, _m2, H2 ); + Mat A, X; + convertPointsToHomogeneous(_m1, A); + A.convertTo(A, CV_64F); + A = A.reshape(1, npoints); + Mat BxBy = _m2.reshape(1, npoints); + Mat B = BxBy.col(0); + solve(A, B, X, DECOMP_SVD); + CV_Assert(X.isContinuous()); + double* x = X.ptr(); + + double ha[] = + { + x[0], x[1], x[2], + 0, 1, 0, + 0, 0, 1 + }; + Mat Ha(3, 3, CV_64F, ha); + Mat H1 = Ha*H0; + perspectiveTransform( _m1, _m1, Ha ); + + if( mirror ) + { + double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; + Mat MM(3, 3, CV_64F, mm); + H1 = MM*H1; + H2 = MM*H2; + } + + H1.copyTo(_Hmat1); + H2.copyTo(_Hmat2); + return true; +} + + +static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0, + InputArrayOfArrays _imgpt3_0, + const Mat& cameraMatrix1, const Mat& distCoeffs1, + const Mat& cameraMatrix3, const Mat& distCoeffs3, + const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 ) +{ + size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total(); + std::vector imgpt1, imgpt3; + + for( int i = 0; i < (int)std::min(n1, n3); i++ ) + { + Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i); + int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F); + CV_Assert( ni1 > 0 && ni1 == ni3 ); + const Point2f* pt1data = pt1.ptr(); + const Point2f* pt3data = pt3.ptr(); + std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1)); + std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3)); + } + + undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1); + undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3); + + double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0; + size_t n = imgpt1.size(); + CV_DbgAssert(n > 0); + + for( size_t i = 0; i < n; i++ ) + { + double y1 = imgpt3[i].y, y2 = imgpt1[i].y; + + y1_ += y1; y2_ += y2; + y1y1_ += y1*y1; y1y2_ += y1*y2; + } + + y1_ /= n; + y2_ /= n; + y1y1_ /= n; + y1y2_ /= n; + + double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_); + double b = y2_ - a*y1_; + + P3.at(0,0) *= a; + P3.at(1,1) *= a; + P3.at(0,2) = P3.at(0,2)*a; + P3.at(1,2) = P3.at(1,2)*a + b; + P3.at(0,3) *= a; + P3.at(1,3) *= a; +} + +float rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, + InputArray _cameraMatrix3, InputArray _distCoeffs3, + InputArrayOfArrays _imgpt1, + InputArrayOfArrays _imgpt3, + Size imageSize, InputArray _Rmat12, InputArray _Tmat12, + InputArray _Rmat13, InputArray _Tmat13, + OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3, + OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3, + OutputArray _Qmat, + double alpha, Size newImgSize, + Rect* roi1, Rect* roi2, int flags ) +{ + // first, rectify the 1-2 stereo pair + stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, + imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat, + flags, alpha, newImgSize, roi1, roi2 ); + + Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat(); + + _Rmat3.create(3, 3, CV_64F); + _Pmat3.create(3, 4, CV_64F); + + Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(); + Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat(); + + // recompute rectification transforms for cameras 1 & 2. + Mat om, r_r, r_r13; + + if( R13.size() != Size(3,3) ) + Rodrigues(R13, r_r13); + else + R13.copyTo(r_r13); + + if( R12.size() == Size(3,3) ) + Rodrigues(R12, om); + else + R12.copyTo(om); + + om *= -0.5; + Rodrigues(om, r_r); // rotate cameras to same orientation by averaging + Mat_ t12 = r_r * T12; + + int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1; + double c = t12(idx,0), nt = norm(t12, NORM_L2); + CV_Assert(fabs(nt) > 0); + Mat_ uu = Mat_::zeros(3,1); + uu(idx, 0) = c > 0 ? 1 : -1; + + // calculate global Z rotation + Mat_ ww = t12.cross(uu), wR; + double nw = norm(ww, NORM_L2); + CV_Assert(fabs(nw) > 0); + ww *= std::acos(fabs(c)/nt)/nw; + Rodrigues(ww, wR); + + // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2. + R3 = wR*r_r.t()*r_r13.t(); + Mat_ t13 = R3 * T13; + + P2.copyTo(P3); + Mat t = P3.col(3); + t13.copyTo(t); + P3.at(0,3) *= P3.at(0,0); + P3.at(1,3) *= P3.at(1,1); + + if( !_imgpt1.empty() && !_imgpt3.empty() ) + adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(), + _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3); + + return (float)((P3.at(idx,3)/P3.at(idx,idx))/ + (P2.at(idx,3)/P2.at(idx,idx))); +} + +void cv::fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size& imageSize, + InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, + OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale) +{ + CV_INSTRUMENT_REGION(); + + CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F)); + CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); + + + Mat aaa = _tvec.getMat().reshape(3, 1); + + Vec3d rvec; // Rodrigues vector + if (_R.size() == Size(3, 3)) + { + Matx33d rmat; + _R.getMat().convertTo(rmat, CV_64F); + rvec = Affine3d(rmat).rvec(); + } + else if (_R.total() * _R.channels() == 3) + _R.getMat().convertTo(rvec, CV_64F); + + Vec3d tvec; + _tvec.getMat().convertTo(tvec, CV_64F); + + // rectification algorithm + rvec *= -0.5; // get average rotation + + Matx33d r_r; + Rodrigues(rvec, r_r); // rotate cameras to same orientation by averaging + + Vec3d t = r_r * tvec; + Vec3d uu(t[0] > 0 ? 1 : -1, 0, 0); + + // calculate global Z rotation + Vec3d ww = t.cross(uu); + double nw = norm(ww); + if (nw > 0.0) + ww *= std::acos(fabs(t[0])/cv::norm(t))/nw; + + Matx33d wr; + Rodrigues(ww, wr); + + // apply to both views + Matx33d ri1 = wr * r_r.t(); + Mat(ri1, false).convertTo(R1, R1.empty() ? CV_64F : R1.type()); + Matx33d ri2 = wr * r_r; + Mat(ri2, false).convertTo(R2, R2.empty() ? CV_64F : R2.type()); + Vec3d tnew = ri2 * tvec; + + // calculate projection/camera matrices. these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) + Matx33d newK1, newK2; + fisheye::estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale); + fisheye::estimateNewCameraMatrixForUndistortRectify(K2, D2, imageSize, R2, newK2, balance, newImageSize, fov_scale); + + double fc_new = std::min(newK1(1,1), newK2(1,1)); + Point2d cc_new[2] = { Vec2d(newK1(0, 2), newK1(1, 2)), Vec2d(newK2(0, 2), newK2(1, 2)) }; + + // Vertical focal length must be the same for both images to keep the epipolar constraint use fy for fx also. + // For simplicity, set the principal points for both cameras to be the average + // of the two principal points (either one of or both x- and y- coordinates) + if( flags & CALIB_ZERO_DISPARITY ) + cc_new[0] = cc_new[1] = (cc_new[0] + cc_new[1]) * 0.5; + else + cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; + + Mat(Matx34d(fc_new, 0, cc_new[0].x, 0, + 0, fc_new, cc_new[0].y, 0, + 0, 0, 1, 0), false).convertTo(P1, P1.empty() ? CV_64F : P1.type()); + + Mat(Matx34d(fc_new, 0, cc_new[1].x, tnew[0]*fc_new, // baseline * focal length;, + 0, fc_new, cc_new[1].y, 0, + 0, 0, 1, 0), false).convertTo(P2, P2.empty() ? CV_64F : P2.type()); + + if (Q.needed()) + Mat(Matx44d(1, 0, 0, -cc_new[0].x, + 0, 1, 0, -cc_new[0].y, + 0, 0, 0, fc_new, + 0, 0, -1./tnew[0], (cc_new[0].x - cc_new[1].x)/tnew[0]), false).convertTo(Q, Q.empty() ? CV_64F : Q.depth()); +} + +} diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index 830dc63aa6..7e09bd26a4 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -40,12 +40,11 @@ // //M*/ +#include "opencv2/core/core_c.h" #include "opencv2/core/types.hpp" #include "precomp.hpp" #include "distortion_model.hpp" -#include "calib3d_c_api.h" - #include "undistort.simd.hpp" #include "undistort.simd_declarations.hpp" // defines CV_CPU_DISPATCH_MODES_ALL=AVX2,...,BASELINE based on CMakeLists.txt content @@ -508,14 +507,6 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM } } -void cvUndistortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, - const CvMat* _distCoeffs, - const CvMat* matR, const CvMat* matP) -{ - cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP, - cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01)); -} - namespace cv { void undistortPoints(InputArray _src, OutputArray _dst, From 37c2af63f099909252eeced1258e9a30a317306f Mon Sep 17 00:00:00 2001 From: Dmitry Kurtaev Date: Mon, 11 Nov 2024 14:13:33 +0300 Subject: [PATCH 06/10] Merge pull request #26434 from dkurt:dk/int64_file_storage_4.x int64 data type support for FileStorage. 1d and empty Mat with exact dimensions #26434 ### Pull Request Readiness Checklist Port of https://github.com/opencv/opencv/pull/26399 to 4.x branch See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- .../core/include/opencv2/core/persistence.hpp | 7 ++ modules/core/src/persistence.cpp | 116 ++++++++++++++++-- modules/core/src/persistence.hpp | 2 + modules/core/src/persistence_impl.hpp | 2 + modules/core/src/persistence_json.cpp | 10 +- modules/core/src/persistence_xml.cpp | 8 +- modules/core/src/persistence_yml.cpp | 8 +- modules/core/test/test_io.cpp | 29 +++++ .../predefined_types.py | 1 + 9 files changed, 166 insertions(+), 17 deletions(-) diff --git a/modules/core/include/opencv2/core/persistence.hpp b/modules/core/include/opencv2/core/persistence.hpp index 33f24d62e5..c6ddf6f0ca 100644 --- a/modules/core/include/opencv2/core/persistence.hpp +++ b/modules/core/include/opencv2/core/persistence.hpp @@ -365,6 +365,8 @@ public: */ CV_WRAP void write(const String& name, int val); /// @overload + CV_WRAP void write(const String& name, int64_t val); + /// @overload CV_WRAP void write(const String& name, double val); /// @overload CV_WRAP void write(const String& name, const String& val); @@ -530,6 +532,8 @@ public: CV_WRAP size_t rawSize() const; //! returns the node content as an integer. If the node stores floating-point number, it is rounded. operator int() const; + //! returns the node content as a signed 64bit integer. If the node stores floating-point number, it is rounded. + operator int64_t() const; //! returns the node content as float operator float() const; //! returns the node content as double @@ -654,6 +658,7 @@ protected: /////////////////// XML & YAML I/O implementation ////////////////// CV_EXPORTS void write( FileStorage& fs, const String& name, int value ); +CV_EXPORTS void write( FileStorage& fs, const String& name, int64_t value ); CV_EXPORTS void write( FileStorage& fs, const String& name, float value ); CV_EXPORTS void write( FileStorage& fs, const String& name, double value ); CV_EXPORTS void write( FileStorage& fs, const String& name, const String& value ); @@ -665,11 +670,13 @@ CV_EXPORTS void write( FileStorage& fs, const String& name, const std::vector +static inline void writeInt(uchar* p, T ival) { // On little endian CPUs, both branches produce the same result. On big endian, only the else branch does. #if CV_LITTLE_ENDIAN_MEM_ACCESS memcpy(p, &ival, sizeof(ival)); #else - p[0] = (uchar)ival; - p[1] = (uchar)(ival >> 8); - p[2] = (uchar)(ival >> 16); - p[3] = (uchar)(ival >> 24); + for (size_t i = 0, j = 0; i < sizeof(ival); ++i, j += 8) + p[i] = (uchar)(ival >> j); #endif } @@ -1056,6 +1091,11 @@ void FileStorage::Impl::write(const String &key, int value) { getEmitter().write(key.c_str(), value); } +void FileStorage::Impl::write(const String &key, int64_t value) { + CV_Assert(write_mode); + getEmitter().write(key.c_str(), value); +} + void FileStorage::Impl::write(const String &key, double value) { CV_Assert(write_mode); getEmitter().write(key.c_str(), value); @@ -1408,7 +1448,7 @@ void FileStorage::Impl::convertToCollection(int type, FileNode &node) { bool named = node.isNamed(); uchar *ptr = node.ptr() + 1 + (named ? 4 : 0); - int ival = 0; + int64_t ival = 0; double fval = 0; std::string sval; bool add_first_scalar = false; @@ -1421,7 +1461,7 @@ void FileStorage::Impl::convertToCollection(int type, FileNode &node) { // otherwise we don't know where to get the element names from CV_Assert(type == FileNode::SEQ); if (node_type == FileNode::INT) { - ival = readInt(ptr); + ival = readLong(ptr); add_first_scalar = true; } else if (node_type == FileNode::REAL) { fval = readReal(ptr); @@ -1783,7 +1823,7 @@ char *FileStorage::Impl::parseBase64(char *ptr, int indent, FileNode &collection int fmt_pairs[CV_FS_MAX_FMT_PAIRS * 2]; int fmt_pair_count = fs::decodeFormat(dt, fmt_pairs, CV_FS_MAX_FMT_PAIRS); - int ival = 0; + int64_t ival = 0; double fval = 0; for (;;) { @@ -2023,6 +2063,11 @@ void writeScalar( FileStorage& fs, int value ) fs.p->write(String(), value); } +void writeScalar( FileStorage& fs, int64_t value ) +{ + fs.p->write(String(), value); +} + void writeScalar( FileStorage& fs, float value ) { fs.p->write(String(), (double)value); @@ -2043,6 +2088,11 @@ void write( FileStorage& fs, const String& name, int value ) fs.p->write(name, value); } +void write( FileStorage& fs, const String& name, int64_t value ) +{ + fs.p->write(name, value); +} + void write( FileStorage& fs, const String& name, float value ) { fs.p->write(name, (double)value); @@ -2059,6 +2109,7 @@ void write( FileStorage& fs, const String& name, const String& value ) } void FileStorage::write(const String& name, int val) { p->write(name, val); } +void FileStorage::write(const String& name, int64_t val) { p->write(name, val); } void FileStorage::write(const String& name, double val) { p->write(name, val); } void FileStorage::write(const String& name, const String& val) { p->write(name, val); } void FileStorage::write(const String& name, const Mat& val) { cv::write(*this, name, val); } @@ -2279,6 +2330,27 @@ FileNode::operator int() const return 0x7fffffff; } +FileNode::operator int64_t() const +{ + const uchar* p = ptr(); + if(!p) + return 0; + int tag = *p; + int type = (tag & TYPE_MASK); + p += (tag & NAMED) ? 5 : 1; + + if( type == INT ) + { + return readLong(p); + } + else if( type == REAL ) + { + return cvRound(readReal(p)); + } + else + return 0x7fffffff; +} + FileNode::operator float() const { const uchar* p = ptr(); @@ -2403,7 +2475,13 @@ void FileNode::setValue( int type, const void* value, int len ) sz += 4; if( type == INT ) - sz += 4; + { + int64_t ival = *(const int64_t*)value; + if (ival > INT_MAX || ival < INT_MIN) + sz += 8; + else + sz += 4; + } else if( type == REAL ) sz += 8; else if( type == STRING ) @@ -2423,8 +2501,11 @@ void FileNode::setValue( int type, const void* value, int len ) if( type == INT ) { - int ival = *(const int*)value; - writeInt(p, ival); + int64_t ival = *(const int64_t*)value; + if (sz > 8) + writeInt(p, ival); + else + writeInt(p, static_cast(ival)); } else if( type == REAL ) { @@ -2580,7 +2661,7 @@ FileNodeIterator& FileNodeIterator::readRaw( const String& fmt, void* _data0, si FileNode node = *(*this); if( node.isInt() ) { - int ival = (int)node; + int64_t ival = static_cast(elem_size == 8 ? (int64_t)node : (int)node); switch( elem_type ) { case CV_8U: @@ -2600,7 +2681,7 @@ FileNodeIterator& FileNodeIterator::readRaw( const String& fmt, void* _data0, si data += sizeof(short); break; case CV_32S: - *(int*)data = ival; + *(int*)data = (int)ival; data += sizeof(int); break; case CV_32F: @@ -2702,6 +2783,15 @@ void read(const FileNode& node, int& val, int default_val) } } +void read(const FileNode& node, int64_t& val, int64_t default_val) +{ + val = default_val; + if( !node.empty() ) + { + val = (int64_t)node; + } +} + void read(const FileNode& node, double& val, double default_val) { val = default_val; diff --git a/modules/core/src/persistence.hpp b/modules/core/src/persistence.hpp index 4b579303fa..2a1f73cba8 100644 --- a/modules/core/src/persistence.hpp +++ b/modules/core/src/persistence.hpp @@ -86,6 +86,7 @@ namespace fs { int strcasecmp(const char* str1, const char* str2); char* itoa( int _val, char* buffer, int /*radix*/ ); +char* itoa( int64_t _val, char* buffer, int /*radix*/, bool _signed ); char* floatToString( char* buf, size_t bufSize, float value, bool halfprecision, bool explicitZero ); char* doubleToString( char* buf, size_t bufSize, double value, bool explicitZero ); @@ -193,6 +194,7 @@ public: int struct_flags, const char* type_name=0 ) = 0; virtual void endWriteStruct(const FStructData& current_struct) = 0; virtual void write(const char* key, int value) = 0; + virtual void write(const char* key, int64_t value) = 0; virtual void write(const char* key, double value) = 0; virtual void write(const char* key, const char* value, bool quote) = 0; virtual void writeScalar(const char* key, const char* value) = 0; diff --git a/modules/core/src/persistence_impl.hpp b/modules/core/src/persistence_impl.hpp index 1c261ce772..7f902079a5 100644 --- a/modules/core/src/persistence_impl.hpp +++ b/modules/core/src/persistence_impl.hpp @@ -69,6 +69,8 @@ public: void write( const String& key, int value ); + void write( const String& key, int64_t value ); + void write( const String& key, double value ); void write( const String& key, const String& value ); diff --git a/modules/core/src/persistence_json.cpp b/modules/core/src/persistence_json.cpp index e201215401..c5326c674a 100644 --- a/modules/core/src/persistence_json.cpp +++ b/modules/core/src/persistence_json.cpp @@ -81,6 +81,12 @@ public: writeScalar( key, fs::itoa( value, buf, 10 )); } + void write(const char* key, int64_t value) + { + char buf[128]; + writeScalar( key, fs::itoa( value, buf, 10, true )); + } + void write( const char* key, double value ) { char buf[128]; @@ -596,7 +602,7 @@ public: } else { - int ival = (int)strtol( beg, &ptr, 0 ); + int64_t ival = strtoll( beg, &ptr, 0 ); CV_PERSISTENCE_CHECK_END_OF_BUFFER_BUG_CPP(); node.setValue(FileNode::INT, &ival); @@ -623,7 +629,7 @@ public: else if( (len == 4 && memcmp( beg, "true", 4 ) == 0) || (len == 5 && memcmp( beg, "false", 5 ) == 0) ) { - int ival = *beg == 't' ? 1 : 0; + int64_t ival = *beg == 't' ? 1 : 0; node.setValue(FileNode::INT, &ival); } else diff --git a/modules/core/src/persistence_xml.cpp b/modules/core/src/persistence_xml.cpp index ed699758fc..29c05fdeca 100644 --- a/modules/core/src/persistence_xml.cpp +++ b/modules/core/src/persistence_xml.cpp @@ -145,6 +145,12 @@ public: writeScalar( key, ptr); } + void write(const char* key, int64_t value) + { + char buf[128], *ptr = fs::itoa( value, buf, 10, true ); + writeScalar( key, ptr); + } + void write( const char* key, double value ) { char buf[128]; @@ -556,7 +562,7 @@ public: } else { - int ival = (int)strtol( ptr, &endptr, 0 ); + int64_t ival = strtoll( ptr, &endptr, 0 ); elem->setValue(FileNode::INT, &ival); } diff --git a/modules/core/src/persistence_yml.cpp b/modules/core/src/persistence_yml.cpp index e4f53c353e..009ab7d8b7 100644 --- a/modules/core/src/persistence_yml.cpp +++ b/modules/core/src/persistence_yml.cpp @@ -107,6 +107,12 @@ public: writeScalar( key, fs::itoa( value, buf, 10 )); } + void write(const char* key, int64_t value) + { + char buf[128]; + writeScalar( key, fs::itoa( value, buf, 10, true )); + } + void write( const char* key, double value ) { char buf[128]; @@ -567,7 +573,7 @@ public: else { force_int: - int ival = (int)strtol( ptr, &endptr, 0 ); + int64_t ival = strtoll( ptr, &endptr, 0 ); node.setValue(FileNode::INT, &ival); } diff --git a/modules/core/test/test_io.cpp b/modules/core/test/test_io.cpp index d766abb319..88b72cd3f1 100644 --- a/modules/core/test/test_io.cpp +++ b/modules/core/test/test_io.cpp @@ -2003,4 +2003,33 @@ TEST(Core_InputOutput, FileStorage_invalid_attribute_value_regression_25946) ASSERT_EQ(0, std::remove(fileName.c_str())); } +template +T fsWriteRead(const T& expectedValue, const char* ext) +{ + std::string fname = cv::tempfile(ext); + FileStorage fs_w(fname, FileStorage::WRITE); + fs_w << "value" << expectedValue; + fs_w.release(); + + FileStorage fs_r(fname, FileStorage::READ); + + T value; + fs_r["value"] >> value; + return value; +} + +typedef testing::TestWithParam FileStorage_exact_type; +TEST_P(FileStorage_exact_type, long_int) +{ + for (const int64_t expected : std::vector{INT64_MAX, INT64_MIN, -1, 1, 0}) + { + int64_t value = fsWriteRead(expected, GetParam()); + EXPECT_EQ(value, expected); + } +} + +INSTANTIATE_TEST_CASE_P(Core_InputOutput, + FileStorage_exact_type, Values(".yml", ".xml", ".json") +); + }} // namespace diff --git a/modules/python/src2/typing_stubs_generation/predefined_types.py b/modules/python/src2/typing_stubs_generation/predefined_types.py index 5aed93c8b0..dd764f6112 100644 --- a/modules/python/src2/typing_stubs_generation/predefined_types.py +++ b/modules/python/src2/typing_stubs_generation/predefined_types.py @@ -27,6 +27,7 @@ _PREDEFINED_TYPES = ( PrimitiveTypeNode.int_("int32_t"), PrimitiveTypeNode.int_("uint32_t"), PrimitiveTypeNode.int_("size_t"), + PrimitiveTypeNode.int_("int64_t"), PrimitiveTypeNode.float_("float"), PrimitiveTypeNode.float_("double"), PrimitiveTypeNode.bool_("bool"), From c230841105876d19cf35d1b824d81f46333e30af Mon Sep 17 00:00:00 2001 From: Dmitry Kurtaev Date: Tue, 12 Nov 2024 17:51:10 +0300 Subject: [PATCH 07/10] Merge pull request #26446 from dkurt:file_storage_empty_and_1d_mat * Change style of empty and 1d Mat in FileStorage * Remove misleading 1d Mat test --- modules/core/src/persistence_types.cpp | 7 ++++++- modules/core/test/test_io.cpp | 18 ++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/modules/core/src/persistence_types.cpp b/modules/core/src/persistence_types.cpp index cd4fad05fb..8aa929cc1d 100644 --- a/modules/core/src/persistence_types.cpp +++ b/modules/core/src/persistence_types.cpp @@ -12,7 +12,7 @@ void write( FileStorage& fs, const String& name, const Mat& m ) { char dt[22]; - if( m.dims <= 2 ) + if( m.dims == 2 || m.empty() ) { fs.startWriteStruct(name, FileNode::MAP, String("opencv-matrix")); fs << "rows" << m.rows; @@ -151,6 +151,11 @@ void read(const FileNode& node, Mat& m, const Mat& default_mat) CV_Assert(!data_node.empty()); size_t nelems = data_node.size(); + if (nelems == 0) + { + m = Mat(); + return; + } CV_Assert(nelems == m.total()*m.channels()); data_node.readRaw(dt, (uchar*)m.ptr(), m.total()*m.elemSize()); diff --git a/modules/core/test/test_io.cpp b/modules/core/test/test_io.cpp index 88b72cd3f1..99972b964c 100644 --- a/modules/core/test/test_io.cpp +++ b/modules/core/test/test_io.cpp @@ -2018,7 +2018,25 @@ T fsWriteRead(const T& expectedValue, const char* ext) return value; } +void testExactMat(const Mat& src, const char* ext) +{ + bool srcIsEmpty = src.empty(); + Mat dst = fsWriteRead(src, ext); + EXPECT_EQ(dst.empty(), srcIsEmpty); + EXPECT_EQ(src.dims, dst.dims); + EXPECT_EQ(src.size, dst.size); + if (!srcIsEmpty) + { + EXPECT_EQ(0.0, cv::norm(src, dst, NORM_INF)); + } +} + typedef testing::TestWithParam FileStorage_exact_type; +TEST_P(FileStorage_exact_type, empty_mat) +{ + testExactMat(Mat(), GetParam()); +} + TEST_P(FileStorage_exact_type, long_int) { for (const int64_t expected : std::vector{INT64_MAX, INT64_MIN, -1, 1, 0}) From 641f43dd48cf9d9c42d728a8407644564b714312 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 12 Nov 2024 17:04:42 +0100 Subject: [PATCH 08/10] build fix --- modules/core/src/opencl/runtime/opencl_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/src/opencl/runtime/opencl_core.cpp b/modules/core/src/opencl/runtime/opencl_core.cpp index 3a454faa29..35d24eb1cd 100644 --- a/modules/core/src/opencl/runtime/opencl_core.cpp +++ b/modules/core/src/opencl/runtime/opencl_core.cpp @@ -204,7 +204,7 @@ static void* GetProcAddress(const char* name) { const std::string path = (i==0) ? getRuntimePath(defaultAndroidPaths[i]) : defaultAndroidPaths[i]; if (!path.empty()) { - handle = GetHandle(path); + handle = GetHandle(path.c_str()); if (handle) { foundOpenCL = true; break; From 5f95827a5f693a1a0a7c50c104d6c3955455cb64 Mon Sep 17 00:00:00 2001 From: Billy Robert O'Neal III Date: Tue, 12 Nov 2024 11:57:39 -0800 Subject: [PATCH 09/10] Add note for people debugging DirectML detection failures to check their Windows SDK version. DirectML was first included with 10.0.18362.0, but dxcore.lib necessary to make the check pass was first in 10.0.19041.0. --- cmake/OpenCVDetectDirectML.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/OpenCVDetectDirectML.cmake b/cmake/OpenCVDetectDirectML.cmake index 0fc71eca03..0394678a0c 100644 --- a/cmake/OpenCVDetectDirectML.cmake +++ b/cmake/OpenCVDetectDirectML.cmake @@ -6,7 +6,7 @@ if(WIN32) OUTPUT_VARIABLE TRY_OUT ) if(NOT __VALID_DIRECTML) - message(STATUS "No support for DirectML (d3d12, dxcore, directml libs are required)") + message(STATUS "No support for DirectML. d3d12, dxcore, directml libs are required, first bundled with Windows SDK 10.0.19041.0.") return() endif() set(HAVE_DIRECTML ON) From 67f07b16cbf7e556bc2d2861d8a586fe9e0f3fab Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 13 Nov 2024 06:33:19 +0100 Subject: [PATCH 10/10] Merge pull request #25624 from savuor:rv/hal_addscalar HAL added for add(array, scalar) #25624 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/core/src/arithm.cpp | 106 ++++++++++++++++++++------- modules/core/src/hal_replacement.hpp | 16 ++++ 2 files changed, 96 insertions(+), 26 deletions(-) diff --git a/modules/core/src/arithm.cpp b/modules/core/src/arithm.cpp index 72d5806ebb..aea697e762 100644 --- a/modules/core/src/arithm.cpp +++ b/modules/core/src/arithm.cpp @@ -585,6 +585,10 @@ static bool ocl_arithm_op(InputArray _src1, InputArray _src2, OutputArray _dst, #endif +typedef int (*ScalarFunc)(const uchar* src, size_t step_src, + uchar* dst, size_t step_dst, int width, int height, + void* scalar, bool scalarIsFirst); + typedef int (*ExtendedTypeFunc)(const uchar* src1, size_t step1, const uchar* src2, size_t step2, uchar* dst, size_t step, int width, int height, @@ -592,7 +596,8 @@ typedef int (*ExtendedTypeFunc)(const uchar* src1, size_t step1, static void arithm_op(InputArray _src1, InputArray _src2, OutputArray _dst, InputArray _mask, int dtype, BinaryFuncC* tab, bool muldiv=false, - void* usrdata=0, int oclop=-1, ExtendedTypeFunc extendedFunc = nullptr ) + void* usrdata=0, int oclop=-1, ExtendedTypeFunc extendedFunc = nullptr, + ScalarFunc scalarFunc = nullptr) { const _InputArray *psrc1 = &_src1, *psrc2 = &_src2; _InputArray::KindFlag kind1 = psrc1->kind(), kind2 = psrc2->kind(); @@ -638,8 +643,7 @@ static void arithm_op(InputArray _src1, InputArray _src2, OutputArray _dst, (kind1 == _InputArray::MATX && (sz1 == Size(1,4) || sz1 == Size(1,1))) || (kind2 == _InputArray::MATX && (sz2 == Size(1,4) || sz2 == Size(1,1))) ) { - if ((type1 == CV_64F && (sz1.height == 1 || sz1.height == 4)) && - checkScalar(*psrc1, type2, kind1, kind2)) + if ((type1 == CV_64F && (sz1.height == 1 || sz1.height == 4)) && src1Scalar) { // src1 is a scalar; swap it with src2 swap(psrc1, psrc2); @@ -654,7 +658,7 @@ static void arithm_op(InputArray _src1, InputArray _src2, OutputArray _dst, if ( oclop == OCL_OP_DIV_SCALE ) oclop = OCL_OP_RDIV_SCALE; } - else if( !checkScalar(*psrc2, type1, kind2, kind1) ) + else if( !src2Scalar ) CV_Error( cv::Error::StsUnmatchedSizes, "The operation is neither 'array op array' " "(where arrays have the same size and the same number of channels), " @@ -866,32 +870,38 @@ static void arithm_op(InputArray _src1, InputArray _src2, OutputArray _dst, const uchar* extSptr1 = sptr1; const uchar* extSptr2 = sptr2; if( swapped12 ) - std::swap(extSptr1, extSptr1); + std::swap(extSptr1, extSptr2); - // try to perform operation with conversion in one call - // if fail, use converter functions + // try to perform operation in 1 call, fallback to classic way if fail uchar* opconverted = haveMask ? maskbuf : dptr; - if (!extendedFunc || extendedFunc(extSptr1, 1, extSptr2, 1, opconverted, 1, - bszn.width, bszn.height, usrdata) != 0) + if (!scalarFunc || src2.total() != 1 || + scalarFunc(extSptr1, 1, opconverted, 1, bszn.width, bszn.height, (void*)extSptr2, swapped12) != 0) { - if( cvtsrc1 ) + // try to perform operation with conversion in one call + // if fail, use converter functions + + if (!extendedFunc || extendedFunc(extSptr1, 1, extSptr2, 1, opconverted, 1, + bszn.width, bszn.height, usrdata) != 0) { - cvtsrc1( sptr1, 1, 0, 1, buf1, 1, bszn, 0 ); - sptr1 = buf1; + if( cvtsrc1 ) + { + cvtsrc1( sptr1, 1, 0, 1, buf1, 1, bszn, 0 ); + sptr1 = buf1; + } + + if( swapped12 ) + std::swap(sptr1, sptr2); + + uchar* fdst = ( haveMask || cvtdst ) ? wbuf : dptr; + func( sptr1, 1, sptr2, 1, fdst, 1, bszn.width, bszn.height, usrdata ); + + if (cvtdst) + { + uchar* cdst = haveMask ? maskbuf : dptr; + cvtdst(wbuf, 1, 0, 1, cdst, 1, bszn, 0); + } + opconverted = cvtdst ? maskbuf : wbuf; } - - if( swapped12 ) - std::swap(sptr1, sptr2); - - uchar* fdst = ( haveMask || cvtdst ) ? wbuf : dptr; - func( sptr1, 1, sptr2, 1, fdst, 1, bszn.width, bszn.height, usrdata ); - - if (cvtdst) - { - uchar* cdst = haveMask ? maskbuf : dptr; - cvtdst(wbuf, 1, 0, 1, cdst, 1, bszn, 0); - } - opconverted = cvtdst ? maskbuf : wbuf; } if (haveMask) @@ -920,6 +930,48 @@ static BinaryFuncC* getAddTab() return addTab; } +static int addScalar32f32fWrapper(const uchar* src, size_t step_src, uchar* dst, size_t step_dst, int width, int height, + void* scalar, bool /*scalarIsFirst*/) +{ + int res = cv_hal_addScalar32f32f((const float*)src, step_src, (float *)dst, step_dst, width, height, (const float*)scalar); + if (res == CV_HAL_ERROR_OK || res == CV_HAL_ERROR_NOT_IMPLEMENTED) + return res; + else + { + CV_Error_(cv::Error::StsInternal, ("HAL implementation addScalar32f32f ==> " CVAUX_STR(cv_hal_addScalar32f32f) + " returned %d (0x%08x)", res, res)); + } +} + +static int addScalar16s16sWrapper(const uchar* src, size_t step_src, uchar* dst, size_t step_dst, int width, int height, + void* scalar, bool /*scalarIsFirst*/) +{ + int res = cv_hal_addScalar16s16s((const int16_t*)src, step_src, (int16_t *)dst, step_dst, width, height, (const int16_t*)scalar); + if (res == CV_HAL_ERROR_OK || res == CV_HAL_ERROR_NOT_IMPLEMENTED) + return res; + else + { + CV_Error_(cv::Error::StsInternal, ("HAL implementation addScalar16s16s ==> " CVAUX_STR(cv_hal_addScalar16s16s) + " returned %d (0x%08x)", res, res)); + } +} + +static ScalarFunc getAddScalarFunc(int srcType, int dstType) +{ + if (srcType == CV_32F && dstType == CV_32F) + { + return addScalar32f32fWrapper; + } + else if (srcType == CV_16S && dstType == CV_16S) + { + return addScalar16s16sWrapper; + } + else + { + return nullptr; + } +} + static int sub8u32fWrapper(const uchar* src1, size_t step1, const uchar* src2, size_t step2, uchar* dst, size_t step, int width, int height, void* ) { @@ -1004,7 +1056,9 @@ void cv::add( InputArray src1, InputArray src2, OutputArray dst, return; } - arithm_op(src1, src2, dst, mask, dtype, getAddTab(), false, 0, OCL_OP_ADD ); + ScalarFunc scalarFunc = getAddScalarFunc(src1.depth(), dtype < 0 ? dst.depth() : dtype); + arithm_op(src1, src2, dst, mask, dtype, getAddTab(), false, 0, OCL_OP_ADD, nullptr, + /* scalarFunc */ scalarFunc ); } void cv::subtract( InputArray _src1, InputArray _src2, OutputArray _dst, diff --git a/modules/core/src/hal_replacement.hpp b/modules/core/src/hal_replacement.hpp index 67f3ac7141..013ca97875 100644 --- a/modules/core/src/hal_replacement.hpp +++ b/modules/core/src/hal_replacement.hpp @@ -98,6 +98,20 @@ inline int hal_ni_sub64f(const double *src1_data, size_t src1_step, const double inline int hal_ni_sub8u32f(const uchar *src1_data, size_t src1_step, const uchar *src2_data, size_t src2_step, float *dst_data, size_t dst_step, int width, int height) { return CV_HAL_ERROR_NOT_IMPLEMENTED; } inline int hal_ni_sub8s32f(const schar *src1_data, size_t src1_step, const schar *src2_data, size_t src2_step, float *dst_data, size_t dst_step, int width, int height) { return CV_HAL_ERROR_NOT_IMPLEMENTED; } + +/** +Add scalar: _dst[i] = src[i] + scalar + +@param src_data source image data +@param src_step source image step +@param dst_data destination image data +@param dst_step destination image step +@param width width of the images +@param height height of the images +@param scalar_data pointer to scalar value +*/ +inline int hal_ni_addScalar32f32f(const float *src_data, size_t src_step, float *dst_data, size_t dst_step, int width, int height, const float* scalar_data) { return CV_HAL_ERROR_NOT_IMPLEMENTED; } +inline int hal_ni_addScalar16s16s(const int16_t *src_data, size_t src_step, int16_t *dst_data, size_t dst_step, int width, int height, const int16_t* scalar_data) { return CV_HAL_ERROR_NOT_IMPLEMENTED; } //! @} /** @@ -192,6 +206,8 @@ inline int hal_ni_not8u(const uchar *src_data, size_t src_step, uchar *dst_data, #define cv_hal_sub64f hal_ni_sub64f #define cv_hal_sub8u32f hal_ni_sub8u32f #define cv_hal_sub8s32f hal_ni_sub8s32f +#define cv_hal_addScalar32f32f hal_ni_addScalar32f32f +#define cv_hal_addScalar16s16s hal_ni_addScalar16s16s #define cv_hal_max8u hal_ni_max8u #define cv_hal_max8s hal_ni_max8s #define cv_hal_max16u hal_ni_max16u