void IPPE::PoseSolver::rot2vec(InputArray _R, OutputArray _r) { assert(_R.type() == CV_64FC1); assert(_R.rows() == 3); assert(_R.cols() == 3); _r.create(3, 1, CV_64FC1); cv::Mat R = _R.getMat(); cv::Mat rvec = _r.getMat(); double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2); double w_norm = acos((trace - 1.0) / 2.0); double c0, c1, c2; double eps = std::numeric_limits<float>::epsilon(); double d = 1 / (2 * sin(w_norm)) * w_norm; if (w_norm < eps) //rotation is the identity { rvec.setTo(0); } else { c0 = R.at<double>(2, 1) - R.at<double>(1, 2); c1 = R.at<double>(0, 2) - R.at<double>(2, 0); c2 = R.at<double>(1, 0) - R.at<double>(0, 1); rvec.at<double>(0) = d * c0; rvec.at<double>(1) = d * c1; rvec.at<double>(2) = d * c2; } }
GuidedFilterRefImpl::GuidedFilterRefImpl(InputArray _guide, int _rad, double _eps) : height(_guide.rows()), width(_guide.cols()), rad(_rad), chNum(_guide.channels()), eps(_eps) { Mat guide = _guide.getMat(); CV_Assert(chNum > 0 && chNum <= 3); channels = new Mat[chNum]; exps = new Mat[chNum]; A = new Mat *[chNum]; vars = new Mat *[chNum]; for (int i = 0; i < chNum; ++i) { A[i] = new Mat[chNum]; vars[i] = new Mat[chNum]; } split(guide, channels); for (int i = 0; i < chNum; ++i) { channels[i].convertTo(channels[i], CV_32F); meanFilter(channels[i], exps[i]); } computeCovGuide(); computeCovGuideInv(); }
double IPPE::PoseSolver::meanSceneDepth(InputArray _objectPoints, InputArray _rvec, InputArray _tvec) { assert((_objectPoints.type() == CV_64FC3) | (_objectPoints.type() == CV_64FC3)); size_t n = _objectPoints.rows() * _objectPoints.cols(); Mat R; Mat q; Rodrigues(_rvec, R); double zBar = 0; for (size_t i = 0; i < n; i++) { cv::Mat p(_objectPoints.getMat().at<Point3d>(i)); q = R * p + _tvec.getMat(); double z; if (q.depth() == CV_64FC1) { z = q.at<double>(2); } else { z = static_cast<double>(q.at<float>(2)); } zBar += z; //if (z <= 0) { // std::cout << "Warning: object point " << i << " projects behind the camera! This should not be allowed. " << std::endl; //} } return zBar / static_cast<double>(n); }
static bool convolve_32F(InputArray _image, InputArray _templ, OutputArray _result) { _result.create(_image.rows() - _templ.rows() + 1, _image.cols() - _templ.cols() + 1, CV_32F); if (_image.channels() == 1) return(convolve_dft(_image, _templ, _result)); else { UMat image = _image.getUMat(); UMat templ = _templ.getUMat(); UMat result_(image.rows-templ.rows+1,(image.cols-templ.cols+1)*image.channels(), CV_32F); bool ok = convolve_dft(image.reshape(1), templ.reshape(1), result_); if (ok==false) return false; UMat result = _result.getUMat(); return (extractFirstChannel_32F(result_, _result, _image.channels())); } }
static bool ocl_sepFilter3x3_8UC1(InputArray _src, OutputArray _dst, int ddepth, InputArray _kernelX, InputArray _kernelY, double delta, int borderType) { const ocl::Device & dev = ocl::Device::getDefault(); int type = _src.type(), sdepth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); if ( !(dev.isIntel() && (type == CV_8UC1) && (ddepth == CV_8U) && (_src.offset() == 0) && (_src.step() % 4 == 0) && (_src.cols() % 16 == 0) && (_src.rows() % 2 == 0)) ) return false; Mat kernelX = _kernelX.getMat().reshape(1, 1); if (kernelX.cols % 2 != 1) return false; Mat kernelY = _kernelY.getMat().reshape(1, 1); if (kernelY.cols % 2 != 1) return false; if (ddepth < 0) ddepth = sdepth; Size size = _src.size(); size_t globalsize[2] = { 0, 0 }; size_t localsize[2] = { 0, 0 }; globalsize[0] = size.width / 16; globalsize[1] = size.height / 2; const char * const borderMap[] = { "BORDER_CONSTANT", "BORDER_REPLICATE", "BORDER_REFLECT", 0, "BORDER_REFLECT_101" }; char build_opts[1024]; sprintf(build_opts, "-D %s %s%s", borderMap[borderType], ocl::kernelToStr(kernelX, CV_32F, "KERNEL_MATRIX_X").c_str(), ocl::kernelToStr(kernelY, CV_32F, "KERNEL_MATRIX_Y").c_str()); ocl::Kernel kernel("sepFilter3x3_8UC1_cols16_rows2", cv::ocl::imgproc::sepFilter3x3_oclsrc, build_opts); if (kernel.empty()) return false; UMat src = _src.getUMat(); _dst.create(size, CV_MAKETYPE(ddepth, cn)); if (!(_dst.offset() == 0 && _dst.step() % 4 == 0)) return false; UMat dst = _dst.getUMat(); int idxArg = kernel.set(0, ocl::KernelArg::PtrReadOnly(src)); idxArg = kernel.set(idxArg, (int)src.step); idxArg = kernel.set(idxArg, ocl::KernelArg::PtrWriteOnly(dst)); idxArg = kernel.set(idxArg, (int)dst.step); idxArg = kernel.set(idxArg, (int)dst.rows); idxArg = kernel.set(idxArg, (int)dst.cols); idxArg = kernel.set(idxArg, static_cast<float>(delta)); return kernel.run(2, globalsize, (localsize[0] == 0) ? NULL : localsize, false); }
void IPPE::PoseSolver::makeCanonicalObjectPoints(InputArray _objectPoints, OutputArray _canonicalObjPoints, OutputArray _MmodelPoints2Canonical) { int objType = _objectPoints.type(); assert((objType == CV_64FC3) | (objType == CV_32FC3)); size_t n = _objectPoints.rows() * _objectPoints.cols(); _canonicalObjPoints.create(1, n, CV_64FC2); _MmodelPoints2Canonical.create(4, 4, CV_64FC1); cv::Mat objectPoints = _objectPoints.getMat(); cv::Mat canonicalObjPoints = _canonicalObjPoints.getMat(); cv::Mat UZero(3, n, CV_64FC1); double xBar = 0; double yBar = 0; double zBar = 0; bool isOnZPlane = true; for (size_t i = 0; i < n; i++) { double x, y, z; if (objType == CV_32FC3) { x = static_cast<double>(objectPoints.at<Vec3f>(i)[0]); y = static_cast<double>(objectPoints.at<Vec3f>(i)[1]); z = static_cast<double>(objectPoints.at<Vec3f>(i)[2]); } else { x = objectPoints.at<Vec3d>(i)[0]; y = objectPoints.at<Vec3d>(i)[1]; z = objectPoints.at<Vec3d>(i)[2]; if (abs(z) > IPPE_SMALL) { isOnZPlane = false; } } xBar += x; yBar += y; zBar += z; UZero.at<double>(0, i) = x; UZero.at<double>(1, i) = y; UZero.at<double>(2, i) = z; } xBar = xBar / (double)n; yBar = yBar / (double)n; zBar = zBar / (double)n; for (size_t i = 0; i < n; i++) { UZero.at<double>(0, i) -= xBar; UZero.at<double>(1, i) -= yBar; UZero.at<double>(2, i) -= zBar; } cv::Mat MCenter(4, 4, CV_64FC1); MCenter.setTo(0); MCenter.at<double>(0, 0) = 1; MCenter.at<double>(1, 1) = 1; MCenter.at<double>(2, 2) = 1; MCenter.at<double>(3, 3) = 1; MCenter.at<double>(0, 3) = -xBar; MCenter.at<double>(1, 3) = -yBar; MCenter.at<double>(2, 3) = -zBar; if (isOnZPlane) { //MmodelPoints2Canonical is given by MCenter MCenter.copyTo(_MmodelPoints2Canonical); for (size_t i = 0; i < n; i++) { canonicalObjPoints.at<Vec2d>(i)[0] = UZero.at<double>(0, i); canonicalObjPoints.at<Vec2d>(i)[1] = UZero.at<double>(1, i); } } else { cv::Mat UZeroAligned(3, n, CV_64FC1); cv::Mat R; //rotation that rotates objectPoints to the plane z=0 if (!computeObjextSpaceR3Pts(objectPoints,R)) { //we could not compute R, problably because there is a duplicate point in {objectPoints(0),objectPoints(1),objectPoints(2)}. So we compute it with the SVD (which is slower): computeObjextSpaceRSvD(UZero,R); } UZeroAligned = R * UZero; for (size_t i = 0; i < n; i++) { canonicalObjPoints.at<Vec2d>(i)[0] = UZeroAligned.at<double>(0, i); canonicalObjPoints.at<Vec2d>(i)[1] = UZeroAligned.at<double>(1, i); assert(abs(UZeroAligned.at<double>(2, i))<=IPPE_SMALL); } cv::Mat MRot(4, 4, CV_64FC1); MRot.setTo(0); MRot.at<double>(3, 3) = 1; R.copyTo(MRot.colRange(0, 3).rowRange(0, 3)); cv::Mat Mb = MRot * MCenter; Mb.copyTo(_MmodelPoints2Canonical); } }
void IPPE::PoseSolver::computeTranslation(InputArray _objectPoints, InputArray _normalizedImgPoints, InputArray _R, OutputArray _t) { //This is solved by building the linear system At = b, where t corresponds to the (unknown) translation. //This is then inverted with the associated normal equations to give t = inv(transpose(A)*A)*transpose(A)*b //For efficiency we only store the coefficients of (transpose(A)*A) and (transpose(A)*b) assert(_objectPoints.type() == CV_64FC2); assert(_normalizedImgPoints.type() == CV_64FC2); assert(_R.type() == CV_64FC1); assert((_R.rows() == 3) & (_R.cols() == 3)); assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1)); assert((_normalizedImgPoints.rows() == 1) | (_normalizedImgPoints.cols() == 1)); size_t n = _normalizedImgPoints.rows() * _normalizedImgPoints.cols(); assert(n == static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols())); cv::Mat objectPoints = _objectPoints.getMat(); cv::Mat imgPoints = _normalizedImgPoints.getMat(); _t.create(3, 1, CV_64FC1); cv::Mat R = _R.getMat(); //coefficients of (transpose(A)*A) double ATA00 = n; double ATA02 = 0; double ATA11 = n; double ATA12 = 0; double ATA20 = 0; double ATA21 = 0; double ATA22 = 0; //coefficients of (transpose(A)*b) double ATb0 = 0; double ATb1 = 0; double ATb2 = 0; //S gives inv(transpose(A)*A)/det(A)^2 double S00, S01, S02; double S10, S11, S12; double S20, S21, S22; double rx, ry, rz; double a2; double b2; double bx, by; //now loop through each point and increment the coefficients: for (size_t i = 0; i < n; i++) { rx = R.at<double>(0, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(0, 1) * objectPoints.at<Vec2d>(i)(1); ry = R.at<double>(1, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(1, 1) * objectPoints.at<Vec2d>(i)(1); rz = R.at<double>(2, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(2, 1) * objectPoints.at<Vec2d>(i)(1); a2 = -imgPoints.at<Vec2d>(i)(0); b2 = -imgPoints.at<Vec2d>(i)(1); ATA02 = ATA02 + a2; ATA12 = ATA12 + b2; ATA20 = ATA20 + a2; ATA21 = ATA21 + b2; ATA22 = ATA22 + a2 * a2 + b2 * b2; bx = -a2 * rz - rx; by = -b2 * rz - ry; ATb0 = ATb0 + bx; ATb1 = ATb1 + by; ATb2 = ATb2 + a2 * bx + b2 * by; } double detAInv = 1.0 / (ATA00 * ATA11 * ATA22 - ATA00 * ATA12 * ATA21 - ATA02 * ATA11 * ATA20); //construct S: S00 = ATA11 * ATA22 - ATA12 * ATA21; S01 = ATA02 * ATA21; S02 = -ATA02 * ATA11; S10 = ATA12 * ATA20; S11 = ATA00 * ATA22 - ATA02 * ATA20; S12 = -ATA00 * ATA12; S20 = -ATA11 * ATA20; S21 = -ATA00 * ATA21; S22 = ATA00 * ATA11; //solve t: Mat t = _t.getMat(); t.at<double>(0) = detAInv * (S00 * ATb0 + S01 * ATb1 + S02 * ATb2); t.at<double>(1) = detAInv * (S10 * ATb0 + S11 * ATb1 + S12 * ATb2); t.at<double>(2) = detAInv * (S20 * ATb0 + S21 * ATb1 + S22 * ATb2); }
static bool openvx_sobel(InputArray _src, OutputArray _dst, int dx, int dy, int ksize, double scale, double delta, int borderType) { if (_src.type() != CV_8UC1 || _dst.type() != CV_16SC1 || ksize != 3 || scale != 1.0 || delta != 0.0 || (dx | dy) != 1 || (dx + dy) != 1 || _src.cols() < ksize || _src.rows() < ksize || ovx::skipSmallImages<VX_KERNEL_SOBEL_3x3>(_src.cols(), _src.rows()) ) return false; Mat src = _src.getMat(); Mat dst = _dst.getMat(); if ((borderType & BORDER_ISOLATED) == 0 && src.isSubmatrix()) return false; //Process isolated borders only vx_enum border; switch (borderType & ~BORDER_ISOLATED) { case BORDER_CONSTANT: border = VX_BORDER_CONSTANT; break; case BORDER_REPLICATE: // border = VX_BORDER_REPLICATE; // break; default: return false; } try { ivx::Context ctx = ovx::getOpenVXContext(); //if ((vx_size)ksize > ctx.convolutionMaxDimension()) // return false; Mat a; if (dst.data != src.data) a = src; else src.copyTo(a); ivx::Image ia = ivx::Image::createFromHandle(ctx, VX_DF_IMAGE_U8, ivx::Image::createAddressing(a.cols, a.rows, 1, (vx_int32)(a.step)), a.data), ib = ivx::Image::createFromHandle(ctx, VX_DF_IMAGE_S16, ivx::Image::createAddressing(dst.cols, dst.rows, 2, (vx_int32)(dst.step)), dst.data); //ATTENTION: VX_CONTEXT_IMMEDIATE_BORDER attribute change could lead to strange issues in multi-threaded environments //since OpenVX standard says nothing about thread-safety for now ivx::border_t prevBorder = ctx.immediateBorder(); ctx.setImmediateBorder(border, (vx_uint8)(0)); if(dx) ivx::IVX_CHECK_STATUS(vxuSobel3x3(ctx, ia, ib, NULL)); else ivx::IVX_CHECK_STATUS(vxuSobel3x3(ctx, ia, NULL, ib)); ctx.setImmediateBorder(prevBorder); } catch (ivx::RuntimeError & e) { VX_DbgThrow(e.what()); } catch (ivx::WrapperError & e) { VX_DbgThrow(e.what()); } return true; }
static bool ocl_Laplacian5(InputArray _src, OutputArray _dst, const Mat & kd, const Mat & ks, double scale, double delta, int borderType, int depth, int ddepth) { const size_t tileSizeX = 16; const size_t tileSizeYmin = 8; const ocl::Device dev = ocl::Device::getDefault(); int stype = _src.type(); int sdepth = CV_MAT_DEPTH(stype), cn = CV_MAT_CN(stype), esz = CV_ELEM_SIZE(stype); bool doubleSupport = dev.doubleFPConfig() > 0; if (!doubleSupport && (sdepth == CV_64F || ddepth == CV_64F)) return false; Mat kernelX = kd.reshape(1, 1); if (kernelX.cols % 2 != 1) return false; Mat kernelY = ks.reshape(1, 1); if (kernelY.cols % 2 != 1) return false; CV_Assert(kernelX.cols == kernelY.cols); size_t wgs = dev.maxWorkGroupSize(); size_t lmsz = dev.localMemSize(); size_t src_step = _src.step(), src_offset = _src.offset(); const size_t tileSizeYmax = wgs / tileSizeX; // workaround for Nvidia: 3 channel vector type takes 4*elem_size in local memory int loc_mem_cn = dev.vendorID() == ocl::Device::VENDOR_NVIDIA && cn == 3 ? 4 : cn; if (((src_offset % src_step) % esz == 0) && ( (borderType == BORDER_CONSTANT || borderType == BORDER_REPLICATE) || ((borderType == BORDER_REFLECT || borderType == BORDER_WRAP || borderType == BORDER_REFLECT_101) && (_src.cols() >= (int) (kernelX.cols + tileSizeX) && _src.rows() >= (int) (kernelY.cols + tileSizeYmax))) ) && (tileSizeX * tileSizeYmin <= wgs) && (LAPLACIAN_LOCAL_MEM(tileSizeX, tileSizeYmin, kernelX.cols, loc_mem_cn * 4) <= lmsz) ) { Size size = _src.size(), wholeSize; Point origin; int dtype = CV_MAKE_TYPE(ddepth, cn); int wdepth = CV_32F; size_t tileSizeY = tileSizeYmax; while ((tileSizeX * tileSizeY > wgs) || (LAPLACIAN_LOCAL_MEM(tileSizeX, tileSizeY, kernelX.cols, loc_mem_cn * 4) > lmsz)) { tileSizeY /= 2; } size_t lt2[2] = { tileSizeX, tileSizeY}; size_t gt2[2] = { lt2[0] * (1 + (size.width - 1) / lt2[0]), lt2[1] }; char cvt[2][40]; const char * const borderMap[] = { "BORDER_CONSTANT", "BORDER_REPLICATE", "BORDER_REFLECT", "BORDER_WRAP", "BORDER_REFLECT_101" }; String opts = cv::format("-D BLK_X=%d -D BLK_Y=%d -D RADIUS=%d%s%s" " -D convertToWT=%s -D convertToDT=%s" " -D %s -D srcT1=%s -D dstT1=%s -D WT1=%s" " -D srcT=%s -D dstT=%s -D WT=%s" " -D CN=%d ", (int)lt2[0], (int)lt2[1], kernelX.cols / 2, ocl::kernelToStr(kernelX, wdepth, "KERNEL_MATRIX_X").c_str(), ocl::kernelToStr(kernelY, wdepth, "KERNEL_MATRIX_Y").c_str(), ocl::convertTypeStr(sdepth, wdepth, cn, cvt[0]), ocl::convertTypeStr(wdepth, ddepth, cn, cvt[1]), borderMap[borderType], ocl::typeToStr(sdepth), ocl::typeToStr(ddepth), ocl::typeToStr(wdepth), ocl::typeToStr(CV_MAKETYPE(sdepth, cn)), ocl::typeToStr(CV_MAKETYPE(ddepth, cn)), ocl::typeToStr(CV_MAKETYPE(wdepth, cn)), cn); ocl::Kernel k("laplacian", ocl::imgproc::laplacian5_oclsrc, opts); if (k.empty()) return false; UMat src = _src.getUMat(); _dst.create(size, dtype); UMat dst = _dst.getUMat(); int src_offset_x = static_cast<int>((src_offset % src_step) / esz); int src_offset_y = static_cast<int>(src_offset / src_step); src.locateROI(wholeSize, origin); k.args(ocl::KernelArg::PtrReadOnly(src), (int)src_step, src_offset_x, src_offset_y, wholeSize.height, wholeSize.width, ocl::KernelArg::WriteOnly(dst), static_cast<float>(scale), static_cast<float>(delta)); return k.run(2, gt2, lt2, false); } int iscale = cvRound(scale), idelta = cvRound(delta); bool floatCoeff = std::fabs(delta - idelta) > DBL_EPSILON || std::fabs(scale - iscale) > DBL_EPSILON; int wdepth = std::max(depth, floatCoeff ? CV_32F : CV_32S), kercn = 1; if (!doubleSupport && wdepth == CV_64F) return false; char cvt[2][40]; ocl::Kernel k("sumConvert", ocl::imgproc::laplacian5_oclsrc, format("-D ONLY_SUM_CONVERT " "-D srcT=%s -D WT=%s -D dstT=%s -D coeffT=%s -D wdepth=%d " "-D convertToWT=%s -D convertToDT=%s%s", ocl::typeToStr(CV_MAKE_TYPE(depth, kercn)), ocl::typeToStr(CV_MAKE_TYPE(wdepth, kercn)), ocl::typeToStr(CV_MAKE_TYPE(ddepth, kercn)), ocl::typeToStr(wdepth), wdepth, ocl::convertTypeStr(depth, wdepth, kercn, cvt[0]), ocl::convertTypeStr(wdepth, ddepth, kercn, cvt[1]), doubleSupport ? " -D DOUBLE_SUPPORT" : "")); if (k.empty()) return false; UMat d2x, d2y; sepFilter2D(_src, d2x, depth, kd, ks, Point(-1, -1), 0, borderType); sepFilter2D(_src, d2y, depth, ks, kd, Point(-1, -1), 0, borderType); UMat dst = _dst.getUMat(); ocl::KernelArg d2xarg = ocl::KernelArg::ReadOnlyNoSize(d2x), d2yarg = ocl::KernelArg::ReadOnlyNoSize(d2y), dstarg = ocl::KernelArg::WriteOnly(dst, cn, kercn); if (wdepth >= CV_32F) k.args(d2xarg, d2yarg, dstarg, (float)scale, (float)delta); else k.args(d2xarg, d2yarg, dstarg, iscale, idelta); size_t globalsize[] = { dst.cols * cn / kercn, dst.rows }; return k.run(2, globalsize, NULL, false); }