generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Warning fixes continued

Browse Source
This commit is contained in:
Andrey Kamaev
2012-06-09 15:00:04 +00:00
parent f6b451c607
commit f2d3b9b4a1
127 changed files with 6298 additions and 6277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
modules/calib3d/src/calibinit.cpp
View File

@@ -230,7 +230,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
int found = 0;
CvCBQuad *quads = 0, **quad_group = 0;
CvCBCorner *corners = 0, **corner_group = 0;
try
{
int k = 0;
@@ -252,11 +252,11 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
if( out_corner_count )
*out_corner_count = 0;
IplImage _img;
int check_chessboard_result;
int quad_count = 0, group_idx = 0, i = 0, dilations = 0;
int quad_count = 0, group_idx = 0, dilations = 0;
img = cvGetMat( img, &stub );
//debug_img = img;
@@ -316,8 +316,8 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
for( dilations = min_dilations; dilations <= max_dilations; dilations++ )
{
if (found)
break; // already found it
break; // already found it
cvFree(&quads);
cvFree(&corners);
@@ -378,7 +378,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
cvCopy(dbg_img, dbg1_img);
cvNamedWindow("all_quads", 1);
// copy corners to temp array
for( i = 0; i < quad_count; i++ )
for(int i = 0; i < quad_count; i++ )
{
for (int k=0; k<4; k++)
{
@@ -432,7 +432,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
cvCopy(dbg_img,dbg2_img);
cvNamedWindow("connected_group", 1);
// copy corners to temp array
for( i = 0; i < quad_count; i++ )
for(int i = 0; i < quad_count; i++ )
{
if (quads[i].group_idx == group_idx)
for (int k=0; k<4; k++)
@@ -455,7 +455,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
#endif
if (count == 0)
continue; // haven't found inner quads
continue; // haven't found inner quads
// If count is more than it should be, this will remove those quads
@@ -472,7 +472,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
float sum_dist = 0;
int total = 0;
for( i = 0; i < n; i++ )
for(int i = 0; i < n; i++ )
{
int ni = 0;
float avgi = corner_group[i]->meanDist(&ni);
@@ -484,7 +484,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
if( count > 0 || (out_corner_count && -count > *out_corner_count) )
{
// copy corners to output array
for( i = 0; i < n; i++ )
for(int i = 0; i < n; i++ )
out_corners[i] = corner_group[i]->pt;
if( out_corner_count )
@@ -505,19 +505,19 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
if( found )
found = icvCheckBoardMonotony( out_corners, pattern_size );
// check that none of the found corners is too close to the image boundary
// check that none of the found corners is too close to the image boundary
if( found )
{
const int BORDER = 8;
for( k = 0; k < pattern_size.width*pattern_size.height; k++ )
{
if( out_corners[k].x <= BORDER || out_corners[k].x > img->cols - BORDER ||
out_corners[k].y <= BORDER || out_corners[k].y > img->rows - BORDER )
break;
}
found = k == pattern_size.width*pattern_size.height;
}
{
const int BORDER = 8;
for( k = 0; k < pattern_size.width*pattern_size.height; k++ )
{
if( out_corners[k].x <= BORDER || out_corners[k].x > img->cols - BORDER ||
out_corners[k].y <= BORDER || out_corners[k].y > img->rows - BORDER )
break;
}
found = k == pattern_size.width*pattern_size.height;
}
if( found && pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0 )
{
@@ -525,8 +525,8 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
double dy0 = out_corners[last_row].y - out_corners[0].y;
if( dy0 < 0 )
{
int i, n = pattern_size.width*pattern_size.height;
for( i = 0; i < n/2; i++ )
int n = pattern_size.width*pattern_size.height;
for(int i = 0; i < n/2; i++ )
{
CvPoint2D32f temp;
CV_SWAP(out_corners[i], out_corners[n-i-1], temp);
@@ -559,7 +559,7 @@ int cvFindChessboardCorners( const void* arr, CvSize pattern_size,
cvFree(&corner_group);
throw;
}
cvFree(&quads);
cvFree(&corners);
cvFree(&quad_group);
@@ -582,7 +582,7 @@ static int
icvCheckBoardMonotony( CvPoint2D32f* corners, CvSize pattern_size )
{
int i, j, k;
for( k = 0; k < 2; k++ )
{
for( i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++ )
@@ -627,11 +627,10 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
{
cv::Ptr<CvMemStorage> temp_storage = cvCreateChildMemStorage( storage );
CvSeq* stack = cvCreateSeq( 0, sizeof(*stack), sizeof(void*), temp_storage );
int i;
// first find an interior quad
CvCBQuad *start = NULL;
for (i=0; i<quad_count; i++)
for (int i=0; i<quad_count; i++)
{
if (quads[i]->count == 4)
{
@@ -682,7 +681,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
case 1:
col += 2; break;
case 2:
row += 2; break;
row += 2; break;
case 3:
col -= 2; break;
}
@@ -700,7 +699,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
}
}
for (i=col_min; i<=col_max; i++)
for (int i=col_min; i<=col_max; i++)
PRINTF("HIST[%d] = %d\n", i, col_hist[i]);
// analyze inner quad structure
@@ -763,7 +762,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
// if there is an outer quad missing, fill it in
// first order all inner quads
int found = 0;
for (i=0; i<quad_count; i++)
for (int i=0; i<quad_count; i++)
{
if (quads[i]->count == 4)
{ // ok, look at neighbors
@@ -778,7 +777,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
case 1:
col += 2; break;
case 2:
row += 2; break;
row += 2; break;
case 3:
col -= 2; break;
}
@@ -817,7 +816,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
// final trimming of outer quads
if (dcol == w && drow == h) // found correct inner quads
if (dcol == w && drow == h) // found correct inner quads
{
PRINTF("Inner bounds ok, check outer quads\n");
int rcount = quad_count;
@@ -832,7 +831,7 @@ icvOrderFoundConnectedQuads( int quad_count, CvCBQuad **quads,
if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered)
outer = true;
}
if (!outer) // not an outer quad, eliminate
if (!outer) // not an outer quad, eliminate
{
PRINTF("Removing quad %d\n", i);
icvRemoveQuadFromGroup(quads,rcount,quads[i]);
@@ -876,7 +875,7 @@ icvAddOuterQuad( CvCBQuad *quad, CvCBQuad **quads, int quad_count,
quad->count += 1;
q->neighbors[j] = quad;
q->group_idx = quad->group_idx;
q->count = 1; // number of neighbors
q->count = 1; // number of neighbors
q->ordered = false;
q->edge_len = quad->edge_len;
@@ -1262,7 +1261,7 @@ icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
int width = 0, height = 0;
int hist[5] = {0,0,0,0,0};
CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
// build dual graph, which vertices are internal quad corners
// and two vertices are connected iff they lie on the same quad edge
for( i = 0; i < quad_count; i++ )
@@ -1485,7 +1484,7 @@ icvCheckQuadGroup( CvCBQuad **quad_group, int quad_count,
result = corner_count;
finalize:
if( result <= 0 )
{
corner_count = MIN( corner_count, pattern_size.width*pattern_size.height );
@@ -1697,7 +1696,7 @@ icvGenerateQuads( CvCBQuad **out_quads, CvCBCorner **out_corners,
CV_POLY_APPROX_DP, (float)approx_level );
if( dst_contour->total == 4 )
break;
// we call this again on its own output, because sometimes
// cvApproxPoly() does not simplify as much as it should.
dst_contour = cvApproxPoly( dst_contour, sizeof(CvContour), temp_storage,
@@ -2006,17 +2005,17 @@ bool cv::findCirclesGrid( InputArray _image, Size patternSize,
#endif
if (isFound)
{
switch(parameters.gridType)
{
switch(parameters.gridType)
{
case CirclesGridFinderParameters::SYMMETRIC_GRID:
boxFinder.getHoles(centers);
break;
case CirclesGridFinderParameters::ASYMMETRIC_GRID:
boxFinder.getAsymmetricHoles(centers);
break;
boxFinder.getAsymmetricHoles(centers);
break;
default:
CV_Error(CV_StsBadArg, "Unkown pattern type");
}
}
if (i != 0)
{
@@ -2027,7 +2026,7 @@ bool cv::findCirclesGrid( InputArray _image, Size patternSize,
Mat(centers).copyTo(_centers);
return true;
}
boxFinder.getHoles(centers);
if (i != attempts - 1)
{

2
modules/calib3d/src/calibration.cpp
View File

@@ -1153,7 +1153,7 @@ CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
int useExtrinsicGuess )
{
const int max_iter = 20;
Ptr<CvMat> matM, _Mxy, _m, _mn, matL, matJ;
Ptr<CvMat> matM, _Mxy, _m, _mn, matL;
int i, count;
double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];

16
modules/calib3d/src/circlesgrid.cpp
View File

@@ -65,14 +65,14 @@ void drawPoints(const vector<Point2f> &points, Mat &outImage, int radius = 2, S
}
#endif
void CirclesGridClusterFinder::hierarchicalClustering(const vector<Point2f> points, const Size &patternSize, vector<Point2f> &patternPoints)
void CirclesGridClusterFinder::hierarchicalClustering(const vector<Point2f> points, const Size &patternSz, vector<Point2f> &patternPoints)
{
#ifdef HAVE_TEGRA_OPTIMIZATION
if(tegra::hierarchicalClustering(points, patternSize, patternPoints))
if(tegra::hierarchicalClustering(points, patternSz, patternPoints))
return;
#endif
int i, j, n = (int)points.size();
size_t pn = static_cast<size_t>(patternSize.area());
int j, n = (int)points.size();
size_t pn = static_cast<size_t>(patternSz.area());
patternPoints.clear();
if (pn >= points.size())
@@ -84,7 +84,7 @@ void CirclesGridClusterFinder::hierarchicalClustering(const vector<Point2f> poin
Mat dists(n, n, CV_32FC1, Scalar(0));
Mat distsMask(dists.size(), CV_8UC1, Scalar(0));
for(i = 0; i < n; i++)
for(int i = 0; i < n; i++)
{
for(j = i+1; j < n; j++)
{
@@ -122,7 +122,7 @@ void CirclesGridClusterFinder::hierarchicalClustering(const vector<Point2f> poin
}
//the largest cluster can have more than pn points -- we need to filter out such situations
if(clusters[patternClusterIdx].size() != static_cast<size_t>(patternSize.area()))
if(clusters[patternClusterIdx].size() != static_cast<size_t>(patternSz.area()))
{
return;
}
@@ -505,11 +505,11 @@ void Graph::floydWarshall(cv::Mat &distanceMatrix, int infinity) const
{
for (Vertices::const_iterator it3 = vertices.begin(); it3 != vertices.end(); it3++)
{
int i1 = (int)it1->first, i2 = (int)it2->first, i3 = (int)it3->first;
int i1 = (int)it1->first, i2 = (int)it2->first, i3 = (int)it3->first;
int val1 = distanceMatrix.at<int> (i2, i3);
int val2;
if (distanceMatrix.at<int> (i2, i1) == infinity ||
distanceMatrix.at<int> (i1, i3) == infinity)
distanceMatrix.at<int> (i1, i3) == infinity)
val2 = val1;
else
{

166
modules/calib3d/src/epnp.cpp
View File

@@ -8,26 +8,26 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_camera_parameters<double>(cameraMatrix);
number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
pws.resize(3 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
max_nr = 0;
@@ -97,15 +97,15 @@ void epnp::compute_barycentric_coordinates(void)
for(int j = 0; j < 3; j++)
a[1 + j] =
ci[3 * j ] * (pi[0] - cws[0][0]) +
ci[3 * j + 1] * (pi[1] - cws[0][1]) +
ci[3 * j + 2] * (pi[2] - cws[0][2]);
ci[3 * j ] * (pi[0] - cws[0][0]) +
ci[3 * j + 1] * (pi[1] - cws[0][1]) +
ci[3 * j + 2] * (pi[2] - cws[0][2]);
a[0] = 1.0f - a[1] - a[2] - a[3];
}
}
void epnp::fill_M(CvMat * M,
const int row, const double * as, const double u, const double v)
const int row, const double * as, const double u, const double v)
{
double * M1 = M->data.db + row * 12;
double * M2 = M1 + 12;
@@ -130,7 +130,7 @@ void epnp::compute_ccs(const double * betas, const double * ut)
const double * v = ut + 12 * (11 - i);
for(int j = 0; j < 4; j++)
for(int k = 0; k < 3; k++)
ccs[j][k] += betas[i] * v[3 * j + k];
ccs[j][k] += betas[i] * v[3 * j + k];
}
}
@@ -195,7 +195,7 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
}
void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
double R_dst[3][3], double t_dst[3])
double R_dst[3][3], double t_dst[3])
{
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
@@ -282,7 +282,7 @@ void epnp::solve_for_sign(void)
if (pcs[2] < 0.0) {
for(int i = 0; i < 4; i++)
for(int j = 0; j < 3; j++)
ccs[i][j] = -ccs[i][j];
ccs[i][j] = -ccs[i][j];
for(int i = 0; i < number_of_correspondences; i++) {
pcs[3 * i ] = -pcs[3 * i];
@@ -293,7 +293,7 @@ void epnp::solve_for_sign(void)
}
double epnp::compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3])
double R[3][3], double t[3])
{
compute_ccs(betas, ut);
compute_pcs();
@@ -322,13 +322,13 @@ double epnp::reprojection_error(const double R[3][3], const double t[3])
}
return sum2 / number_of_correspondences;
}
}
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_1 = [B11 B12 B13 B14]
void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x4[6 * 4], b4[4];
CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
@@ -360,7 +360,7 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
// betas_approx_2 = [B11 B12 B22 ]
void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x3[6 * 3], b3[3];
CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
@@ -392,7 +392,7 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
// betas_approx_3 = [B11 B12 B22 B13 B23 ]
void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x5[6 * 5], b5[5];
CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
@@ -440,8 +440,8 @@ void epnp::compute_L_6x10(const double * ut, double * l_6x10)
b++;
if (b > 3) {
a++;
b = a + 1;
a++;
b = a + 1;
}
}
}
@@ -473,7 +473,7 @@ void epnp::compute_rho(double * rho)
}
void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double betas[4], CvMat * A, CvMat * b)
const double betas[4], CvMat * A, CvMat * b)
{
for(int i = 0; i < 6; i++) {
const double * rowL = l_6x10 + i * 10;
@@ -485,23 +485,22 @@ void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rh
rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
cvmSet(b, i, 0, rho[i] -
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[1] * betas[1] +
rowL[3] * betas[0] * betas[2] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
rowL[6] * betas[0] * betas[3] +
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[1] * betas[1] +
rowL[3] * betas[0] * betas[2] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
rowL[6] * betas[0] * betas[3] +
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
}
}
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
double betas[4])
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4])
{
const int iterations_number = 5;
@@ -510,12 +509,13 @@ void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
CvMat B = cvMat(6, 1, CV_64F, b);
CvMat X = cvMat(4, 1, CV_64F, x);
for(int k = 0; k < iterations_number; k++) {
for(int k = 0; k < iterations_number; k++)
{
compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
betas, &A, &B);
betas, &A, &B);
qr_solve(&A, &B, &X);
for(int i = 0; i < 4; i++)
betas[i] += x[i];
betas[i] += x[i];
}
}
@@ -524,53 +524,64 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
const int nr = A->rows;
const int nc = A->cols;
if (max_nr != 0 && max_nr < nr) {
if (max_nr != 0 && max_nr < nr)
{
delete [] A1;
delete [] A2;
}
if (max_nr < nr) {
if (max_nr < nr)
{
max_nr = nr;
A1 = new double[nr];
A2 = new double[nr];
}
double * pA = A->data.db, * ppAkk = pA;
for(int k = 0; k < nc; k++) {
double * ppAik = ppAkk, eta = fabs(*ppAik);
for(int i = k + 1; i < nr; i++) {
double elt = fabs(*ppAik);
for(int k = 0; k < nc; k++)
{
double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
for(int i = k + 1; i < nr; i++)
{
double elt = fabs(*ppAik1);
if (eta < elt) eta = elt;
ppAik += nc;
ppAik1 += nc;
}
if (eta == 0) {
if (eta == 0)
{
A1[k] = A2[k] = 0.0;
//cerr << "God damnit, A is singular, this shouldn't happen." << endl;
return;
} else {
double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++) {
*ppAik *= inv_eta;
sum += *ppAik * *ppAik;
ppAik += nc;
}
else
{
double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++)
{
*ppAik2 *= inv_eta;
sum2 += *ppAik2 * *ppAik2;
ppAik2 += nc;
}
double sigma = sqrt(sum);
double sigma = sqrt(sum2);
if (*ppAkk < 0)
sigma = -sigma;
sigma = -sigma;
*ppAkk += sigma;
A1[k] = sigma * *ppAkk;
A2[k] = -eta * sigma;
for(int j = k + 1; j < nc; j++) {
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++) {
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++) {
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
for(int j = k + 1; j < nc; j++)
{
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++)
{
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++)
{
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
}
}
ppAkk += nc + 1;
@@ -578,15 +589,18 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
// b <- Qt b
double * ppAjj = pA, * pb = b->data.db;
for(int j = 0; j < nc; j++) {
for(int j = 0; j < nc; j++)
{
double * ppAij = ppAjj, tau = 0;
for(int i = j; i < nr; i++) {
for(int i = j; i < nr; i++)
{
tau += *ppAij * pb[i];
ppAij += nc;
}
tau /= A1[j];
ppAij = ppAjj;
for(int i = j; i < nr; i++) {
for(int i = j; i < nr; i++)
{
pb[i] -= tau * *ppAij;
ppAij += nc;
}
@@ -596,10 +610,12 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
// X = R-1 b
double * pX = X->data.db;
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--) {
for(int i = nc - 2; i >= 0; i--)
{
double * ppAij = pA + i * nc + (i + 1), sum = 0;
for(int j = i + 1; j < nc; j++) {
for(int j = i + 1; j < nc; j++)
{
sum += *ppAij * pX[j];
ppAij++;
}

580
modules/calib3d/src/p3p.cpp
View File

@@ -9,151 +9,151 @@ using namespace std;
void p3p::init_inverse_parameters()
{
inv_fx = 1. / fx;
inv_fy = 1. / fy;
cx_fx = cx / fx;
cy_fy = cy / fy;
inv_fx = 1. / fx;
inv_fy = 1. / fy;
cx_fx = cx / fx;
cy_fy = cy / fy;
}
p3p::p3p(cv::Mat cameraMatrix)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_inverse_parameters();
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_inverse_parameters();
}
p3p::p3p(double _fx, double _fy, double _cx, double _cy)
{
fx = _fx;
fy = _fy;
cx = _cx;
cy = _cy;
init_inverse_parameters();
fx = _fx;
fy = _fy;
cx = _cx;
cy = _cy;
init_inverse_parameters();
}
bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
{
double rotation_matrix[3][3], translation[3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
double rotation_matrix[3][3], translation[3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
return result;
return result;
}
bool p3p::solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3)
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3)
{
double Rs[4][3][3], ts[4][3];
double Rs[4][3][3], ts[4][3];
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
if (n == 0)
return false;
if (n == 0)
return false;
int ns = 0;
double min_reproj = 0;
for(int i = 0; i < n; i++) {
double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
double mu3p = cx + fx * X3p / Z3p;
double mv3p = cy + fy * Y3p / Z3p;
double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
if (i == 0 || min_reproj > reproj) {
ns = i;
min_reproj = reproj;
}
}
int ns = 0;
double min_reproj = 0;
for(int i = 0; i < n; i++) {
double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
double mu3p = cx + fx * X3p / Z3p;
double mv3p = cy + fy * Y3p / Z3p;
double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
if (i == 0 || min_reproj > reproj) {
ns = i;
min_reproj = reproj;
}
}
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R[i][j] = Rs[ns][i][j];
t[i] = ts[ns][i];
}
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R[i][j] = Rs[ns][i][j];
t[i] = ts[ns][i];
}
return true;
return true;
}
int p3p::solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2)
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2)
{
double mk0, mk1, mk2;
double norm;
double mk0, mk1, mk2;
double norm;
mu0 = inv_fx * mu0 - cx_fx;
mv0 = inv_fy * mv0 - cy_fy;
norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
mu0 = inv_fx * mu0 - cx_fx;
mv0 = inv_fy * mv0 - cy_fy;
norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
mu1 = inv_fx * mu1 - cx_fx;
mv1 = inv_fy * mv1 - cy_fy;
norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
mu1 = inv_fx * mu1 - cx_fx;
mv1 = inv_fy * mv1 - cy_fy;
norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
mu2 = inv_fx * mu2 - cx_fx;
mv2 = inv_fy * mv2 - cy_fy;
norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
mu2 = inv_fx * mu2 - cx_fx;
mv2 = inv_fy * mv2 - cy_fy;
norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
double distances[3];
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
double distances[3];
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
// Calculate angles
double cosines[3];
cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
// Calculate angles
double cosines[3];
cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
double lengths[4][3];
int n = solve_for_lengths(lengths, distances, cosines);
double lengths[4][3];
int n = solve_for_lengths(lengths, distances, cosines);
int nb_solutions = 0;
for(int i = 0; i < n; i++) {
double M_orig[3][3];
int nb_solutions = 0;
for(int i = 0; i < n; i++) {
double M_orig[3][3];
M_orig[0][0] = lengths[i][0] * mu0;
M_orig[0][1] = lengths[i][0] * mv0;
M_orig[0][2] = lengths[i][0] * mk0;
M_orig[0][0] = lengths[i][0] * mu0;
M_orig[0][1] = lengths[i][0] * mv0;
M_orig[0][2] = lengths[i][0] * mk0;
M_orig[1][0] = lengths[i][1] * mu1;
M_orig[1][1] = lengths[i][1] * mv1;
M_orig[1][2] = lengths[i][1] * mk1;
M_orig[1][0] = lengths[i][1] * mu1;
M_orig[1][1] = lengths[i][1] * mv1;
M_orig[1][2] = lengths[i][1] * mk1;
M_orig[2][0] = lengths[i][2] * mu2;
M_orig[2][1] = lengths[i][2] * mv2;
M_orig[2][2] = lengths[i][2] * mk2;
M_orig[2][0] = lengths[i][2] * mu2;
M_orig[2][1] = lengths[i][2] * mv2;
M_orig[2][2] = lengths[i][2] * mk2;
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
continue;
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
continue;
nb_solutions++;
}
nb_solutions++;
}
return nb_solutions;
return nb_solutions;
}
/// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
@@ -170,247 +170,247 @@ int p3p::solve(double R[4][3][3], double t[4][3],
int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
{
double p = cosines[0] * 2;
double q = cosines[1] * 2;
double r = cosines[2] * 2;
double p = cosines[0] * 2;
double q = cosines[1] * 2;
double r = cosines[2] * 2;
double inv_d22 = 1. / (distances[2] * distances[2]);
double a = inv_d22 * (distances[0] * distances[0]);
double b = inv_d22 * (distances[1] * distances[1]);
double inv_d22 = 1. / (distances[2] * distances[2]);
double a = inv_d22 * (distances[0] * distances[0]);
double b = inv_d22 * (distances[1] * distances[1]);
double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
double pr = p * r, pqr = q * pr;
double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
double pr = p * r, pqr = q * pr;
// Check reality condition (the four points should not be coplanar)
if (p2 + q2 + r2 - pqr - 1 == 0)
return 0;
// Check reality condition (the four points should not be coplanar)
if (p2 + q2 + r2 - pqr - 1 == 0)
return 0;
double ab = a * b, a_2 = 2*a;
double ab = a * b, a_2 = 2*a;
double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2;
double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2;
// Check reality condition
if (A == 0) return 0;
// Check reality condition
if (A == 0) return 0;
double a_4 = 4*a;
double a_4 = 4*a;
double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab);
double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2;
double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2);
double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2;
double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab);
double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2;
double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2);
double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2;
double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr);
double b0 = b * temp * temp;
// Check reality condition
if (b0 == 0)
return 0;
double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr);
double b0 = b * temp * temp;
// Check reality condition
if (b0 == 0)
return 0;
double real_roots[4];
int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]);
double real_roots[4];
int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]);
if (n == 0)
return 0;
if (n == 0)
return 0;
int nb_solutions = 0;
double r3 = r2*r, pr2 = p*r2, r3q = r3 * q;
double inv_b0 = 1. / b0;
int nb_solutions = 0;
double r3 = r2*r, pr2 = p*r2, r3q = r3 * q;
double inv_b0 = 1. / b0;
// For each solution of x
for(int i = 0; i < n; i++) {
double x = real_roots[i];
// For each solution of x
for(int i = 0; i < n; i++) {
double x = real_roots[i];
// Check reality condition
if (x <= 0)
continue;
// Check reality condition
if (x <= 0)
continue;
double x2 = x*x;
double x2 = x*x;
double b1 =
((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) *
(((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x +
double b1 =
((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) *
(((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x +
(r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 +
(r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 +
(r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x +
(r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x +
2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) +
p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1)));
2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) +
p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1)));
// Check reality condition
if (b1 <= 0)
continue;
// Check reality condition
if (b1 <= 0)
continue;
double y = inv_b0 * b1;
double v = x2 + y*y - x*y*r;
double y = inv_b0 * b1;
double v = x2 + y*y - x*y*r;
if (v <= 0)
continue;
if (v <= 0)
continue;
double Z = distances[2] / sqrt(v);
double X = x * Z;
double Y = y * Z;
double Z = distances[2] / sqrt(v);
double X = x * Z;
double Y = y * Z;
lengths[nb_solutions][0] = X;
lengths[nb_solutions][1] = Y;
lengths[nb_solutions][2] = Z;
lengths[nb_solutions][0] = X;
lengths[nb_solutions][1] = Y;
lengths[nb_solutions][2] = Z;
nb_solutions++;
}
nb_solutions++;
}
return nb_solutions;
return nb_solutions;
}
bool p3p::align(double M_end[3][3],
double X0, double Y0, double Z0,
double X1, double Y1, double Z1,
double X2, double Y2, double Z2,
double R[3][3], double T[3])
double X0, double Y0, double Z0,
double X1, double Y1, double Z1,
double X2, double Y2, double Z2,
double R[3][3], double T[3])
{
// Centroids:
double C_start[3], C_end[3];
for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
C_start[0] = (X0 + X1 + X2) / 3;
C_start[1] = (Y0 + Y1 + Y2) / 3;
C_start[2] = (Z0 + Z1 + Z2) / 3;
// Centroids:
double C_start[3], C_end[3];
for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
C_start[0] = (X0 + X1 + X2) / 3;
C_start[1] = (Y0 + Y1 + Y2) / 3;
C_start[2] = (Z0 + Z1 + Z2) / 3;
// Covariance matrix s:
double s[3 * 3];
for(int j = 0; j < 3; j++) {
s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
}
// Covariance matrix s:
double s[3 * 3];
for(int j = 0; j < 3; j++) {
s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
}
double Qs[16], evs[4], U[16];
double Qs[16], evs[4], U[16];
Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
jacobi_4x4(Qs, evs, U);
jacobi_4x4(Qs, evs, U);
// Looking for the largest eigen value:
int i_ev = 0;
double ev_max = evs[i_ev];
for(int i = 1; i < 4; i++)
if (evs[i] > ev_max)
ev_max = evs[i_ev = i];
// Looking for the largest eigen value:
int i_ev = 0;
double ev_max = evs[i_ev];
for(int i = 1; i < 4; i++)
if (evs[i] > ev_max)
ev_max = evs[i_ev = i];
// Quaternion:
double q[4];
for(int i = 0; i < 4; i++)
q[i] = U[i * 4 + i_ev];
// Quaternion:
double q[4];
for(int i = 0; i < 4; i++)
q[i] = U[i * 4 + i_ev];
double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];
double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
double q2_3 = q[2] * q[3];
double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];
double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
double q2_3 = q[2] * q[3];
R[0][0] = q02 + q12 - q22 - q32;
R[0][1] = 2. * (q1_2 - q0_3);
R[0][2] = 2. * (q1_3 + q0_2);
R[0][0] = q02 + q12 - q22 - q32;
R[0][1] = 2. * (q1_2 - q0_3);
R[0][2] = 2. * (q1_3 + q0_2);
R[1][0] = 2. * (q1_2 + q0_3);
R[1][1] = q02 + q22 - q12 - q32;
R[1][2] = 2. * (q2_3 - q0_1);
R[1][0] = 2. * (q1_2 + q0_3);
R[1][1] = q02 + q22 - q12 - q32;
R[1][2] = 2. * (q2_3 - q0_1);
R[2][0] = 2. * (q1_3 - q0_2);
R[2][1] = 2. * (q2_3 + q0_1);
R[2][2] = q02 + q32 - q12 - q22;
R[2][0] = 2. * (q1_3 - q0_2);
R[2][1] = 2. * (q2_3 + q0_1);
R[2][2] = q02 + q32 - q12 - q22;
for(int i = 0; i < 3; i++)
T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);
for(int i = 0; i < 3; i++)
T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);
return true;
return true;
}
bool p3p::jacobi_4x4(double * A, double * D, double * U)
{
double B[4], Z[4];
double Id[16] = {1., 0., 0., 0.,
0., 1., 0., 0.,
0., 0., 1., 0.,
0., 0., 0., 1.};
double B[4], Z[4];
double Id[16] = {1., 0., 0., 0.,
0., 1., 0., 0.,
0., 0., 1., 0.,
0., 0., 0., 1.};
memcpy(U, Id, 16 * sizeof(double));
memcpy(U, Id, 16 * sizeof(double));
B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
memcpy(D, B, 4 * sizeof(double));
memset(Z, 0, 4 * sizeof(double));
B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
memcpy(D, B, 4 * sizeof(double));
memset(Z, 0, 4 * sizeof(double));
for(int iter = 0; iter < 50; iter++) {
double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
for(int iter = 0; iter < 50; iter++) {
double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
if (sum == 0.0)
return true;
if (sum == 0.0)
return true;
double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
for(int i = 0; i < 3; i++) {
double * pAij = A + 5 * i + 1;
for(int j = i + 1 ; j < 4; j++) {
double Aij = *pAij;
double eps_machine = 100.0 * fabs(Aij);
double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
for(int i = 0; i < 3; i++) {
double * pAij = A + 5 * i + 1;
for(int j = i + 1 ; j < 4; j++) {
double Aij = *pAij;
double eps_machine = 100.0 * fabs(Aij);
if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) )
*pAij = 0.0;
else if (fabs(Aij) > tresh) {
double h = D[j] - D[i], t;
if (fabs(h) + eps_machine == fabs(h))
t = Aij / h;
else {
double theta = 0.5 * h / Aij;
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
if (theta < 0.0) t = -t;
}
if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) )
*pAij = 0.0;
else if (fabs(Aij) > tresh) {
double hh = D[j] - D[i], t;
if (fabs(hh) + eps_machine == fabs(hh))
t = Aij / hh;
else {
double theta = 0.5 * hh / Aij;
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
if (theta < 0.0) t = -t;
}
h = t * Aij;
Z[i] -= h;
Z[j] += h;
D[i] -= h;
D[j] += h;
*pAij = 0.0;
hh = t * Aij;
Z[i] -= hh;
Z[j] += hh;
D[i] -= hh;
D[j] += hh;
*pAij = 0.0;
double c = 1.0 / sqrt(1 + t * t);
double s = t * c;
double tau = s / (1.0 + c);
for(int k = 0; k <= i - 1; k++) {
double g = A[k * 4 + i], h = A[k * 4 + j];
A[k * 4 + i] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = i + 1; k <= j - 1; k++) {
double g = A[i * 4 + k], h = A[k * 4 + j];
A[i * 4 + k] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = j + 1; k < 4; k++) {
double g = A[i * 4 + k], h = A[j * 4 + k];
A[i * 4 + k] = g - s * (h + g * tau);
A[j * 4 + k] = h + s * (g - h * tau);
}
for(int k = 0; k < 4; k++) {
double g = U[k * 4 + i], h = U[k * 4 + j];
U[k * 4 + i] = g - s * (h + g * tau);
U[k * 4 + j] = h + s * (g - h * tau);
}
}
pAij++;
}
}
double c = 1.0 / sqrt(1 + t * t);
double s = t * c;
double tau = s / (1.0 + c);
for(int k = 0; k <= i - 1; k++) {
double g = A[k * 4 + i], h = A[k * 4 + j];
A[k * 4 + i] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = i + 1; k <= j - 1; k++) {
double g = A[i * 4 + k], h = A[k * 4 + j];
A[i * 4 + k] = g - s * (h + g * tau);
A[k * 4 + j] = h + s * (g - h * tau);
}
for(int k = j + 1; k < 4; k++) {
double g = A[i * 4 + k], h = A[j * 4 + k];
A[i * 4 + k] = g - s * (h + g * tau);
A[j * 4 + k] = h + s * (g - h * tau);
}
for(int k = 0; k < 4; k++) {
double g = U[k * 4 + i], h = U[k * 4 + j];
U[k * 4 + i] = g - s * (h + g * tau);
U[k * 4 + j] = h + s * (g - h * tau);
}
}
pAij++;
}
}
for(int i = 0; i < 4; i++) B[i] += Z[i];
memcpy(D, B, 4 * sizeof(double));
memset(Z, 0, 4 * sizeof(double));
}
for(int i = 0; i < 4; i++) B[i] += Z[i];
memcpy(D, B, 4 * sizeof(double));
memset(Z, 0, 4 * sizeof(double));
}
return false;
return false;
}

8
modules/calib3d/src/solvepnp.cpp
View File

@@ -253,10 +253,10 @@ namespace cv
}
}
}
PnPSolver(const Mat& objectPoints, const Mat& imagePoints, const Parameters& parameters,
Mat& rvec, Mat& tvec, vector<int>& inliers):
objectPoints(objectPoints), imagePoints(imagePoints), parameters(parameters),
rvec(rvec), tvec(tvec), inliers(inliers)
PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
Mat& _rvec, Mat& _tvec, vector<int>& _inliers):
objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
rvec(_rvec), tvec(_tvec), inliers(_inliers)
{
rvec.copyTo(initRvec);
tvec.copyTo(initTvec);

141
modules/calib3d/src/stereobm.cpp
View File

@@ -155,7 +155,7 @@ static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uc
val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
dptr[x] = tab[val + OFS];
}
sum += vsum[x+wsz2] - vsum[x-wsz2-1];
val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
dptr[x] = tab[val + OFS];
@@ -170,15 +170,15 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
const int OFS = 256*4, TABSZ = OFS*2 + 256;
uchar tab[TABSZ];
Size size = src.size();
for( x = 0; x < TABSZ; x++ )
tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
uchar val0 = tab[0 + OFS];
#if CV_SSE2
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif
for( y = 0; y < size.height-1; y += 2 )
{
const uchar* srow1 = src.ptr<uchar>(y);
@@ -187,10 +187,10 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
const uchar* srow3 = y < size.height-2 ? srow1 + src.step*2 : srow1;
uchar* dptr0 = dst.ptr<uchar>(y);
uchar* dptr1 = dptr0 + dst.step;
dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0;
x = 1;
#if CV_SSE2
if( useSIMD )
{
@@ -205,26 +205,26 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
d0 = _mm_sub_epi16(d0, c0);
d1 = _mm_sub_epi16(d1, c1);
__m128i c2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
__m128i c3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
__m128i d2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
__m128i d3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
d2 = _mm_sub_epi16(d2, c2);
d3 = _mm_sub_epi16(d3, c3);
__m128i v0 = _mm_add_epi16(d0, _mm_add_epi16(d2, _mm_add_epi16(d1, d1)));
__m128i v1 = _mm_add_epi16(d1, _mm_add_epi16(d3, _mm_add_epi16(d2, d2)));
v0 = _mm_packus_epi16(_mm_add_epi16(v0, ftz), _mm_add_epi16(v1, ftz));
v0 = _mm_min_epu8(v0, ftz2);
_mm_storel_epi64((__m128i*)(dptr0 + x), v0);
_mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
}
}
#endif
for( ; x < size.width-1; x++ )
{
int d0 = srow0[x+1] - srow0[x-1], d1 = srow1[x+1] - srow1[x-1],
@@ -235,7 +235,7 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
dptr1[x] = (uchar)v1;
}
}
for( ; y < size.height; y++ )
{
uchar* dptr = dst.ptr<uchar>(y);
@@ -336,7 +336,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
short* costptr = cost.data ? (short*)cost.data + lofs + x : &costbuf;
int x0 = x - wsz2 - 1, x1 = x + wsz2;
const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
hsad = hsad0 - dy0*ndisp;
lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
@@ -374,7 +374,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
// initialize sums
for( d = 0; d < ndisp; d++ )
sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
for( d = 0; d < ndisp; d += 16 )
@@ -405,28 +405,28 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
{
__m128i u0 = _mm_load_si128((__m128i*)(hsad_sub + d));
__m128i u1 = _mm_load_si128((__m128i*)(hsad + d));
__m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d + 8));
__m128i v1 = _mm_load_si128((__m128i*)(hsad + d + 8));
__m128i usad8 = _mm_load_si128((__m128i*)(sad + d));
__m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8));
u1 = _mm_sub_epi16(u1, u0);
v1 = _mm_sub_epi16(v1, v0);
usad8 = _mm_add_epi16(usad8, u1);
vsad8 = _mm_add_epi16(vsad8, v1);
mask = _mm_cmpgt_epi16(minsad8, usad8);
minsad8 = _mm_min_epi16(minsad8, usad8);
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
_mm_store_si128((__m128i*)(sad + d), usad8);
_mm_store_si128((__m128i*)(sad + d + 8), vsad8);
mask = _mm_cmpgt_epi16(minsad8, vsad8);
minsad8 = _mm_min_epi16(minsad8, vsad8);
d8 = _mm_add_epi16(d8, dd_8);
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
d8 = _mm_add_epi16(d8, dd_8);
@@ -438,32 +438,33 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
dptr[y*dstep] = FILTERED;
continue;
}
__m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8);
__m128i mind82 = _mm_unpackhi_epi64(mind8, mind8);
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
minsad8 = _mm_min_epi16(minsad8, minsad82);
minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2));
mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2));
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
minsad8 = _mm_min_epi16(minsad8, minsad82);
minsad82 = _mm_shufflelo_epi16(minsad8, 1);
mind82 = _mm_shufflelo_epi16(mind8, 1);
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
mind = (short)_mm_cvtsi128_si32(mind8);
minsad = sad[mind];
if( uniquenessRatio > 0 )
{
int thresh = minsad + ((minsad * uniquenessRatio) >> 8);
__m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));
__m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));
__m128i dd_16 = _mm_add_epi16(dd_8, dd_8), d8 = _mm_sub_epi16(d0_8, dd_16);
__m128i dd_16 = _mm_add_epi16(dd_8, dd_8);
d8 = _mm_sub_epi16(d0_8, dd_16);
for( d = 0; d < ndisp; d += 16 )
{
@@ -492,7 +493,8 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
if( 0 < mind && mind < ndisp - 1 )
{
int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind] + std::abs(p - n);
int p = sad[mind+1], n = sad[mind-1];
d = p + n - 2*sad[mind] + std::abs(p - n);
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
}
else
@@ -567,7 +569,7 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right,
htext[y] += tab[lval];
}
}
// initialize the left and right borders of the disparity map
for( y = 0; y < height; y++ )
{
@@ -583,7 +585,7 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right,
int* costptr = cost.data ? (int*)cost.data + lofs + x : &costbuf;
int x0 = x - wsz2 - 1, x1 = x + wsz2;
const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
hsad = hsad0 - dy0*ndisp;
lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
@@ -611,7 +613,7 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right,
// initialize sums
for( d = 0; d < ndisp; d++ )
sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
for( d = 0; d < ndisp; d++ )
@@ -662,7 +664,8 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right,
{
sad[-1] = sad[1];
sad[ndisp] = sad[ndisp-2];
int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind] + std::abs(p - n);
int p = sad[mind+1], n = sad[mind-1];
d = p + n - 2*sad[mind] + std::abs(p - n);
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
costptr[y*coststep] = sad[mind];
}
@@ -681,16 +684,16 @@ struct PrefilterInvoker
state = _state;
}
void operator()( int ind ) const
void operator()( int ind ) const
{
if( state->preFilterType == CV_STEREO_BM_NORMALIZED_RESPONSE )
prefilterNorm( *imgs0[ind], *imgs[ind], state->preFilterSize, state->preFilterCap, buf[ind] );
else
prefilterXSobel( *imgs0[ind], *imgs[ind], state->preFilterCap );
prefilterXSobel( *imgs0[ind], *imgs[ind], state->preFilterCap );
}
const Mat* imgs0[2];
Mat* imgs[2];
Mat* imgs[2];
uchar* buf[2];
CvStereoBMState *state;
};
@@ -709,21 +712,21 @@ struct FindStereoCorrespInvoker
useShorts = _useShorts;
validDisparityRect = _validDisparityRect;
}
void operator()( const BlockedRange& range ) const
void operator()( const BlockedRange& range ) const
{
int cols = left->cols, rows = left->rows;
int _row0 = min(cvRound(range.begin() * rows / nstripes), rows);
int _row1 = min(cvRound(range.end() * rows / nstripes), rows);
uchar *ptr = state->slidingSumBuf->data.ptr + range.begin() * stripeBufSize;
int FILTERED = (state->minDisparity - 1)*16;
Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0);
if( roi.height == 0 )
return;
int row0 = roi.y;
int row1 = roi.y + roi.height;
Mat part;
if( row0 > _row0 )
{
@@ -735,22 +738,22 @@ struct FindStereoCorrespInvoker
part = disp->rowRange(row1, _row1);
part = Scalar::all(FILTERED);
}
Mat left_i = left->rowRange(row0, row1);
Mat right_i = right->rowRange(row0, row1);
Mat disp_i = disp->rowRange(row0, row1);
Mat cost_i = state->disp12MaxDiff >= 0 ? Mat(state->cost).rowRange(row0, row1) : Mat();
#if CV_SSE2
#if CV_SSE2
if( useShorts )
findStereoCorrespondenceBM_SSE2( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
else
#endif
#endif
findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
if( state->disp12MaxDiff >= 0 )
validateDisparity( disp_i, cost_i, state->minDisparity, state->numberOfDisparities, state->disp12MaxDiff );
if( roi.x > 0 )
{
part = disp_i.colRange(0, roi.x);
@@ -767,7 +770,7 @@ protected:
const Mat *left, *right;
Mat* disp;
CvStereoBMState *state;
int nstripes;
int stripeBufSize;
bool useShorts;
@@ -775,7 +778,7 @@ protected:
};
static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat& disp0, CvStereoBMState* state)
{
{
if (left0.size() != right0.size() || disp0.size() != left0.size())
CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" );
@@ -783,7 +786,7 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" );
if (disp0.type() != CV_16SC1 && disp0.type() != CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
if( !state )
CV_Error( CV_StsNullPtr, "Stereo BM state is NULL." );
@@ -809,7 +812,7 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
if( state->uniquenessRatio < 0 )
CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
if( !state->preFilteredImg0 || state->preFilteredImg0->cols * state->preFilteredImg0->rows < left0.cols * left0.rows )
{
cvReleaseMat( &state->preFilteredImg0 );
@@ -822,7 +825,7 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
}
Mat left(left0.size(), CV_8U, state->preFilteredImg0->data.ptr);
Mat right(right0.size(), CV_8U, state->preFilteredImg1->data.ptr);
int mindisp = state->minDisparity;
int ndisp = state->numberOfDisparities;
@@ -832,15 +835,15 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
int rofs = -min(ndisp - 1 + mindisp, 0);
int width1 = width - rofs - ndisp + 1;
int FILTERED = (state->minDisparity - 1) << DISPARITY_SHIFT;
if( lofs >= width || rofs >= width || width1 < 1 )
{
disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
return;
}
Mat disp = disp0;
if( disp0.type() == CV_32F)
{
if( !state->disp || state->disp->rows != disp0.rows || state->disp->cols != disp0.cols )
@@ -850,8 +853,8 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
}
disp = cv::cvarrToMat(state->disp);
}
int wsz = state->SADWindowSize;
int wsz = state->SADWindowSize;
int bufSize0 = (int)((ndisp + 2)*sizeof(int));
bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int));
bufSize0 += (int)((height + wsz + 2)*sizeof(int));
@@ -861,16 +864,16 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
int bufSize2 = 0;
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 )
bufSize2 = width*height*(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar));
#if CV_SSE2
bool useShorts = state->preFilterCap <= 31 && state->SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
#else
const bool useShorts = false;
#endif
#ifdef HAVE_TBB
#ifdef HAVE_TBB
const double SAD_overhead_coeff = 10.0;
double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
double maxStripeSize = min(max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height);
int nstripes = cvCeil(height / maxStripeSize);
#else
@@ -878,27 +881,27 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
#endif
int bufSize = max(bufSize0 * nstripes, max(bufSize1 * 2, bufSize2));
if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize )
{
cvReleaseMat( &state->slidingSumBuf );
state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U );
}
uchar *_buf = state->slidingSumBuf->data.ptr;
int idx[] = {0,1};
parallel_do(idx, idx+2, PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, state));
Rect validDisparityRect(0, 0, width, height), R1 = state->roi1, R2 = state->roi2;
validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
state->minDisparity, state->numberOfDisparities,
state->SADWindowSize);
state->SADWindowSize);
parallel_for(BlockedRange(0, nstripes),
FindStereoCorrespInvoker(left, right, disp, state, nstripes,
bufSize0, useShorts, validDisparityRect));
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 )
{
Mat buf(state->slidingSumBuf);
@@ -906,7 +909,7 @@ static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat
}
if (disp0.data != disp.data)
disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0);
disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0);
}
StereoBM::StereoBM()
@@ -928,13 +931,13 @@ void StereoBM::operator()( InputArray _left, InputArray _right,
CV_Assert( disptype == CV_16S || disptype == CV_32F );
_disparity.create(left.size(), disptype);
Mat disparity = _disparity.getMat();
findStereoCorrespondenceBM(left, right, disparity, state);
}
template<> void Ptr<CvStereoBMState>::delete_obj()
{ cvReleaseStereoBMState(&obj); }
}
CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,
@@ -942,7 +945,7 @@ CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* ri
{
cv::Mat left = cv::cvarrToMat(leftarr),
right = cv::cvarrToMat(rightarr),
disp = cv::cvarrToMat(disparr);
disp = cv::cvarrToMat(disparr);
cv::findStereoCorrespondenceBM(left, right, disp, state);
}

336
modules/calib3d/src/stereosgbm.cpp
View File

@@ -44,17 +44,17 @@
This is a variation of
"Stereo Processing by Semiglobal Matching and Mutual Information"
by Heiko Hirschmuller.
We match blocks rather than individual pixels, thus the algorithm is called
SGBM (Semi-global block matching)
*/
*/
#include "precomp.hpp"
#include <limits.h>
namespace cv
{
typedef uchar PixType;
typedef short CostType;
typedef short DispType;
@@ -105,7 +105,7 @@ StereoSGBM::~StereoSGBM()
row1[x] and row2[x-d]. The subpixel algorithm from
"Depth Discontinuities by Pixel-to-Pixel Stereo" by Stan Birchfield and C. Tomasi
is used, hence the suffix BT.
the temporary buffer should contain width2*2 elements
*/
static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
@@ -119,25 +119,25 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
int D = maxD - minD, width1 = maxX1 - minX1, width2 = maxX2 - minX2;
const PixType *row1 = img1.ptr<PixType>(y), *row2 = img2.ptr<PixType>(y);
PixType *prow1 = buffer + width2*2, *prow2 = prow1 + width*cn*2;
tab += tabOfs;
for( c = 0; c < cn*2; c++ )
{
prow1[width*c] = prow1[width*c + width-1] =
prow1[width*c] = prow1[width*c + width-1] =
prow2[width*c] = prow2[width*c + width-1] = tab[0];
}
int n1 = y > 0 ? -(int)img1.step : 0, s1 = y < img1.rows-1 ? (int)img1.step : 0;
int n2 = y > 0 ? -(int)img2.step : 0, s2 = y < img2.rows-1 ? (int)img2.step : 0;
if( cn == 1 )
{
for( x = 1; x < width-1; x++ )
{
prow1[x] = tab[(row1[x+1] - row1[x-1])*2 + row1[x+n1+1] - row1[x+n1-1] + row1[x+s1+1] - row1[x+s1-1]];
prow2[width-1-x] = tab[(row2[x+1] - row2[x-1])*2 + row2[x+n2+1] - row2[x+n2-1] + row2[x+s2+1] - row2[x+s2-1]];
prow1[x+width] = row1[x];
prow2[width-1-x+width] = row2[x];
}
@@ -149,35 +149,35 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
prow1[x] = tab[(row1[x*3+3] - row1[x*3-3])*2 + row1[x*3+n1+3] - row1[x*3+n1-3] + row1[x*3+s1+3] - row1[x*3+s1-3]];
prow1[x+width] = tab[(row1[x*3+4] - row1[x*3-2])*2 + row1[x*3+n1+4] - row1[x*3+n1-2] + row1[x*3+s1+4] - row1[x*3+s1-2]];
prow1[x+width*2] = tab[(row1[x*3+5] - row1[x*3-1])*2 + row1[x*3+n1+5] - row1[x*3+n1-1] + row1[x*3+s1+5] - row1[x*3+s1-1]];
prow2[width-1-x] = tab[(row2[x*3+3] - row2[x*3-3])*2 + row2[x*3+n2+3] - row2[x*3+n2-3] + row2[x*3+s2+3] - row2[x*3+s2-3]];
prow2[width-1-x+width] = tab[(row2[x*3+4] - row2[x*3-2])*2 + row2[x*3+n2+4] - row2[x*3+n2-2] + row2[x*3+s2+4] - row2[x*3+s2-2]];
prow2[width-1-x+width*2] = tab[(row2[x*3+5] - row2[x*3-1])*2 + row2[x*3+n2+5] - row2[x*3+n2-1] + row2[x*3+s2+5] - row2[x*3+s2-1]];
prow1[x+width*3] = row1[x*3];
prow1[x+width*4] = row1[x*3+1];
prow1[x+width*5] = row1[x*3+2];
prow2[width-1-x+width*3] = row2[x*3];
prow2[width-1-x+width*4] = row2[x*3+1];
prow2[width-1-x+width*5] = row2[x*3+2];
}
}
memset( cost, 0, width1*D*sizeof(cost[0]) );
buffer -= minX2;
cost -= minX1*D + minD; // simplify the cost indices inside the loop
#if CV_SSE2
#if CV_SSE2
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif
#if 1
#if 1
for( c = 0; c < cn*2; c++, prow1 += width, prow2 += width )
{
int diff_scale = c < cn ? 0 : 2;
// precompute
// v0 = min(row2[x-1/2], row2[x], row2[x+1/2]) and
// v1 = max(row2[x-1/2], row2[x], row2[x+1/2]) and
@@ -191,7 +191,7 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
buffer[x] = (PixType)v0;
buffer[x + width2] = (PixType)v1;
}
for( x = minX1; x < maxX1; x++ )
{
int u = prow1[x];
@@ -199,14 +199,14 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
int ur = x < width-1 ? (u + prow1[x+1])/2 : u;
int u0 = min(ul, ur); u0 = min(u0, u);
int u1 = max(ul, ur); u1 = max(u1, u);
#if CV_SSE2
if( useSIMD )
{
__m128i _u = _mm_set1_epi8((char)u), _u0 = _mm_set1_epi8((char)u0);
__m128i _u1 = _mm_set1_epi8((char)u1), z = _mm_setzero_si128();
__m128i ds = _mm_cvtsi32_si128(diff_scale);
for( int d = minD; d < maxD; d += 16 )
{
__m128i _v = _mm_loadu_si128((const __m128i*)(prow2 + width-x-1 + d));
@@ -215,10 +215,10 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
__m128i c0 = _mm_max_epu8(_mm_subs_epu8(_u, _v1), _mm_subs_epu8(_v0, _u));
__m128i c1 = _mm_max_epu8(_mm_subs_epu8(_v, _u1), _mm_subs_epu8(_u0, _v));
__m128i diff = _mm_min_epu8(c0, c1);
c0 = _mm_load_si128((__m128i*)(cost + x*D + d));
c1 = _mm_load_si128((__m128i*)(cost + x*D + d + 8));
_mm_store_si128((__m128i*)(cost + x*D + d), _mm_adds_epi16(c0, _mm_srl_epi16(_mm_unpacklo_epi8(diff,z), ds)));
_mm_store_si128((__m128i*)(cost + x*D + d + 8), _mm_adds_epi16(c1, _mm_srl_epi16(_mm_unpackhi_epi8(diff,z), ds)));
}
@@ -233,7 +233,7 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
int v1 = buffer[width-x-1 + d + width2];
int c0 = max(0, u - v1); c0 = max(c0, v0 - u);
int c1 = max(0, v - u1); c1 = max(c1, u0 - v);
cost[x*D + d] = (CostType)(cost[x*D+d] + (min(c0, c1) >> diff_scale));
}
}
@@ -249,14 +249,14 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
if( useSIMD )
{
__m128i _u = _mm_set1_epi8(u), z = _mm_setzero_si128();
for( int d = minD; d < maxD; d += 16 )
{
__m128i _v = _mm_loadu_si128((const __m128i*)(prow2 + width-1-x + d));
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(_u,_v), _mm_subs_epu8(_v,_u));
__m128i c0 = _mm_load_si128((__m128i*)(cost + x*D + d));
__m128i c1 = _mm_load_si128((__m128i*)(cost + x*D + d + 8));
_mm_store_si128((__m128i*)(cost + x*D + d), _mm_adds_epi16(c0, _mm_unpacklo_epi8(diff,z)));
_mm_store_si128((__m128i*)(cost + x*D + d + 8), _mm_adds_epi16(c1, _mm_unpackhi_epi8(diff,z)));
}
@@ -282,22 +282,22 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
minD <= d < maxD.
disp2full is the reverse disparity map, that is:
disp2full(x+roi.x,y+roi.y)=d means that img2(x+roi.x, y+roi.y) ~ img1(x+roi.x+d, y+roi.y)
note that disp1buf will have the same size as the roi and
disp2full will have the same size as img1 (or img2).
On exit disp2buf is not the final disparity, it is an intermediate result that becomes
final after all the tiles are processed.
the disparity in disp1buf is written with sub-pixel accuracy
(4 fractional bits, see CvStereoSGBM::DISP_SCALE),
using quadratic interpolation, while the disparity in disp2buf
is written as is, without interpolation.
disp2cost also has the same size as img1 (or img2).
It contains the minimum current cost, used to find the best disparity, corresponding to the minimal cost.
*/
*/
static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
Mat& disp1, const StereoSGBM& params,
Mat& disp1, const StereoSGBM& params,
Mat& buffer )
{
#if CV_SSE2
@@ -312,15 +312,15 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
};
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif
#endif
const int ALIGN = 16;
const int DISP_SHIFT = StereoSGBM::DISP_SHIFT;
const int DISP_SCALE = StereoSGBM::DISP_SCALE;
const CostType MAX_COST = SHRT_MAX;
int minD = params.minDisparity, maxD = minD + params.numberOfDisparities;
Size SADWindowSize;
SADWindowSize.width = SADWindowSize.height = params.SADWindowSize > 0 ? params.SADWindowSize : 5;
@@ -336,28 +336,28 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
int npasses = params.fullDP ? 2 : 1;
const int TAB_OFS = 256*4, TAB_SIZE = 256 + TAB_OFS*2;
PixType clipTab[TAB_SIZE];
for( k = 0; k < TAB_SIZE; k++ )
clipTab[k] = (PixType)(min(max(k - TAB_OFS, -ftzero), ftzero) + ftzero);
if( minX1 >= maxX1 )
{
disp1 = Scalar::all(INVALID_DISP_SCALED);
return;
}
CV_Assert( D % 16 == 0 );
// NR - the number of directions. the loop on x below that computes Lr assumes that NR == 8.
// if you change NR, please, modify the loop as well.
int D2 = D+16, NRD2 = NR2*D2;
// the number of L_r(.,.) and min_k L_r(.,.) lines in the buffer:
// for 8-way dynamic programming we need the current row and
// the previous row, i.e. 2 rows in total
const int NLR = 2;
const int LrBorder = NLR - 1;
// for each possible stereo match (img1(x,y) <=> img2(x-d,y))
// we keep pixel difference cost (C) and the summary cost over NR directions (S).
// we also keep all the partial costs for the previous line L_r(x,d) and also min_k L_r(x, k)
@@ -370,29 +370,29 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
CSBufSize*2*sizeof(CostType) + // C, S
width*16*img1.channels()*sizeof(PixType) + // temp buffer for computing per-pixel cost
width*(sizeof(CostType) + sizeof(DispType)) + 1024; // disp2cost + disp2
if( !buffer.data || !buffer.isContinuous() ||
buffer.cols*buffer.rows*buffer.elemSize() < totalBufSize )
buffer.create(1, (int)totalBufSize, CV_8U);
// summary cost over different (nDirs) directions
CostType* Cbuf = (CostType*)alignPtr(buffer.data, ALIGN);
CostType* Sbuf = Cbuf + CSBufSize;
CostType* hsumBuf = Sbuf + CSBufSize;
CostType* pixDiff = hsumBuf + costBufSize*hsumBufNRows;
CostType* disp2cost = pixDiff + costBufSize + (LrSize + minLrSize)*NLR;
DispType* disp2ptr = (DispType*)(disp2cost + width);
PixType* tempBuf = (PixType*)(disp2ptr + width);
// add P2 to every C(x,y). it saves a few operations in the inner loops
for( k = 0; k < width1*D; k++ )
Cbuf[k] = (CostType)P2;
for( int pass = 1; pass <= npasses; pass++ )
{
int x1, y1, x2, y2, dx, dy;
if( pass == 1 )
{
y1 = 0; y2 = height; dy = 1;
@@ -403,9 +403,9 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
y1 = height-1; y2 = -1; dy = -1;
x1 = width1-1; x2 = -1; dx = -1;
}
CostType *Lr[NLR]={0}, *minLr[NLR]={0};
for( k = 0; k < NLR; k++ )
{
// shift Lr[k] and minLr[k] pointers, because we allocated them with the borders,
@@ -418,26 +418,26 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
minLr[k] = pixDiff + costBufSize + LrSize*NLR + minLrSize*k + NR2*2;
memset( minLr[k] - LrBorder*NR2, 0, minLrSize*sizeof(CostType) );
}
for( int y = y1; y != y2; y += dy )
{
int x, d;
DispType* disp1ptr = disp1.ptr<DispType>(y);
CostType* C = Cbuf + (!params.fullDP ? 0 : y*costBufSize);
CostType* S = Sbuf + (!params.fullDP ? 0 : y*costBufSize);
if( pass == 1 ) // compute C on the first pass, and reuse it on the second pass, if any.
{
int dy1 = y == 0 ? 0 : y + SH2, dy2 = y == 0 ? SH2 : dy1;
for( k = dy1; k <= dy2; k++ )
{
CostType* hsumAdd = hsumBuf + (min(k, height-1) % hsumBufNRows)*costBufSize;
if( k < height )
{
calcPixelCostBT( img1, img2, k, minD, maxD, pixDiff, tempBuf, clipTab, TAB_OFS, ftzero );
memset(hsumAdd, 0, D*sizeof(CostType));
for( x = 0; x <= SW2*D; x += D )
{
@@ -445,17 +445,17 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
for( d = 0; d < D; d++ )
hsumAdd[d] = (CostType)(hsumAdd[d] + pixDiff[x + d]*scale);
}
if( y > 0 )
{
const CostType* hsumSub = hsumBuf + (max(y - SH2 - 1, 0) % hsumBufNRows)*costBufSize;
const CostType* Cprev = !params.fullDP || y == 0 ? C : C - costBufSize;
for( x = D; x < width1*D; x += D )
{
const CostType* pixAdd = pixDiff + min(x + SW2*D, (width1-1)*D);
const CostType* pixSub = pixDiff + max(x - (SW2+1)*D, 0);
#if CV_SSE2
if( useSIMD )
{
@@ -490,13 +490,13 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
{
const CostType* pixAdd = pixDiff + min(x + SW2*D, (width1-1)*D);
const CostType* pixSub = pixDiff + max(x - (SW2+1)*D, 0);
for( d = 0; d < D; d++ )
hsumAdd[x + d] = (CostType)(hsumAdd[x - D + d] + pixAdd[d] - pixSub[d]);
}
}
}
if( y == 0 )
{
int scale = k == 0 ? SH2 + 1 : 1;
@@ -504,18 +504,18 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
C[x] = (CostType)(C[x] + hsumAdd[x]*scale);
}
}
// also, clear the S buffer
for( k = 0; k < width1*D; k++ )
S[k] = 0;
}
// clear the left and the right borders
memset( Lr[0] - NRD2*LrBorder - 8, 0, NRD2*LrBorder*sizeof(CostType) );
memset( Lr[0] + width1*NRD2 - 8, 0, NRD2*LrBorder*sizeof(CostType) );
memset( minLr[0] - NR2*LrBorder, 0, NR2*LrBorder*sizeof(CostType) );
memset( minLr[0] + width1*NR2, 0, NR2*LrBorder*sizeof(CostType) );
/*
[formula 13 in the paper]
compute L_r(p, d) = C(p, d) +
@@ -537,87 +537,87 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
for( x = x1; x != x2; x += dx )
{
int xm = x*NR2, xd = xm*D2;
int delta0 = minLr[0][xm - dx*NR2] + P2, delta1 = minLr[1][xm - NR2 + 1] + P2;
int delta2 = minLr[1][xm + 2] + P2, delta3 = minLr[1][xm + NR2 + 3] + P2;
CostType* Lr_p0 = Lr[0] + xd - dx*NRD2;
CostType* Lr_p1 = Lr[1] + xd - NRD2 + D2;
CostType* Lr_p2 = Lr[1] + xd + D2*2;
CostType* Lr_p3 = Lr[1] + xd + NRD2 + D2*3;
Lr_p0[-1] = Lr_p0[D] = Lr_p1[-1] = Lr_p1[D] =
Lr_p2[-1] = Lr_p2[D] = Lr_p3[-1] = Lr_p3[D] = MAX_COST;
CostType* Lr_p = Lr[0] + xd;
const CostType* Cp = C + x*D;
CostType* Sp = S + x*D;
#if CV_SSE2
if( useSIMD )
{
__m128i _P1 = _mm_set1_epi16((short)P1);
__m128i _delta0 = _mm_set1_epi16((short)delta0);
__m128i _delta1 = _mm_set1_epi16((short)delta1);
__m128i _delta2 = _mm_set1_epi16((short)delta2);
__m128i _delta3 = _mm_set1_epi16((short)delta3);
__m128i _minL0 = _mm_set1_epi16((short)MAX_COST);
for( d = 0; d < D; d += 8 )
{
__m128i Cpd = _mm_load_si128((const __m128i*)(Cp + d));
__m128i L0, L1, L2, L3;
L0 = _mm_load_si128((const __m128i*)(Lr_p0 + d));
L1 = _mm_load_si128((const __m128i*)(Lr_p1 + d));
L2 = _mm_load_si128((const __m128i*)(Lr_p2 + d));
L3 = _mm_load_si128((const __m128i*)(Lr_p3 + d));
L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d - 1)), _P1));
L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d + 1)), _P1));
L1 = _mm_min_epi16(L1, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p1 + d - 1)), _P1));
L1 = _mm_min_epi16(L1, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p1 + d + 1)), _P1));
L2 = _mm_min_epi16(L2, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p2 + d - 1)), _P1));
L2 = _mm_min_epi16(L2, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p2 + d + 1)), _P1));
L3 = _mm_min_epi16(L3, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p3 + d - 1)), _P1));
L3 = _mm_min_epi16(L3, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p3 + d + 1)), _P1));
L0 = _mm_min_epi16(L0, _delta0);
L0 = _mm_adds_epi16(_mm_subs_epi16(L0, _delta0), Cpd);
L1 = _mm_min_epi16(L1, _delta1);
L1 = _mm_adds_epi16(_mm_subs_epi16(L1, _delta1), Cpd);
L2 = _mm_min_epi16(L2, _delta2);
L2 = _mm_adds_epi16(_mm_subs_epi16(L2, _delta2), Cpd);
L3 = _mm_min_epi16(L3, _delta3);
L3 = _mm_adds_epi16(_mm_subs_epi16(L3, _delta3), Cpd);
_mm_store_si128( (__m128i*)(Lr_p + d), L0);
_mm_store_si128( (__m128i*)(Lr_p + d + D2), L1);
_mm_store_si128( (__m128i*)(Lr_p + d + D2*2), L2);
_mm_store_si128( (__m128i*)(Lr_p + d + D2*3), L3);
__m128i t0 = _mm_min_epi16(_mm_unpacklo_epi16(L0, L2), _mm_unpackhi_epi16(L0, L2));
__m128i t1 = _mm_min_epi16(_mm_unpacklo_epi16(L1, L3), _mm_unpackhi_epi16(L1, L3));
t0 = _mm_min_epi16(_mm_unpacklo_epi16(t0, t1), _mm_unpackhi_epi16(t0, t1));
_minL0 = _mm_min_epi16(_minL0, t0);
__m128i Sval = _mm_load_si128((const __m128i*)(Sp + d));
L0 = _mm_adds_epi16(L0, L1);
L2 = _mm_adds_epi16(L2, L3);
Sval = _mm_adds_epi16(Sval, L0);
Sval = _mm_adds_epi16(Sval, L2);
_mm_store_si128((__m128i*)(Sp + d), Sval);
}
_minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 8));
_mm_storel_epi64((__m128i*)&minLr[0][xm], _minL0);
}
@@ -625,28 +625,28 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
#endif
{
int minL0 = MAX_COST, minL1 = MAX_COST, minL2 = MAX_COST, minL3 = MAX_COST;
for( d = 0; d < D; d++ )
{
int Cpd = Cp[d], L0, L1, L2, L3;
L0 = Cpd + min((int)Lr_p0[d], min(Lr_p0[d-1] + P1, min(Lr_p0[d+1] + P1, delta0))) - delta0;
L1 = Cpd + min((int)Lr_p1[d], min(Lr_p1[d-1] + P1, min(Lr_p1[d+1] + P1, delta1))) - delta1;
L1 = Cpd + min((int)Lr_p1[d], min(Lr_p1[d-1] + P1, min(Lr_p1[d+1] + P1, delta1))) - delta1;
L2 = Cpd + min((int)Lr_p2[d], min(Lr_p2[d-1] + P1, min(Lr_p2[d+1] + P1, delta2))) - delta2;
L3 = Cpd + min((int)Lr_p3[d], min(Lr_p3[d-1] + P1, min(Lr_p3[d+1] + P1, delta3))) - delta3;
Lr_p[d] = (CostType)L0;
minL0 = min(minL0, L0);
Lr_p[d + D2] = (CostType)L1;
minL1 = min(minL1, L1);
Lr_p[d + D2*2] = (CostType)L2;
minL2 = min(minL2, L2);
Lr_p[d + D2*3] = (CostType)L3;
minL3 = min(minL3, L3);
Sp[d] = saturate_cast<CostType>(Sp[d] + L0 + L1 + L2 + L3);
}
minLr[0][xm] = (CostType)minL0;
@@ -655,7 +655,7 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
minLr[0][xm+3] = (CostType)minL3;
}
}
if( pass == npasses )
{
for( x = 0; x < width; x++ )
@@ -663,73 +663,73 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
disp1ptr[x] = disp2ptr[x] = (DispType)INVALID_DISP_SCALED;
disp2cost[x] = MAX_COST;
}
for( x = width1 - 1; x >= 0; x-- )
{
CostType* Sp = S + x*D;
int minS = MAX_COST, bestDisp = -1;
if( npasses == 1 )
{
int xm = x*NR2, xd = xm*D2;
int minL0 = MAX_COST;
int delta0 = minLr[0][xm + NR2] + P2;
CostType* Lr_p0 = Lr[0] + xd + NRD2;
Lr_p0[-1] = Lr_p0[D] = MAX_COST;
CostType* Lr_p = Lr[0] + xd;
const CostType* Cp = C + x*D;
#if CV_SSE2
if( useSIMD )
{
__m128i _P1 = _mm_set1_epi16((short)P1);
__m128i _delta0 = _mm_set1_epi16((short)delta0);
__m128i _minL0 = _mm_set1_epi16((short)minL0);
__m128i _minS = _mm_set1_epi16(MAX_COST), _bestDisp = _mm_set1_epi16(-1);
__m128i _d8 = _mm_setr_epi16(0, 1, 2, 3, 4, 5, 6, 7), _8 = _mm_set1_epi16(8);
for( d = 0; d < D; d += 8 )
{
__m128i Cpd = _mm_load_si128((const __m128i*)(Cp + d)), L0;
L0 = _mm_load_si128((const __m128i*)(Lr_p0 + d));
L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d - 1)), _P1));
L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d + 1)), _P1));
L0 = _mm_min_epi16(L0, _delta0);
L0 = _mm_adds_epi16(_mm_subs_epi16(L0, _delta0), Cpd);
_mm_store_si128((__m128i*)(Lr_p + d), L0);
_minL0 = _mm_min_epi16(_minL0, L0);
L0 = _mm_adds_epi16(L0, *(__m128i*)(Sp + d));
_mm_store_si128((__m128i*)(Sp + d), L0);
__m128i mask = _mm_cmpgt_epi16(_minS, L0);
_minS = _mm_min_epi16(_minS, L0);
_bestDisp = _mm_xor_si128(_bestDisp, _mm_and_si128(_mm_xor_si128(_bestDisp,_d8), mask));
_d8 = _mm_adds_epi16(_d8, _8);
}
short CV_DECL_ALIGNED(16) bestDispBuf[8];
_mm_store_si128((__m128i*)bestDispBuf, _bestDisp);
_minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 8));
_minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 4));
_minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 2));
__m128i qS = _mm_min_epi16(_minS, _mm_srli_si128(_minS, 8));
qS = _mm_min_epi16(qS, _mm_srli_si128(qS, 4));
qS = _mm_min_epi16(qS, _mm_srli_si128(qS, 2));
minLr[0][xm] = (CostType)_mm_cvtsi128_si32(_minL0);
minS = (CostType)_mm_cvtsi128_si32(qS);
qS = _mm_shuffle_epi32(_mm_unpacklo_epi16(qS, qS), 0);
qS = _mm_cmpeq_epi16(_minS, qS);
int idx = _mm_movemask_epi8(_mm_packs_epi16(qS, qS)) & 255;
bestDisp = bestDispBuf[LSBTab[idx]];
}
else
@@ -738,10 +738,10 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
for( d = 0; d < D; d++ )
{
int L0 = Cp[d] + min((int)Lr_p0[d], min(Lr_p0[d-1] + P1, min(Lr_p0[d+1] + P1, delta0))) - delta0;
Lr_p[d] = (CostType)L0;
minL0 = min(minL0, L0);
int Sval = Sp[d] = saturate_cast<CostType>(Sp[d] + L0);
if( Sval < minS )
{
@@ -764,7 +764,7 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
}
}
}
for( d = 0; d < D; d++ )
{
if( Sp[d]*(100 - uniquenessRatio) < minS*100 && std::abs(bestDisp - d) > 1 )
@@ -773,13 +773,13 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
if( d < D )
continue;
d = bestDisp;
int x2 = x + minX1 - d - minD;
if( disp2cost[x2] > minS )
int _x2 = x + minX1 - d - minD;
if( disp2cost[_x2] > minS )
{
disp2cost[x2] = (CostType)minS;
disp2ptr[x2] = (DispType)(d + minD);
disp2cost[_x2] = (CostType)minS;
disp2ptr[_x2] = (DispType)(d + minD);
}
if( 0 < d && d < D-1 )
{
// do subpixel quadratic interpolation:
@@ -792,24 +792,24 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
d *= DISP_SCALE;
disp1ptr[x + minX1] = (DispType)(d + minD*DISP_SCALE);
}
for( x = minX1; x < maxX1; x++ )
{
// we round the computed disparity both towards -inf and +inf and check
// if either of the corresponding disparities in disp2 is consistent.
// This is to give the computed disparity a chance to look valid if it is.
int d = disp1ptr[x];
if( d == INVALID_DISP_SCALED )
int d1 = disp1ptr[x];
if( d1 == INVALID_DISP_SCALED )
continue;
int _d = d >> DISP_SHIFT;
int d_ = (d + DISP_SCALE-1) >> DISP_SHIFT;
int _d = d1 >> DISP_SHIFT;
int d_ = (d1 + DISP_SCALE-1) >> DISP_SHIFT;
int _x = x - _d, x_ = x - d_;
if( 0 <= _x && _x < width && disp2ptr[_x] >= minD && std::abs(disp2ptr[_x] - _d) > disp12MaxDiff &&
0 <= x_ && x_ < width && disp2ptr[x_] >= minD && std::abs(disp2ptr[x_] - d_) > disp12MaxDiff )
disp1ptr[x] = (DispType)INVALID_DISP_SCALED;
}
}
// now shift the cyclic buffers
std::swap( Lr[0], Lr[1] );
std::swap( minLr[0], minLr[1] );
@@ -825,13 +825,13 @@ void StereoSGBM::operator ()( InputArray _left, InputArray _right,
Mat left = _left.getMat(), right = _right.getMat();
CV_Assert( left.size() == right.size() && left.type() == right.type() &&
left.depth() == DataType<PixType>::depth );
_disp.create( left.size(), CV_16S );
Mat disp = _disp.getMat();
computeDisparitySGBM( left, right, disp, *this, buffer );
medianBlur(disp, disp, 3);
if( speckleWindowSize > 0 )
filterSpeckles(disp, (minDisparity - 1)*DISP_SCALE, speckleWindowSize, DISP_SCALE*speckleRange, buffer);
}
@@ -844,33 +844,33 @@ Rect getValidDisparityROI( Rect roi1, Rect roi2,
{
int SW2 = SADWindowSize/2;
int minD = minDisparity, maxD = minDisparity + numberOfDisparities - 1;
int xmin = max(roi1.x, roi2.x + maxD) + SW2;
int xmax = min(roi1.x + roi1.width, roi2.x + roi2.width - minD) - SW2;
int ymin = max(roi1.y, roi2.y) + SW2;
int ymax = min(roi1.y + roi1.height, roi2.y + roi2.height) - SW2;
Rect r(xmin, ymin, xmax - xmin, ymax - ymin);
return r.width > 0 && r.height > 0 ? r : Rect();
}
}
}
void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize,
double _maxDiff, InputOutputArray __buf )
{
Mat img = _img.getMat();
Mat temp, &_buf = __buf.needed() ? __buf.getMatRef() : temp;
CV_Assert( img.type() == CV_16SC1 );
int newVal = cvRound(_newval);
int maxDiff = cvRound(_maxDiff);
int width = img.cols, height = img.rows, npixels = width*height;
size_t bufSize = npixels*(int)(sizeof(Point2s) + sizeof(int) + sizeof(uchar));
if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize )
_buf.create(1, (int)bufSize, CV_8U);
uchar* buf = _buf.data;
int i, j, dstep = (int)(img.step/sizeof(short));
int* labels = (int*)buf;
@@ -879,33 +879,33 @@ void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSi
buf += npixels*sizeof(wbuf[0]);
uchar* rtype = (uchar*)buf;
int curlabel = 0;
// clear out label assignments
memset(labels, 0, npixels*sizeof(labels[0]));
for( i = 0; i < height; i++ )
{
short* ds = img.ptr<short>(i);
int* ls = labels + width*i;
for( j = 0; j < width; j++ )
{
if( ds[j] != newVal ) // not a bad disparity
if( ds[j] != newVal ) // not a bad disparity
{
if( ls[j] ) // has a label, check for bad label
{
if( ls[j] ) // has a label, check for bad label
{
if( rtype[ls[j]] ) // small region, zero out disparity
ds[j] = (short)newVal;
}
// no label, assign and propagate
else
{
Point2s* ws = wbuf; // initialize wavefront
Point2s p((short)j, (short)i); // current pixel
curlabel++; // next label
int count = 0; // current region size
Point2s* ws = wbuf; // initialize wavefront
Point2s p((short)j, (short)i); // current pixel
curlabel++; // next label
int count = 0; // current region size
ls[j] = curlabel;
// wavefront propagation
while( ws >= wbuf ) // wavefront not empty
{
@@ -914,50 +914,50 @@ void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSi
short* dpp = &img.at<short>(p.y, p.x);
short dp = *dpp;
int* lpp = labels + width*p.y + p.x;
if( p.x < width-1 && !lpp[+1] && dpp[+1] != newVal && std::abs(dp - dpp[+1]) <= maxDiff )
{
lpp[+1] = curlabel;
*ws++ = Point2s(p.x+1, p.y);
}
if( p.x > 0 && !lpp[-1] && dpp[-1] != newVal && std::abs(dp - dpp[-1]) <= maxDiff )
{
lpp[-1] = curlabel;
*ws++ = Point2s(p.x-1, p.y);
}
if( p.y < height-1 && !lpp[+width] && dpp[+dstep] != newVal && std::abs(dp - dpp[+dstep]) <= maxDiff )
{
lpp[+width] = curlabel;
*ws++ = Point2s(p.x, p.y+1);
}
if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff )
{
lpp[-width] = curlabel;
*ws++ = Point2s(p.x, p.y-1);
}
// pop most recent and propagate
// NB: could try least recent, maybe better convergence
p = *--ws;
}
// assign label type
if( count <= maxSpeckleSize ) // speckle region
if( count <= maxSpeckleSize ) // speckle region
{
rtype[ls[j]] = 1; // small region label
rtype[ls[j]] = 1; // small region label
ds[j] = (short)newVal;
}
else
rtype[ls[j]] = 0; // large region label
rtype[ls[j]] = 0; // large region label
}
}
}
}
}
}
void cv::validateDisparity( InputOutputArray _disp, InputArray _cost, int minDisparity,
int numberOfDisparities, int disp12MaxDiff )
{
@@ -971,32 +971,32 @@ void cv::validateDisparity( InputOutputArray _disp, InputArray _cost, int minDis
const int DISP_SHIFT = 4, DISP_SCALE = 1 << DISP_SHIFT;
int INVALID_DISP = minD - 1, INVALID_DISP_SCALED = INVALID_DISP*DISP_SCALE;
int costType = cost.type();
disp12MaxDiff *= DISP_SCALE;
CV_Assert( numberOfDisparities > 0 && disp.type() == CV_16S &&
(costType == CV_16S || costType == CV_32S) &&
disp.size() == cost.size() );
for( int y = 0; y < rows; y++ )
{
short* dptr = disp.ptr<short>(y);
for( x = 0; x < cols; x++ )
{
disp2buf[x] = INVALID_DISP_SCALED;
disp2cost[x] = INT_MAX;
}
if( costType == CV_16S )
{
const short* cptr = cost.ptr<short>(y);
for( x = minX1; x < maxX1; x++ )
{
int d = dptr[x], c = cptr[x];
int x2 = x - ((d + DISP_SCALE/2) >> DISP_SHIFT);
if( disp2cost[x2] > c )
{
disp2cost[x2] = c;
@@ -1007,12 +1007,12 @@ void cv::validateDisparity( InputOutputArray _disp, InputArray _cost, int minDis
else
{
const int* cptr = cost.ptr<int>(y);
for( x = minX1; x < maxX1; x++ )
{
int d = dptr[x], c = cptr[x];
int x2 = x - ((d + DISP_SCALE/2) >> DISP_SHIFT);
if( disp2cost[x2] < c )
{
disp2cost[x2] = c;
@@ -1020,7 +1020,7 @@ void cv::validateDisparity( InputOutputArray _disp, InputArray _cost, int minDis
}
}
}
for( x = minX1; x < maxX1; x++ )
{
// we round the computed disparity both towards -inf and +inf and check

1372
modules/calib3d/test/test_cameracalibration.cpp
View File

File diff suppressed because it is too large Load Diff

140
modules/calib3d/test/test_cameracalibration_artificial.cpp
View File

@@ -55,22 +55,22 @@ using namespace cv;
using namespace std;
//template<class T> ostream& operator<<(ostream& out, const Mat_<T>& mat)
//{
//{
// for(Mat_<T>::const_iterator pos = mat.begin(), end = mat.end(); pos != end; ++pos)
// out << *pos << " ";
// return out;
//}
//ostream& operator<<(ostream& out, const Mat& mat) { return out << Mat_<double>(mat); }
//ostream& operator<<(ostream& out, const Mat& mat) { return out << Mat_<double>(mat); }
Mat calcRvec(const vector<Point3f>& points, const Size& cornerSize)
{
{
Point3f p00 = points[0];
Point3f p10 = points[1];
Point3f p01 = points[cornerSize.width];
Point3f p01 = points[cornerSize.width];
Vec3d ex(p10.x - p00.x, p10.y - p00.y, p10.z - p00.z);
Vec3d ey(p01.x - p00.x, p01.y - p00.y, p01.z - p00.z);
Vec3d ez = ex.cross(ey);
Vec3d ey(p01.x - p00.x, p01.y - p00.y, p01.z - p00.z);
Vec3d ez = ex.cross(ey);
Mat rot(3, 3, CV_64F);
*rot.ptr<Vec3d>(0) = ex;
@@ -89,7 +89,7 @@ public:
{
}
~CV_CalibrateCameraArtificialTest() {}
protected:
protected:
int r;
const static int JUST_FIND_CORNERS = 0;
@@ -111,7 +111,7 @@ protected:
{
ts->printf( cvtest::TS::LOG, "Bad shape of camera matrix returned \n");
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
}
}
double fx_e = camMat_est.at<double>(0, 0), fy_e = camMat_est.at<double>(1, 1);
double cx_e = camMat_est.at<double>(0, 2), cy_e = camMat_est.at<double>(1, 2);
@@ -121,19 +121,19 @@ protected:
const double eps = 1e-2;
const double dlt = 1e-5;
bool fail = checkErr(fx_e, fx, eps, dlt) || checkErr(fy_e, fy, eps, dlt) ||
checkErr(cx_e, cx, eps, dlt) || checkErr(cy_e, cy, eps, dlt);
bool fail = checkErr(fx_e, fx, eps, dlt) || checkErr(fy_e, fy, eps, dlt) ||
checkErr(cx_e, cx, eps, dlt) || checkErr(cy_e, cy, eps, dlt);
if (fail)
{
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
ts->printf( cvtest::TS::LOG, "%d) Expected [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx, fy, cx, cy);
ts->printf( cvtest::TS::LOG, "%d) Estimated [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx_e, fy_e, cx_e, cy_e);
ts->printf( cvtest::TS::LOG, "%d) Estimated [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx_e, fy_e, cx_e, cy_e);
}
void compareDistCoeffs(const Mat_<double>& distCoeffs, const Mat& distCoeffs_est)
{
{
const double *dt_e = distCoeffs_est.ptr<double>();
double k1_e = dt_e[0], k2_e = dt_e[1], k3_e = dt_e[4];
@@ -143,21 +143,21 @@ protected:
double p1 = distCoeffs(0, 2), p2 = distCoeffs(0, 3);
const double eps = 5e-2;
const double dlt = 1e-3;
const double dlt = 1e-3;
const double eps_k3 = 5;
const double dlt_k3 = 1e-3;
const double dlt_k3 = 1e-3;
bool fail = checkErr(k1_e, k1, eps, dlt) || checkErr(k2_e, k2, eps, dlt) || checkErr(k3_e, k3, eps_k3, dlt_k3) ||
checkErr(p1_e, p1, eps, dlt) || checkErr(p2_e, p2, eps, dlt);
bool fail = checkErr(k1_e, k1, eps, dlt) || checkErr(k2_e, k2, eps, dlt) || checkErr(k3_e, k3, eps_k3, dlt_k3) ||
checkErr(p1_e, p1, eps, dlt) || checkErr(p2_e, p2, eps, dlt);
if (fail)
{
// commented according to vp123's recomendation. TODO - improve accuaracy
//ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); ss
}
//ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); ss
}
ts->printf( cvtest::TS::LOG, "%d) DistCoeff exp=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1, k2, p1, p2, k3);
ts->printf( cvtest::TS::LOG, "%d) DistCoeff est=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1_e, k2_e, p1_e, p2_e, k3_e);
ts->printf( cvtest::TS::LOG, "%d) DistCoeff est=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1_e, k2_e, p1_e, p2_e, k3_e);
ts->printf( cvtest::TS::LOG, "%d) AbsError = [%.5f %.5f %.5f %.5f %.5f]\n", r, fabs(k1-k1_e), fabs(k2-k2_e), fabs(p1-p1_e), fabs(p2-p2_e), fabs(k3-k3_e));
}
@@ -173,20 +173,20 @@ protected:
const Point3d& tvec = *tvecs[i].ptr<Point3d>();
const Point3d& tvec_est = *tvecs_est[i].ptr<Point3d>();
if (norm(tvec_est - tvec) > eps* (norm(tvec) + dlt))
if (norm(tvec_est - tvec) > eps* (norm(tvec) + dlt))
{
if (err_count++ < errMsgNum)
{
if (err_count == errMsgNum)
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
else
if (err_count == errMsgNum)
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
else
{
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned tvecs. Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned tvecs. Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) norm(tvec_est - tvec) = %f, norm(tvec_exp) = %f \n", r, norm(tvec_est - tvec), norm(tvec));
}
}
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
}
@@ -199,20 +199,20 @@ protected:
int err_count = 0;
const int errMsgNum = 4;
for(size_t i = 0; i < rvecs.size(); ++i)
{
{
Rodrigues(rvecs[i], rmat);
Rodrigues(rvecs_est[i], rmat_est);
Rodrigues(rvecs_est[i], rmat_est);
if (norm(rmat_est, rmat) > eps* (norm(rmat) + dlt))
{
if (err_count++ < errMsgNum)
{
if (err_count == errMsgNum)
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
ts->printf( cvtest::TS::LOG, "%d) ...\n", r);
else
{
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned rvecs (rotation matrs). Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) norm(rot_mat_est - rot_mat_exp) = %f, norm(rot_mat_exp) = %f \n", r, norm(rmat_est, rmat), norm(rmat));
ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned rvecs (rotation matrs). Index = %d\n", r, i);
ts->printf( cvtest::TS::LOG, "%d) norm(rot_mat_est - rot_mat_exp) = %f, norm(rot_mat_exp) = %f \n", r, norm(rmat_est, rmat), norm(rmat));
}
}
@@ -221,19 +221,19 @@ protected:
}
}
double reprojectErrorWithoutIntrinsics(const vector<Point3f>& cb3d, const vector<Mat>& rvecs_exp, const vector<Mat>& tvecs_exp,
double reprojectErrorWithoutIntrinsics(const vector<Point3f>& cb3d, const vector<Mat>& _rvecs_exp, const vector<Mat>& _tvecs_exp,
const vector<Mat>& rvecs_est, const vector<Mat>& tvecs_est)
{
{
const static Mat eye33 = Mat::eye(3, 3, CV_64F);
const static Mat zero15 = Mat::zeros(1, 5, CV_64F);
Mat chessboard3D(cb3d);
Mat _chessboard3D(cb3d);
vector<Point2f> uv_exp, uv_est;
double res = 0;
double res = 0;
for(size_t i = 0; i < rvecs_exp.size(); ++i)
{
projectPoints(chessboard3D, rvecs_exp[i], tvecs_exp[i], eye33, zero15, uv_exp);
projectPoints(chessboard3D, rvecs_est[i], tvecs_est[i], eye33, zero15, uv_est);
for(size_t i = 0; i < rvecs_exp.size(); ++i)
{
projectPoints(_chessboard3D, _rvecs_exp[i], _tvecs_exp[i], eye33, zero15, uv_exp);
projectPoints(_chessboard3D, rvecs_est[i], tvecs_est[i], eye33, zero15, uv_est);
for(size_t j = 0; j < cb3d.size(); ++j)
res += norm(uv_exp[i] - uv_est[i]);
}
@@ -243,7 +243,7 @@ protected:
Size2f sqSile;
vector<Point3f> chessboard3D;
vector<Mat> boards, rvecs_exp, tvecs_exp, rvecs_spnp, tvecs_spnp;
vector<Mat> boards, rvecs_exp, tvecs_exp, rvecs_spnp, tvecs_spnp;
vector< vector<Point3f> > objectPoints;
vector< vector<Point2f> > imagePoints_art;
vector< vector<Point2f> > imagePoints_findCb;
@@ -268,29 +268,29 @@ protected:
imagePoints_findCb.clear();
vector<Point2f> corners_art, corners_fcb;
for(size_t i = 0; i < brdsNum; ++i)
{
for(size_t i = 0; i < brdsNum; ++i)
{
for(;;)
{
boards[i] = cbg(bg, camMat, distCoeffs, sqSile, corners_art);
if(findChessboardCorners(boards[i], cornersSize, corners_fcb))
break;
}
if(findChessboardCorners(boards[i], cornersSize, corners_fcb))
break;
}
//cv::namedWindow("CB"); imshow("CB", boards[i]); cv::waitKey();
imagePoints_art.push_back(corners_art);
imagePoints_art.push_back(corners_art);
imagePoints_findCb.push_back(corners_fcb);
tvecs_exp[i].create(1, 3, CV_64F);
*tvecs_exp[i].ptr<Point3d>() = cbg.corners3d[0];
rvecs_exp[i] = calcRvec(cbg.corners3d, cbg.cornersSize());
rvecs_exp[i] = calcRvec(cbg.corners3d, cbg.cornersSize());
}
}
void runTest(const Size& imgSize, const Mat_<double>& camMat, const Mat_<double>& distCoeffs, size_t brdsNum, const Size& cornersSize, int flag = 0)
{
{
const TermCriteria tc(TermCriteria::EPS|TermCriteria::MAX_ITER, 30, 0.1);
vector< vector<Point2f> > imagePoints;
@@ -300,9 +300,9 @@ protected:
case JUST_FIND_CORNERS: imagePoints = imagePoints_findCb; break;
case ARTIFICIAL_CORNERS: imagePoints = imagePoints_art; break;
case USE_CORNERS_SUBPIX:
case USE_CORNERS_SUBPIX:
for(size_t i = 0; i < brdsNum; ++i)
{
{
Mat gray;
cvtColor(boards[i], gray, CV_BGR2GRAY);
vector<Point2f> tmp = imagePoints_findCb[i];
@@ -312,9 +312,9 @@ protected:
break;
case USE_4QUAD_CORNERS:
for(size_t i = 0; i < brdsNum; ++i)
{
{
Mat gray;
cvtColor(boards[i], gray, CV_BGR2GRAY);
cvtColor(boards[i], gray, CV_BGR2GRAY);
vector<Point2f> tmp = imagePoints_findCb[i];
find4QuadCornerSubpix(gray, tmp, Size(5, 5));
imagePoints.push_back(tmp);
@@ -323,7 +323,7 @@ protected:
default:
throw std::exception();
}
Mat camMat_est = Mat::eye(3, 3, CV_64F), distCoeffs_est = Mat::zeros(1, 5, CV_64F);
vector<Mat> rvecs_est, tvecs_est;
@@ -342,9 +342,9 @@ protected:
compareCameraMatrs(camMat, camMat_est);
compareDistCoeffs(distCoeffs, distCoeffs_est);
compareShiftVecs(tvecs_exp, tvecs_est);
compareRotationVecs(rvecs_exp, rvecs_est);
compareRotationVecs(rvecs_exp, rvecs_est);
double rep_errorWOI = reprojectErrorWithoutIntrinsics(chessboard3D, rvecs_exp, tvecs_exp, rvecs_est, tvecs_est);
double rep_errorWOI = reprojectErrorWithoutIntrinsics(chessboard3D, rvecs_exp, tvecs_exp, rvecs_est, tvecs_est);
rep_errorWOI /= brdsNum * cornersSize.area();
const double thres2 = 0.01;
@@ -352,8 +352,8 @@ protected:
{
ts->printf( cvtest::TS::LOG, "%d) Too big reproject error without intrinsics = %f\n", r, rep_errorWOI);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
ts->printf( cvtest::TS::LOG, "%d) Testing solvePnP...\n", r);
rvecs_spnp.resize(brdsNum);
tvecs_spnp.resize(brdsNum);
@@ -361,11 +361,11 @@ protected:
solvePnP(Mat(objectPoints[i]), Mat(imagePoints[i]), camMat, distCoeffs, rvecs_spnp[i], tvecs_spnp[i]);
compareShiftVecs(tvecs_exp, tvecs_spnp);
compareRotationVecs(rvecs_exp, rvecs_spnp);
compareRotationVecs(rvecs_exp, rvecs_spnp);
}
void run(int)
{
{
ts->set_failed_test_info(cvtest::TS::OK);
RNG& rng = theRNG();
@@ -373,11 +373,11 @@ protected:
int progress = 0;
int repeat_num = 3;
for(r = 0; r < repeat_num; ++r)
{
const int brds_num = 20;
{
const int brds_num = 20;
Mat bg(Size(640, 480), CV_8UC3);
randu(bg, Scalar::all(32), Scalar::all(255));
Mat bg(Size(640, 480), CV_8UC3);
randu(bg, Scalar::all(32), Scalar::all(255));
GaussianBlur(bg, bg, Size(5, 5), 2);
double fx = 300 + (20 * (double)rng - 10);
@@ -399,20 +399,20 @@ protected:
Mat_<double> distCoeffs(1, 5, 0.0);
distCoeffs << k1, k2, p1, p2, k3;
ChessBoardGenerator cbg(Size(9, 8));
ChessBoardGenerator cbg(Size(9, 8));
cbg.min_cos = 0.9;
cbg.cov = 0.8;
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "\n");
ts->printf( cvtest::TS::LOG, "\n");
prepareForTest(bg, camMat, distCoeffs, brds_num, cbg);
ts->printf( cvtest::TS::LOG, "artificial corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), ARTIFICIAL_CORNERS);
ts->printf( cvtest::TS::LOG, "artificial corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), ARTIFICIAL_CORNERS);
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "findChessboard corners\n");
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), JUST_FIND_CORNERS);
runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), JUST_FIND_CORNERS);
progress = update_progress(progress, r, repeat_num, 0);
ts->printf( cvtest::TS::LOG, "cornersSubPix corners\n");
@@ -424,6 +424,6 @@ protected:
progress = update_progress(progress, r, repeat_num, 0);
}
}
};
};
TEST(Calib3d_CalibrateCamera_CPP, accuracy_on_artificial_data) { CV_CalibrateCameraArtificialTest test; test.safe_run(); }

159
modules/calib3d/test/test_chesscorners.cpp
View File

@@ -54,9 +54,9 @@ void show_points( const Mat& gray, const Mat& u, const vector<Point2f>& v, Size
{
Mat rgb( gray.size(), CV_8U);
merge(vector<Mat>(3, gray), rgb);
for(size_t i = 0; i < v.size(); i++ )
circle( rgb, v[i], 3, CV_RGB(255, 0, 0), CV_FILLED);
circle( rgb, v[i], 3, CV_RGB(255, 0, 0), CV_FILLED);
if( !u.empty() )
{
@@ -67,7 +67,7 @@ void show_points( const Mat& gray, const Mat& u, const vector<Point2f>& v, Size
}
if (!v.empty())
{
Mat corners((int)v.size(), 1, CV_32FC2, (void*)&v[0]);
Mat corners((int)v.size(), 1, CV_32FC2, (void*)&v[0]);
drawChessboardCorners( rgb, pattern_size, corners, was_found );
}
//namedWindow( "test", 0 ); imshow( "test", rgb ); waitKey(0);
@@ -122,11 +122,11 @@ double calcError(const vector<Point2f>& v, const Mat& u)
//printf("\n");
err = min(err, err1);
}
#if defined(_L2_ERR)
err = sqrt(err/count_exp);
#endif //_L2_ERR
return err;
}
@@ -137,8 +137,7 @@ const double precise_success_error_level = 2;
/* ///////////////////// chess_corner_test ///////////////////////// */
void CV_ChessboardDetectorTest::run( int /*start_from */)
{
cvtest::TS& ts = *this->ts;
ts.set_failed_test_info( cvtest::TS::OK );
ts->set_failed_test_info( cvtest::TS::OK );
/*if (!checkByGenerator())
return;*/
@@ -146,23 +145,23 @@ void CV_ChessboardDetectorTest::run( int /*start_from */)
{
case CHESSBOARD:
checkByGenerator();
if (ts.get_err_code() != cvtest::TS::OK)
if (ts->get_err_code() != cvtest::TS::OK)
{
break;
}
run_batch("negative_list.dat");
if (ts.get_err_code() != cvtest::TS::OK)
if (ts->get_err_code() != cvtest::TS::OK)
{
break;
}
run_batch("chessboard_list.dat");
if (ts.get_err_code() != cvtest::TS::OK)
if (ts->get_err_code() != cvtest::TS::OK)
{
break;
}
run_batch("chessboard_list_subpixel.dat");
break;
case CIRCLES_GRID:
@@ -176,36 +175,34 @@ void CV_ChessboardDetectorTest::run( int /*start_from */)
void CV_ChessboardDetectorTest::run_batch( const string& filename )
{
cvtest::TS& ts = *this->ts;
ts.printf(cvtest::TS::LOG, "\nRunning batch %s\n", filename.c_str());
ts->printf(cvtest::TS::LOG, "\nRunning batch %s\n", filename.c_str());
//#define WRITE_POINTS 1
#ifndef WRITE_POINTS
#ifndef WRITE_POINTS
double max_rough_error = 0, max_precise_error = 0;
#endif
string folder;
switch( pattern )
{
case CHESSBOARD:
folder = string(ts.get_data_path()) + "cameracalibration/";
folder = string(ts->get_data_path()) + "cameracalibration/";
break;
case CIRCLES_GRID:
folder = string(ts.get_data_path()) + "cameracalibration/circles/";
folder = string(ts->get_data_path()) + "cameracalibration/circles/";
break;
case ASYMMETRIC_CIRCLES_GRID:
folder = string(ts.get_data_path()) + "cameracalibration/asymmetric_circles/";
folder = string(ts->get_data_path()) + "cameracalibration/asymmetric_circles/";
break;
}
FileStorage fs( folder + filename, FileStorage::READ );
FileNode board_list = fs["boards"];
if( !fs.isOpened() || board_list.empty() || !board_list.isSeq() || board_list.size() % 2 != 0 )
{
ts.printf( cvtest::TS::LOG, "%s can not be readed or is not valid\n", (folder + filename).c_str() );
ts.printf( cvtest::TS::LOG, "fs.isOpened=%d, board_list.empty=%d, board_list.isSeq=%d,board_list.size()%2=%d\n",
ts->printf( cvtest::TS::LOG, "%s can not be readed or is not valid\n", (folder + filename).c_str() );
ts->printf( cvtest::TS::LOG, "fs.isOpened=%d, board_list.empty=%d, board_list.isSeq=%d,board_list.size()%2=%d\n",
fs.isOpened(), (int)board_list.empty(), board_list.isSeq(), board_list.size()%2);
ts.set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
return;
}
@@ -216,29 +213,29 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )
for(int idx = 0; idx < max_idx; ++idx )
{
ts.update_context( this, idx, true );
ts->update_context( this, idx, true );
/* read the image */
string img_file = board_list[idx * 2];
string img_file = board_list[idx * 2];
Mat gray = imread( folder + img_file, 0);
if( gray.empty() )
{
ts.printf( cvtest::TS::LOG, "one of chessboard images can't be read: %s\n", img_file.c_str() );
ts.set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
ts->printf( cvtest::TS::LOG, "one of chessboard images can't be read: %s\n", img_file.c_str() );
ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
return;
}
string filename = folder + (string)board_list[idx * 2 + 1];
string _filename = folder + (string)board_list[idx * 2 + 1];
bool doesContatinChessboard;
Mat expected;
{
FileStorage fs(filename, FileStorage::READ);
fs["corners"] >> expected;
fs["isFound"] >> doesContatinChessboard;
fs.release();
}
size_t count_exp = static_cast<size_t>(expected.cols * expected.rows);
FileStorage fs1(_filename, FileStorage::READ);
fs1["corners"] >> expected;
fs1["isFound"] >> doesContatinChessboard;
fs1.release();
}
size_t count_exp = static_cast<size_t>(expected.cols * expected.rows);
Size pattern_size = expected.size();
vector<Point2f> v;
@@ -256,11 +253,11 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )
break;
}
show_points( gray, Mat(), v, pattern_size, result );
if( result ^ doesContatinChessboard || v.size() != count_exp )
{
ts.printf( cvtest::TS::LOG, "chessboard is detected incorrectly in %s\n", img_file.c_str() );
ts.set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
ts->printf( cvtest::TS::LOG, "chessboard is detected incorrectly in %s\n", img_file.c_str() );
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
return;
}
@@ -291,45 +288,45 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename )
#if 1
if( err > precise_success_error_level )
{
ts.printf( cvtest::TS::LOG, "Image %s: bad accuracy of adjusted corners %f\n", img_file.c_str(), err );
ts.set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
ts->printf( cvtest::TS::LOG, "Image %s: bad accuracy of adjusted corners %f\n", img_file.c_str(), err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
#endif
ts.printf(cvtest::TS::LOG, "Error on %s is %f\n", img_file.c_str(), err);
ts->printf(cvtest::TS::LOG, "Error on %s is %f\n", img_file.c_str(), err);
max_precise_error = MAX( max_precise_error, err );
#endif
#endif
}
#ifdef WRITE_POINTS
Mat mat_v(pattern_size, CV_32FC2, (void*)&v[0]);
FileStorage fs(filename, FileStorage::WRITE);
FileStorage fs(_filename, FileStorage::WRITE);
fs << "isFound" << result;
fs << "corners" << mat_v;
fs.release();
#endif
progress = update_progress( progress, idx, max_idx, 0 );
}
}
sum_error /= count;
ts.printf(cvtest::TS::LOG, "Average error is %f\n", sum_error);
ts->printf(cvtest::TS::LOG, "Average error is %f\n", sum_error);
}
double calcErrorMinError(const Size& cornSz, const vector<Point2f>& corners_found, const vector<Point2f>& corners_generated)
{
Mat m1(cornSz, CV_32FC2, (Point2f*)&corners_generated[0]);
Mat m1(cornSz, CV_32FC2, (Point2f*)&corners_generated[0]);
Mat m2; flip(m1, m2, 0);
Mat m3; flip(m1, m3, 1); m3 = m3.t(); flip(m3, m3, 1);
Mat m4 = m1.t(); flip(m4, m4, 1);
double min1 = min(calcError(corners_found, m1), calcError(corners_found, m2));
double min2 = min(calcError(corners_found, m3), calcError(corners_found, m4));
double min1 = min(calcError(corners_found, m1), calcError(corners_found, m2));
double min2 = min(calcError(corners_found, m3), calcError(corners_found, m4));
return min(min1, min2);
}
bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz,
bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz,
const vector<Point2f>& corners_generated)
{
Size cornersSize = cbg.cornersSize();
@@ -341,7 +338,7 @@ bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz,
for(int j = 1; j < mat.cols - 2; ++j)
{
const Point2f& cur = mat(i, j);
tmp = norm( cur - mat(i + 1, j + 1) );
if (tmp < minNeibDist)
tmp = minNeibDist;
@@ -361,33 +358,33 @@ bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz,
const double threshold = 0.25;
double cbsize = (max(cornersSize.width, cornersSize.height) + 1) * minNeibDist;
int imgsize = min(imgSz.height, imgSz.width);
int imgsize = min(imgSz.height, imgSz.width);
return imgsize * threshold < cbsize;
}
bool CV_ChessboardDetectorTest::checkByGenerator()
{
{
bool res = true;
//theRNG() = 0x58e6e895b9913160;
//cv::DefaultRngAuto dra;
//theRNG() = *ts->get_rng();
Mat bg(Size(800, 600), CV_8UC3, Scalar::all(255));
randu(bg, Scalar::all(0), Scalar::all(255));
GaussianBlur(bg, bg, Size(7,7), 3.0);
Mat bg(Size(800, 600), CV_8UC3, Scalar::all(255));
randu(bg, Scalar::all(0), Scalar::all(255));
GaussianBlur(bg, bg, Size(7,7), 3.0);
Mat_<float> camMat(3, 3);
camMat << 300.f, 0.f, bg.cols/2.f, 0, 300.f, bg.rows/2.f, 0.f, 0.f, 1.f;
Mat_<float> distCoeffs(1, 5);
distCoeffs << 1.2f, 0.2f, 0.f, 0.f, 0.f;
const Size sizes[] = { Size(6, 6), Size(8, 6), Size(11, 12), Size(5, 4) };
const size_t sizes_num = sizeof(sizes)/sizeof(sizes[0]);
const int test_num = 16;
const size_t sizes_num = sizeof(sizes)/sizeof(sizes[0]);
const int test_num = 16;
int progress = 0;
for(int i = 0; i < test_num; ++i)
{
{
progress = update_progress( progress, i, test_num, 0 );
ChessBoardGenerator cbg(sizes[i % sizes_num]);
@@ -398,37 +395,37 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
if(!validateData(cbg, cb.size(), corners_generated))
{
ts->printf( cvtest::TS::LOG, "Chess board skipped - too small" );
continue;
continue;
}
/*cb = cb * 0.8 + Scalar::all(30);
/*cb = cb * 0.8 + Scalar::all(30);
GaussianBlur(cb, cb, Size(3, 3), 0.8); */
//cv::addWeighted(cb, 0.8, bg, 0.2, 20, cb);
//cv::addWeighted(cb, 0.8, bg, 0.2, 20, cb);
//cv::namedWindow("CB"); cv::imshow("CB", cb); cv::waitKey();
vector<Point2f> corners_found;
int flags = i % 8; // need to check branches for all flags
bool found = findChessboardCorners(cb, cbg.cornersSize(), corners_found, flags);
if (!found)
{
if (!found)
{
ts->printf( cvtest::TS::LOG, "Chess board corners not found\n" );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
res = false;
return res;
return res;
}
double err = calcErrorMinError(cbg.cornersSize(), corners_found, corners_generated);
double err = calcErrorMinError(cbg.cornersSize(), corners_found, corners_generated);
if( err > rough_success_error_level )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of corner guesses" );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
res = false;
return res;
}
}
}
}
/* ***** negative ***** */
{
{
vector<Point2f> corners_found;
bool found = findChessboardCorners(bg, Size(8, 7), corners_found);
if (found)
@@ -437,27 +434,27 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
ChessBoardGenerator cbg(Size(8, 7));
vector<Point2f> cg;
Mat cb = cbg(bg, camMat, distCoeffs, cg);
Mat cb = cbg(bg, camMat, distCoeffs, cg);
found = findChessboardCorners(cb, Size(3, 4), corners_found);
if (found)
res = false;
res = false;
Point2f c = std::accumulate(cg.begin(), cg.end(), Point2f(), plus<Point2f>()) * (1.f/cg.size());
Mat_<double> aff(2, 3);
aff << 1.0, 0.0, -(double)c.x, 0.0, 1.0, 0.0;
Mat sh;
warpAffine(cb, sh, aff, cb.size());
warpAffine(cb, sh, aff, cb.size());
found = findChessboardCorners(sh, cbg.cornersSize(), corners_found);
if (found)
res = false;
res = false;
vector< vector<Point> > cnts(1);
vector<Point>& cnt = cnts[0];
cnt.push_back(cg[ 0]); cnt.push_back(cg[0+2]);
cnt.push_back(cg[7+0]); cnt.push_back(cg[7+2]);
cnt.push_back(cg[ 0]); cnt.push_back(cg[0+2]);
cnt.push_back(cg[7+0]); cnt.push_back(cg[7+2]);
cv::drawContours(cb, cnts, -1, Scalar::all(128), CV_FILLED);
found = findChessboardCorners(cb, cbg.cornersSize(), corners_found);
@@ -466,7 +463,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator()
cv::drawChessboardCorners(cb, cbg.cornersSize(), Mat(corners_found), found);
}
return res;
}

131
modules/calib3d/test/test_compose_rt.cpp
View File

@@ -47,87 +47,87 @@ using namespace std;
class Differential
{
public:
typedef Mat_<double> mat_t;
public:
typedef Mat_<double> mat_t;
Differential(double eps_, const mat_t& rv1_, const mat_t& tv1_, const mat_t& rv2_, const mat_t& tv2_)
Differential(double eps_, const mat_t& rv1_, const mat_t& tv1_, const mat_t& rv2_, const mat_t& tv2_)
: rv1(rv1_), tv1(tv1_), rv2(rv2_), tv2(tv2_), eps(eps_), ev(3, 1) {}
void dRv1(mat_t& dr3_dr1, mat_t& dt3_dr1)
{
{
dr3_dr1.create(3, 3); dt3_dr1.create(3, 3);
for(int i = 0; i < 3; ++i)
for(int i = 0; i < 3; ++i)
{
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1 + ev, tv1, rv2, tv2, rv3_p, tv3_p);
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1 + ev, tv1, rv2, tv2, rv3_p, tv3_p);
composeRT( rv1 - ev, tv1, rv2, tv2, rv3_m, tv3_m);
dr3_dr1.col(i) = rv3_p - rv3_m;
dt3_dr1.col(i) = tv3_p - tv3_m;
dr3_dr1.col(i) = rv3_p - rv3_m;
dt3_dr1.col(i) = tv3_p - tv3_m;
}
dr3_dr1 /= 2 * eps; dt3_dr1 /= 2 * eps;
}
void dRv2(mat_t& dr3_dr2, mat_t& dt3_dr2)
{
{
dr3_dr2.create(3, 3); dt3_dr2.create(3, 3);
for(int i = 0; i < 3; ++i)
for(int i = 0; i < 3; ++i)
{
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1, rv2 + ev, tv2, rv3_p, tv3_p);
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1, rv2 + ev, tv2, rv3_p, tv3_p);
composeRT( rv1, tv1, rv2 - ev, tv2, rv3_m, tv3_m);
dr3_dr2.col(i) = rv3_p - rv3_m;
dt3_dr2.col(i) = tv3_p - tv3_m;
dr3_dr2.col(i) = rv3_p - rv3_m;
dt3_dr2.col(i) = tv3_p - tv3_m;
}
dr3_dr2 /= 2 * eps; dt3_dr2 /= 2 * eps;
}
void dTv1(mat_t& drt3_dt1, mat_t& dt3_dt1)
{
{
drt3_dt1.create(3, 3); dt3_dt1.create(3, 3);
for(int i = 0; i < 3; ++i)
for(int i = 0; i < 3; ++i)
{
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1 + ev, rv2, tv2, rv3_p, tv3_p);
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1 + ev, rv2, tv2, rv3_p, tv3_p);
composeRT( rv1, tv1 - ev, rv2, tv2, rv3_m, tv3_m);
drt3_dt1.col(i) = rv3_p - rv3_m;
dt3_dt1.col(i) = tv3_p - tv3_m;
drt3_dt1.col(i) = rv3_p - rv3_m;
dt3_dt1.col(i) = tv3_p - tv3_m;
}
drt3_dt1 /= 2 * eps; dt3_dt1 /= 2 * eps;
}
void dTv2(mat_t& dr3_dt2, mat_t& dt3_dt2)
{
{
dr3_dt2.create(3, 3); dt3_dt2.create(3, 3);
for(int i = 0; i < 3; ++i)
for(int i = 0; i < 3; ++i)
{
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1, rv2, tv2 + ev, rv3_p, tv3_p);
ev.setTo(Scalar(0)); ev(i, 0) = eps;
composeRT( rv1, tv1, rv2, tv2 + ev, rv3_p, tv3_p);
composeRT( rv1, tv1, rv2, tv2 - ev, rv3_m, tv3_m);
dr3_dt2.col(i) = rv3_p - rv3_m;
dt3_dt2.col(i) = tv3_p - tv3_m;
dr3_dt2.col(i) = rv3_p - rv3_m;
dt3_dt2.col(i) = tv3_p - tv3_m;
}
dr3_dt2 /= 2 * eps; dt3_dt2 /= 2 * eps;
}
private:
const mat_t& rv1, tv1, rv2, tv2;
double eps;
Mat_<double> ev;
Differential& operator=(const Differential&);
Mat rv3_m, tv3_m, rv3_p, tv3_p;
Mat rv3_m, tv3_m, rv3_p, tv3_p;
};
class CV_composeRT_Test : public cvtest::BaseTest
@@ -135,24 +135,23 @@ class CV_composeRT_Test : public cvtest::BaseTest
public:
CV_composeRT_Test() {}
~CV_composeRT_Test() {}
protected:
protected:
void run(int)
{
cvtest::TS& ts = *this->ts;
ts.set_failed_test_info(cvtest::TS::OK);
Mat_<double> rvec1(3, 1), tvec1(3, 1), rvec2(3, 1), tvec2(3, 1);
ts->set_failed_test_info(cvtest::TS::OK);
Mat_<double> rvec1(3, 1), tvec1(3, 1), rvec2(3, 1), tvec2(3, 1);
randu(rvec1, Scalar(0), Scalar(6.29));
randu(rvec2, Scalar(0), Scalar(6.29));
randu(tvec1, Scalar(-2), Scalar(2));
randu(tvec2, Scalar(-2), Scalar(2));
Mat rvec3, tvec3;
composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3);
Mat rvec3_exp, tvec3_exp;
Mat rmat1, rmat2;
@@ -164,53 +163,53 @@ protected:
const double thres = 1e-5;
if (norm(rvec3_exp, rvec3) > thres || norm(tvec3_exp, tvec3) > thres)
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
const double eps = 1e-3;
Differential diff(eps, rvec1, tvec1, rvec2, tvec2);
Mat dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2;
composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3,
composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3,
dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2);
Mat_<double> dr3_dr1, dt3_dr1;
diff.dRv1(dr3_dr1, dt3_dr1);
if (norm(dr3_dr1, dr3dr1) > thres || norm(dt3_dr1, dt3dr1) > thres)
{
ts.printf( cvtest::TS::LOG, "Invalid derivates by r1\n" );
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
{
ts->printf( cvtest::TS::LOG, "Invalid derivates by r1\n" );
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
Mat_<double> dr3_dr2, dt3_dr2;
diff.dRv2(dr3_dr2, dt3_dr2);
if (norm(dr3_dr2, dr3dr2) > thres || norm(dt3_dr2, dt3dr2) > thres)
{
ts.printf( cvtest::TS::LOG, "Invalid derivates by r2\n" );
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
{
ts->printf( cvtest::TS::LOG, "Invalid derivates by r2\n" );
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
Mat_<double> dr3_dt1, dt3_dt1;
diff.dTv1(dr3_dt1, dt3_dt1);
if (norm(dr3_dt1, dr3dt1) > thres || norm(dt3_dt1, dt3dt1) > thres)
{
ts.printf( cvtest::TS::LOG, "Invalid derivates by t1\n" );
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
{
ts->printf( cvtest::TS::LOG, "Invalid derivates by t1\n" );
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
Mat_<double> dr3_dt2, dt3_dt2;
diff.dTv2(dr3_dt2, dt3_dt2);
if (norm(dr3_dt2, dr3dt2) > thres || norm(dt3_dt2, dt3dt2) > thres)
{
ts.printf( cvtest::TS::LOG, "Invalid derivates by t2\n" );
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
{
ts->printf( cvtest::TS::LOG, "Invalid derivates by t2\n" );
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
};
}
};
TEST(Calib3d_ComposeRT, accuracy) { CV_composeRT_Test test; test.safe_run(); }

54
modules/calib3d/test/test_homography.cpp
View File

@@ -86,20 +86,20 @@ protected:
double sigma;
private:
float max_diff, max_2diff;
bool check_matrix_size(const cv::Mat& H);
bool check_matrix_diff(const cv::Mat& original, const cv::Mat& found, const int norm_type, double &diff);
float max_diff, max_2diff;
bool check_matrix_size(const cv::Mat& H);
bool check_matrix_diff(const cv::Mat& original, const cv::Mat& found, const int norm_type, double &diff);
int check_ransac_mask_1(const Mat& src, const Mat& mask);
int check_ransac_mask_2(const Mat& original_mask, const Mat& found_mask);
int check_ransac_mask_2(const Mat& original_mask, const Mat& found_mask);
void print_information_1(int j, int N, int method, const Mat& H);
void print_information_2(int j, int N, int method, const Mat& H, const Mat& H_res, int k, double diff);
void print_information_3(int j, int N, const Mat& mask);
void print_information_4(int method, int j, int N, int k, int l, double diff);
void print_information_5(int method, int j, int N, int l, double diff);
void print_information_6(int j, int N, int k, double diff, bool value);
void print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value);
void print_information_8(int j, int N, int k, int l, double diff);
void print_information_1(int j, int N, int method, const Mat& H);
void print_information_2(int j, int N, int method, const Mat& H, const Mat& H_res, int k, double diff);
void print_information_3(int j, int N, const Mat& mask);
void print_information_4(int method, int j, int N, int k, int l, double diff);
void print_information_5(int method, int j, int N, int l, double diff);
void print_information_6(int j, int N, int k, double diff, bool value);
void print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value);
void print_information_8(int j, int N, int k, int l, double diff);
};
CV_HomographyTest::CV_HomographyTest() : max_diff(1e-2f), max_2diff(2e-2f)
@@ -112,7 +112,7 @@ CV_HomographyTest::CV_HomographyTest() : max_diff(1e-2f), max_2diff(2e-2f)
CV_HomographyTest::~CV_HomographyTest() {}
bool CV_HomographyTest::check_matrix_size(const cv::Mat& H)
bool CV_HomographyTest::check_matrix_size(const cv::Mat& H)
{
return (H.rows == 3) && (H.cols == 3);
}
@@ -138,25 +138,25 @@ int CV_HomographyTest::check_ransac_mask_2(const Mat& original_mask, const Mat&
return 0;
}
void CV_HomographyTest::print_information_1(int j, int N, int method, const Mat& H)
void CV_HomographyTest::print_information_1(int j, int N, int _method, const Mat& H)
{
cout << endl; cout << "Checking for homography matrix sizes..." << endl; cout << endl;
cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>";
cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
cout << "Count of points: " << N << endl; cout << endl;
cout << "Method: "; if (method == 0) cout << 0; else if (method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
cout << "Homography matrix:" << endl; cout << endl;
cout << H << endl; cout << endl;
cout << "Number of rows: " << H.rows << " Number of cols: " << H.cols << endl; cout << endl;
}
void CV_HomographyTest::print_information_2(int j, int N, int method, const Mat& H, const Mat& H_res, int k, double diff)
void CV_HomographyTest::print_information_2(int j, int N, int _method, const Mat& H, const Mat& H_res, int k, double diff)
{
cout << endl; cout << "Checking for accuracy of homography matrix computing..." << endl; cout << endl;
cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>";
cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
cout << "Count of points: " << N << endl; cout << endl;
cout << "Method: "; if (method == 0) cout << 0; else if (method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl;
cout << "Original matrix:" << endl; cout << endl;
cout << H << endl; cout << endl;
cout << "Found matrix:" << endl; cout << endl;
@@ -178,10 +178,10 @@ void CV_HomographyTest::print_information_3(int j, int N, const Mat& mask)
cout << "Number of rows: " << mask.rows << " Number of cols: " << mask.cols << endl; cout << endl;
}
void CV_HomographyTest::print_information_4(int method, int j, int N, int k, int l, double diff)
void CV_HomographyTest::print_information_4(int _method, int j, int N, int k, int l, double diff)
{
cout << endl; cout << "Checking for accuracy of reprojection error computing..." << endl; cout << endl;
cout << "Method: "; if (method == 0) cout << 0 << endl; else cout << "CV_LMEDS" << endl;
cout << "Method: "; if (_method == 0) cout << 0 << endl; else cout << "CV_LMEDS" << endl;
cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>";
cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
cout << "Sigma of normal noise: " << sigma << endl;
@@ -192,10 +192,10 @@ void CV_HomographyTest::print_information_4(int method, int j, int N, int k, int
cout << "Maxumum allowed difference: " << max_2diff << endl; cout << endl;
}
void CV_HomographyTest::print_information_5(int method, int j, int N, int l, double diff)
{
void CV_HomographyTest::print_information_5(int _method, int j, int N, int l, double diff)
{
cout << endl; cout << "Checking for accuracy of reprojection error computing..." << endl; cout << endl;
cout << "Method: "; if (method == 0) cout << 0 << endl; else cout << "CV_LMEDS" << endl;
cout << "Method: "; if (_method == 0) cout << 0 << endl; else cout << "CV_LMEDS" << endl;
cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>";
cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector <Point2f>"; cout << endl;
cout << "Sigma of normal noise: " << sigma << endl;
@@ -371,7 +371,7 @@ void CV_HomographyTest::run(int)
if (code)
{
print_information_3(j, N, mask[j]);
switch (code)
{
case 1: { CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_MASK, MESSAGE_RANSAC_MASK_1); break; }
@@ -380,7 +380,7 @@ void CV_HomographyTest::run(int)
default: break;
}
return;
}
@@ -412,7 +412,7 @@ void CV_HomographyTest::run(int)
{
case 0:
case CV_LMEDS:
{
{
Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f),
cv::findHomography(src_mat_2f, dst_vec),
cv::findHomography(src_vec, dst_mat_2f),
@@ -465,7 +465,7 @@ void CV_HomographyTest::run(int)
}
continue;
}
}
case CV_RANSAC:
{
cv::Mat mask_res [4];
@@ -555,7 +555,7 @@ void CV_HomographyTest::run(int)
}
}
}
continue;
}

17
modules/calib3d/test/test_solvepnp_ransac.cpp
View File

@@ -106,7 +106,7 @@ protected:
}
}
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
{
Mat rvec, tvec;
vector<int> inliers;
@@ -136,7 +136,7 @@ protected:
bool isTestSuccess = inliers.size() >= points.size()*0.95;
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method];
isTestSuccess = isTestSuccess && rvecDiff < epsilon[method] && tvecDiff < epsilon[method];
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
//cout << error << " " << inliers.size() << " " << eps[method] << endl;
if (error > maxError)
@@ -147,8 +147,7 @@ protected:
void run(int)
{
cvtest::TS& ts = *this->ts;
ts.set_failed_test_info(cvtest::TS::OK);
ts->set_failed_test_info(cvtest::TS::OK);
vector<Point3f> points;
const int pointsCount = 500;
@@ -157,7 +156,7 @@ protected:
const int methodsCount = 3;
RNG rng = ts.get_rng();
RNG rng = ts->get_rng();
for (int mode = 0; mode < 2; mode++)
@@ -174,9 +173,9 @@ protected:
//cout << maxError << " " << successfulTestsCount << endl;
if (successfulTestsCount < 0.7*totalTestsCount)
{
ts.printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
@@ -198,7 +197,7 @@ public:
~CV_solvePnP_Test() {}
protected:
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
{
Mat rvec, tvec;
Mat trueRvec, trueTvec;
@@ -226,7 +225,7 @@ protected:
false, method);
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
bool isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method];
bool isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method];
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
if (error > maxError)

8
modules/calib3d/test/test_stereomatching.cpp
View File

@@ -421,7 +421,7 @@ void CV_StereoMatchingTest::run(int)
ts->set_failed_test_info( code );
return;
}
string fullResultFilename = dataPath + ALGORITHMS_DIR + algorithmName + RESULT_FILE;
FileStorage resFS( fullResultFilename, FileStorage::READ );
bool isWrite = true; // write or compare results
@@ -593,11 +593,11 @@ int CV_StereoMatchingTest::readDatasetsParams( FileStorage& fs )
assert(fn.isSeq());
for( int i = 0; i < (int)fn.size(); i+=3 )
{
string name = fn[i];
string _name = fn[i];
DatasetParams params;
string sf = fn[i+1]; params.dispScaleFactor = atoi(sf.c_str());
string uv = fn[i+2]; params.dispUnknVal = atoi(uv.c_str());
datasetsParams[name] = params;
datasetsParams[_name] = params;
}
return cvtest::TS::OK;
}
@@ -713,7 +713,7 @@ class CV_StereoSGBMTest : public CV_StereoMatchingTest
public:
CV_StereoSGBMTest()
{
name = "stereosgbm";
name = "stereosgbm";
fill(rmsEps.begin(), rmsEps.end(), 0.25f);
fill(fracEps.begin(), fracEps.end(), 0.01f);
}