/* Calculates coefficients of perspective transformation * which maps (xi,yi) to (ui,vi), (i=1,2,3,4): * * c00*xi + c01*yi + c02 * ui = --------------------- * c20*xi + c21*yi + c22 * * c10*xi + c11*yi + c12 * vi = --------------------- * c20*xi + c21*yi + c22 * * Coefficients are calculated by solving linear system: * / x0 y0 1 0 0 0 -x0*u0 -y0*u0 \ /c00\ /u0\ * | x1 y1 1 0 0 0 -x1*u1 -y1*u1 | |c01| |u1| * | x2 y2 1 0 0 0 -x2*u2 -y2*u2 | |c02| |u2| * | x3 y3 1 0 0 0 -x3*u3 -y3*u3 |.|c10|=|u3|, * | 0 0 0 x0 y0 1 -x0*v0 -y0*v0 | |c11| |v0| * | 0 0 0 x1 y1 1 -x1*v1 -y1*v1 | |c12| |v1| * | 0 0 0 x2 y2 1 -x2*v2 -y2*v2 | |c20| |v2| * \ 0 0 0 x3 y3 1 -x3*v3 -y3*v3 / \c21/ \v3/ * * where: * cij - matrix coefficients, c22 = 1 */ int getPerspectiveTransform(const Point2f src1[], const Point2f src2[], Mat_<double, 1>& dst) { FBC_Assert(dst.rows == 3 && dst.cols == 3); Mat_<double, 1> X(8, 1, dst.data); double a[8][8], b[8]; Mat_<double, 1> A(8, 8, a), B(8, 1, b); for (int i = 0; i < 4; ++i) { a[i][0] = a[i + 4][3] = src1[i].x; a[i][1] = a[i + 4][4] = src1[i].y; a[i][2] = a[i + 4][5] = 1; a[i][3] = a[i][4] = a[i][5] = a[i + 4][0] = a[i + 4][1] = a[i + 4][2] = 0; a[i][6] = -src1[i].x*src2[i].x; a[i][7] = -src1[i].y*src2[i].x; a[i + 4][6] = -src1[i].x*src2[i].y; a[i + 4][7] = -src1[i].y*src2[i].y; b[i] = src2[i].x; b[i + 4] = src2[i].y; } solve(A, B, X, DECOMP_SVD); double* p = (double*)dst.ptr(); p[8] = 1.; return 0; }
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar()) { FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0); if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) { Size wholeSize; Point ofs; src.locateROI(wholeSize, ofs); int dtop = std::min(ofs.y, top); int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom); int dleft = std::min(ofs.x, left); int dright = std::min(wholeSize.width - src.cols - ofs.x, right); src.adjustROI(dtop, dbottom, dleft, dright); top -= dtop; left -= dleft; bottom -= dbottom; right -= dright; } if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) { dst.release(); dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right); } if (top == 0 && left == 0 && bottom == 0 && right == 0) { if (src.data != dst.data || src.step != dst.step) src.copyTo(dst); return 0; } borderType &= ~BORDER_ISOLATED; if (borderType != BORDER_CONSTANT) { copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType); } else { int cn = src.channels, cn1 = cn; AutoBuffer<double> buf(cn); scalarToRawData<_Tp, chs>(value, buf, cn); copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf); } return 0; }
void spm_bp::getLocalDataCostPerlabel(int sp, const Vec2f& fl, Mat_<float>& localDataCost) { //USE_COLOR_FEATURES cv::Mat_<cv::Vec3f> subLt = subImage1[sp]; #if USE_CENSUS vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp]; #endif int upHeight, upWidth; upHeight = im1Up.rows; upWidth = im1Up.cols; // extract sub-image from subrange int p1 = repPixels1[sp]; int w = subRange1[p1][2] - subRange1[p1][0] + 1; int h = subRange1[p1][3] - subRange1[p1][1] + 1; int x = subRange1[p1][0]; int y = subRange1[p1][1]; // localDataCost.release(); localDataCost.create(h, w); //Mat_<float> rawCost(h, w); Mat_<Vec3f> subRt(h, w); vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w)); #if SAVE_DCOST if (check_id >= 0) { cout << "hit" << endl; } #endif Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); float* rawCostPtr = (float*)(localDataCost.ptr(0)); cv::Vec3f *im2UpPtr = (cv::Vec3f*) im2Up.ptr(0); int im2UpWidth = im2Up.cols; int maxHeight = upHeight - 1; int maxWidth = upWidth - 1; int cy, cx, oy, ox; float fl0 = fl[0], fl1 = fl[1]; #pragma omp parallel for for (cy = 0; cy < h; ++cy) { oy = y + cy; int oyUp = (oy + fl0) * upScale; if (oyUp < 0) oyUp = 0; if (oyUp >= upHeight) oyUp = maxHeight; for (cx = 0; cx < w; ++cx) { ox = x + cx; int oxUp = (ox + fl1) * upScale; if (oxUp < 0) oxUp = 0; if (oxUp >= upWidth) oxUp = maxWidth; #if USE_POINTER_WISE // *subRtPtr++ = im2Up[oyUp][oxUp]; subRtPtr[cy * w + cx] = im2UpPtr[oyUp * im2UpWidth + oxUp]; #else subRt[cy][cx] = im2Up[oyUp][oxUp]; #endif #if USE_CENSUS subRt_css[cy][cx] = censusBS2[oyUp][oxUp]; #endif } } // calculate raw cost subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); rawCostPtr = (float*)(localDataCost.ptr(0)); int iy, ix; for (iy = 0; iy < h; ++iy) { for (ix = 0; ix < w; ++ix) { #if DATA_COST_ADCENSUS bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix]; #if USE_POINTER_WISE float dist_c = fabsf((*subLtPtr)[0] - (*subRtPtr)[0]) + fabsf((*subLtPtr)[1] - (*subRtPtr)[1]) + fabsf((*subLtPtr)[2] - (*subRtPtr)[2]); ++subLtPtr; ++subRtPtr; #else float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0]) + std::abs(subLt[iy][ix][1] - subRt[iy][ix][1]) + std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]); #endif float dist_css = expCensusDiffTable[tmpBS.count()]; float dist_ce = expColorDiffTable[int(dist_c / 3)]; #if USE_POINTER_WISE *rawCostPtr++ = 255 * (dist_css + dist_ce); #else localDataCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c); #endif #endif } } #if SAVE_DCOST label_saved[sp].push_back(fl); #endif }
bool invert(const Mat_<_Tp, chs>&src, Mat_<_Tp, chs>& dst, int method = DECOMP_LU) { FBC_Assert(src.data != NULL && dst.data != NULL); FBC_Assert(src.cols > 0 && src.rows > 0 && dst.cols > 0 && dst.rows > 0); FBC_Assert(typeid(double).name() == typeid(_Tp).name() || typeid(float).name() == typeid(_Tp).name()); FBC_Assert(src.cols == dst.rows && src.rows == dst.cols); bool result = false; size_t esz = sizeof(_Tp) * chs; // size_t esz = CV_ELEM_SIZE(type); int m = src.rows, n = src.cols; if (method == DECOMP_SVD) { // TODO FBC_Assert(0); } FBC_Assert(m == n); if (method == DECOMP_EIG) { // TODO FBC_Assert(0); } FBC_Assert(method == DECOMP_LU || method == DECOMP_CHOLESKY); if (n <= 3) { const uchar* srcdata = src.ptr(); uchar* dstdata = const_cast<uchar*>(dst.ptr()); size_t srcstep = src.step; size_t dststep = dst.step; if (n == 2) { // TODO FBC_Assert(0); } else if (n == 3) { if (typeid(float).name() == typeid(_Tp).name() && chs == 1) { double d = det3(Sf); if (d != 0.) { double t[12]; result = true; d = 1./d; t[0] = (((double)Sf(1,1) * Sf(2,2) - (double)Sf(1,2) * Sf(2,1)) * d); t[1] = (((double)Sf(0,2) * Sf(2,1) - (double)Sf(0,1) * Sf(2,2)) * d); t[2] = (((double)Sf(0,1) * Sf(1,2) - (double)Sf(0,2) * Sf(1,1)) * d); t[3] = (((double)Sf(1,2) * Sf(2,0) - (double)Sf(1,0) * Sf(2,2)) * d); t[4] = (((double)Sf(0,0) * Sf(2,2) - (double)Sf(0,2) * Sf(2,0)) * d); t[5] = (((double)Sf(0,2) * Sf(1,0) - (double)Sf(0,0) * Sf(1,2)) * d); t[6] = (((double)Sf(1,0) * Sf(2,1) - (double)Sf(1,1) * Sf(2,0)) * d); t[7] = (((double)Sf(0,1) * Sf(2,0) - (double)Sf(0,0) * Sf(2,1)) * d); t[8] = (((double)Sf(0,0) * Sf(1,1) - (double)Sf(0,1) * Sf(1,0)) * d); Df(0,0) = (float)t[0]; Df(0,1) = (float)t[1]; Df(0,2) = (float)t[2]; Df(1,0) = (float)t[3]; Df(1,1) = (float)t[4]; Df(1,2) = (float)t[5]; Df(2, 0) = (float)t[6]; Df(2, 1) = (float)t[7]; Df(2, 2) = (float)t[8]; } } else { double d = det3(Sd); if (d != 0.) { result = true; d = 1. / d; double t[9]; t[0] = (Sd(1, 1) * Sd(2, 2) - Sd(1, 2) * Sd(2, 1)) * d; t[1] = (Sd(0, 2) * Sd(2, 1) - Sd(0, 1) * Sd(2, 2)) * d; t[2] = (Sd(0, 1) * Sd(1, 2) - Sd(0, 2) * Sd(1, 1)) * d; t[3] = (Sd(1, 2) * Sd(2, 0) - Sd(1, 0) * Sd(2, 2)) * d; t[4] = (Sd(0, 0) * Sd(2, 2) - Sd(0, 2) * Sd(2, 0)) * d; t[5] = (Sd(0, 2) * Sd(1, 0) - Sd(0, 0) * Sd(1, 2)) * d; t[6] = (Sd(1, 0) * Sd(2, 1) - Sd(1, 1) * Sd(2, 0)) * d; t[7] = (Sd(0, 1) * Sd(2, 0) - Sd(0, 0) * Sd(2, 1)) * d; t[8] = (Sd(0, 0) * Sd(1, 1) - Sd(0, 1) * Sd(1, 0)) * d; Dd(0, 0) = t[0]; Dd(0, 1) = t[1]; Dd(0, 2) = t[2]; Dd(1, 0) = t[3]; Dd(1, 1) = t[4]; Dd(1, 2) = t[5]; Dd(2, 0) = t[6]; Dd(2, 1) = t[7]; Dd(2, 2) = t[8]; } } } else { assert(n == 1); if (typeid(float).name() == typeid(_Tp).name() && chs == 1) { double d = Sf(0, 0); if (d != 0.) { result = true; Df(0, 0) = (float)(1. / d); } } else { double d = Sd(0, 0); if (d != 0.) { result = true; Dd(0, 0) = 1. / d; } } } if (!result) dst.setTo(Scalar(0)); return result; } FBC_Assert(0); // TODO return result; }