/* 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;
}