// Get an 8-bit equivalent of the 32-bit Float image.
// Returns a new image, so remember to call 'cvReleaseImage()' on the result.
IplImage* convertFloatImageToUcharImage(const IplImage *srcImg)
{
IplImage *dstImg = 0;
if ((srcImg) && (srcImg->width > 0 && srcImg->height > 0)) {
// Spread the 32bit floating point pixels to fit within 8bit pixel range.
double minVal, maxVal;
cvMinMaxLoc(srcImg, &minVal, &maxVal);
//cout << "FloatImage:(minV=" << minVal << ", maxV=" << maxVal << ")." << endl;
// Deal with NaN and extreme values, since the DFT seems to give some NaN results.
if (cvIsNaN(minVal) || minVal < -1e30)
minVal = -1e30;
if (cvIsNaN(maxVal) || maxVal > 1e30)
maxVal = 1e30;
if (maxVal-minVal == 0.0f)
maxVal = minVal + 0.001; // remove potential divide by zero errors.
// Convert the format
dstImg = cvCreateImage(cvSize(srcImg->width, srcImg->height), 8, 1);
cvConvertScale(srcImg, dstImg, 255.0 / (maxVal - minVal), - minVal * 255.0 / (maxVal-minVal));
}
return dstImg;
}
int CV_QueryHistTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
int i, j, iters = values->cols;
for( i = 0; i < iters; i++ )
{
float v = values->data.fl[i], v0 = values0->data.fl[i];
if( cvIsNaN(v) || cvIsInf(v) )
{
ts->printf( CvTS::LOG, "The bin #%d has invalid value\n", i );
code = CvTS::FAIL_INVALID_OUTPUT;
}
else if( fabs(v - v0) > FLT_EPSILON )
{
ts->printf( CvTS::LOG, "The bin #%d = %g, while it should be %g\n", i, v, v0 );
code = CvTS::FAIL_BAD_ACCURACY;
}
if( code < 0 )
{
ts->printf( CvTS::LOG, "The bin index = (" );
for( j = 0; j < cdims; j++ )
ts->printf( CvTS::LOG, "%d%s", indices->data.i[i*cdims + j],
j < cdims-1 ? ", " : ")\n" );
break;
}
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
int CV_ThreshHistTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
int i;
float* ptr0 = values->data.fl;
float* ptr = 0;
CvSparseMat* sparse = 0;
if( hist_type == CV_HIST_ARRAY )
ptr = (float*)cvPtr1D( hist[0]->bins, 0 );
else
sparse = (CvSparseMat*)hist[0]->bins;
if( code > 0 )
{
for( i = 0; i < orig_nz_count; i++ )
{
float v0 = ptr0[i], v;
if( hist_type == CV_HIST_ARRAY )
v = ptr[i];
else
{
v = (float)cvGetRealND( sparse, indices->data.i + i*cdims );
cvClearND( sparse, indices->data.i + i*cdims );
}
if( v0 <= threshold ) v0 = 0.f;
if( cvIsNaN(v) || cvIsInf(v) )
{
ts->printf( CvTS::LOG, "The %d-th bin is invalid (=%g)\n", i, v );
code = CvTS::FAIL_INVALID_OUTPUT;
break;
}
else if( fabs(v0 - v) > FLT_EPSILON*10*fabs(v0) )
{
ts->printf( CvTS::LOG, "The %d-th bin is incorrect (=%g, should be =%g)\n", i, v, v0 );
code = CvTS::FAIL_BAD_ACCURACY;
break;
}
}
}
if( code > 0 && hist_type == CV_HIST_SPARSE )
{
if( sparse->heap->active_count > 0 )
{
ts->printf( CvTS::LOG,
"There some extra histogram bins in the sparse histogram after the thresholding\n" );
code = CvTS::FAIL_INVALID_OUTPUT;
}
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
int CV_MinMaxHistTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
if( cvIsNaN(min_val) || cvIsInf(min_val) ||
cvIsNaN(max_val) || cvIsInf(max_val) )
{
ts->printf( CvTS::LOG,
"The extrema histogram bin values are invalid (min = %g, max = %g)\n", min_val, max_val );
code = CvTS::FAIL_INVALID_OUTPUT;
}
else if( fabs(min_val - min_val0) > FLT_EPSILON ||
fabs(max_val - max_val0) > FLT_EPSILON )
{
ts->printf( CvTS::LOG,
"The extrema histogram bin values are incorrect: (min = %g, should be = %g), (max = %g, should be = %g)\n",
min_val, min_val0, max_val, max_val0 );
code = CvTS::FAIL_BAD_ACCURACY;
}
else
{
int i;
for( i = 0; i < cdims; i++ )
{
if( min_idx[i] != min_idx0[i] || max_idx[i] != max_idx0[i] )
{
ts->printf( CvTS::LOG,
"The %d-th coordinates of extrema histogram bin values are incorrect: "
"(min = %d, should be = %d), (max = %d, should be = %d)\n",
i, min_idx[i], min_idx0[i], max_idx[i], max_idx0[i] );
code = CvTS::FAIL_BAD_ACCURACY;
}
}
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
/*
* call-seq:
* angle -> float
*
* Return angle.
*/
VALUE
rb_angle(VALUE self)
{
CvMoments *moments = CVMOMENTS(self);
double
m11 = cvGetCentralMoment(moments, 1, 1),
m20 = cvGetCentralMoment(moments, 2, 0),
m02 = cvGetCentralMoment(moments, 0, 2),
mangle = 0.5 * atan(2 * m11 / (m20 - m02));
if(cvIsNaN(mangle) || cvIsInf(mangle))
return Qnil;
else
return rb_float_new(mangle);
}
void Tonemap::computeLuminance(void)
{
luminanceMap = Image(height, width, CV_32F);
// Take luminance of first pixel as min value
minLum = 0.2126f * radianceMap.ptr<float>(0)[2] + 0.7152f * radianceMap.ptr<float>(0)[1] + 0.0722f * radianceMap.ptr<float>(0)[0];
// Set all other values to zero
maxLum = 0;
avgLum = 0;
logAvgLum = 0;
int numNotNaNPx = 0; // number of pixels that are not NaN
int numLogPx = 0; // number of non-zero pixels
// For all pixels
for (int i=0; i<height; i++)
{
// Pointer to the ith row of the luminance map
float* p = luminanceMap.ptr<float>(i);
// Pointer to the ith row of the radiance map
float* e = radianceMap.ptr<float>(i);
for (int j=0; j<width; j++)
{
// Compute luminance value
p[j] = 0.2126f * e[3*j + 2] + 0.7152f * e[3*j + 1] + 0.0722f * e[3*j + 0];
// Compute min and max luminance values
if (p[j] > maxLum) maxLum = p[j];
if (p[j] < minLum) minLum = p[j];
// Compute average luminance avoiding NaN values
if (cvIsNaN(p[j]) == 0)
{
avgLum += long double(p[j]);
numNotNaNPx++;
}
else
{
#ifdef DEBUG
std::cout << "Warning! Pixel location (" << i << "," << j << ") in radiance map is NaN."
<< "\n\tRed channel value is " << e[3*j + 2]
<< "\n\tGreen channel value is " << e[3*j + 1]
<< "\n\tBlue channel value is " << e[3*j + 0]
<< std::endl;
#endif
}
// Compute log average for non-zero pixels
if (p[j] > 0)
{
logAvgLum += log(p[j]);
numLogPx++;
}
}
int CV_BayesianProbTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
int i, j, n = hist_count/2;
double s[CV_MAX_DIM];
const double err_level = 1e-5;
for( i = 0; i < total_size; i++ )
{
double sum = 0;
for( j = 0; j < n; j++ )
{
double v = hist[j]->mat.data.fl[i];
sum += v;
s[j] = v;
}
sum = sum > DBL_EPSILON ? 1./sum : 0;
for( j = 0; j < n; j++ )
{
double v0 = s[j]*sum;
double v = hist[j+n]->mat.data.fl[i];
if( cvIsNaN(v) || cvIsInf(v) )
{
ts->printf( CvTS::LOG,
"The element #%d in the destination histogram #%d is invalid (=%g)\n",
i, j, v );
code = CvTS::FAIL_INVALID_OUTPUT;
break;
}
else if( fabs(v0 - v) > err_level*fabs(v0) )
{
ts->printf( CvTS::LOG,
"The element #%d in the destination histogram #%d is inaccurate (=%g, should be =%g)\n",
i, j, v, v0 );
code = CvTS::FAIL_BAD_ACCURACY;
break;
}
}
if( j < n )
break;
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
int CV_NormHistTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
double sum = 0;
if( hist_type == CV_HIST_ARRAY )
{
int i;
const float* ptr = (float*)cvPtr1D( hist[0]->bins, 0 );
for( i = 0; i < total_size; i++ )
sum += ptr[i];
}
else
{
CvSparseMat* sparse = (CvSparseMat*)hist[0]->bins;
CvSparseMatIterator iterator;
CvSparseNode *node;
for( node = cvInitSparseMatIterator( sparse, &iterator );
node != 0; node = cvGetNextSparseNode( &iterator ))
{
sum += *(float*)CV_NODE_VAL(sparse,node);
}
}
if( cvIsNaN(sum) || cvIsInf(sum) )
{
ts->printf( CvTS::LOG,
"The normalized histogram has invalid sum =%g\n", sum );
code = CvTS::FAIL_INVALID_OUTPUT;
}
else if( fabs(sum - factor) > FLT_EPSILON*10*fabs(factor) )
{
ts->printf( CvTS::LOG,
"The normalized histogram has incorrect sum =%g, while it should be =%g\n", sum, factor );
code = CvTS::FAIL_BAD_ACCURACY;
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
bool MotionTrackerFB::isFlowCorrect(cv::Point2f u)
{
return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
}
void CV_OptFlowPyrLKTest::run( int )
{
int code = cvtest::TS::OK;
const double success_error_level = 0.3;
const int bad_points_max = 8;
/* test parameters */
double max_err = 0., sum_err = 0;
int pt_cmpd = 0;
int pt_exceed = 0;
int merr_i = 0, merr_j = 0, merr_k = 0;
char filename[1000];
CvPoint2D32f *u = 0, *v = 0, *v2 = 0;
CvMat *_u = 0, *_v = 0, *_v2 = 0;
char* status = 0;
IplImage* imgI = 0;
IplImage* imgJ = 0;
int n = 0, i = 0;
sprintf( filename, "%soptflow/%s", ts->get_data_path().c_str(), "lk_prev.dat" );
_u = (CvMat*)cvLoad( filename );
if( !_u )
{
ts->printf( cvtest::TS::LOG, "could not read %s\n", filename );
code = cvtest::TS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
sprintf( filename, "%soptflow/%s", ts->get_data_path().c_str(), "lk_next.dat" );
_v = (CvMat*)cvLoad( filename );
if( !_v )
{
ts->printf( cvtest::TS::LOG, "could not read %s\n", filename );
code = cvtest::TS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
if( _u->cols != 2 || CV_MAT_TYPE(_u->type) != CV_32F ||
_v->cols != 2 || CV_MAT_TYPE(_v->type) != CV_32F || _v->rows != _u->rows )
{
ts->printf( cvtest::TS::LOG, "the loaded matrices of points are not valid\n" );
code = cvtest::TS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
u = (CvPoint2D32f*)_u->data.fl;
v = (CvPoint2D32f*)_v->data.fl;
/* allocate adidtional buffers */
_v2 = cvCloneMat( _u );
v2 = (CvPoint2D32f*)_v2->data.fl;
/* read first image */
sprintf( filename, "%soptflow/%s", ts->get_data_path().c_str(), "rock_1.bmp" );
imgI = cvLoadImage( filename, -1 );
if( !imgI )
{
ts->printf( cvtest::TS::LOG, "could not read %s\n", filename );
code = cvtest::TS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
/* read second image */
sprintf( filename, "%soptflow/%s", ts->get_data_path().c_str(), "rock_2.bmp" );
imgJ = cvLoadImage( filename, -1 );
if( !imgJ )
{
ts->printf( cvtest::TS::LOG, "could not read %s\n", filename );
code = cvtest::TS::FAIL_MISSING_TEST_DATA;
goto _exit_;
}
n = _u->rows;
status = (char*)cvAlloc(n*sizeof(status[0]));
/* calculate flow */
cvCalcOpticalFlowPyrLK( imgI, imgJ, 0, 0, u, v2, n, cvSize( 41, 41 ),
4, status, 0, cvTermCriteria( CV_TERMCRIT_ITER|
CV_TERMCRIT_EPS, 30, 0.01f ), 0 );
/* compare results */
for( i = 0; i < n; i++ )
{
if( status[i] != 0 )
{
double err;
if( cvIsNaN(v[i].x) )
{
merr_j++;
continue;
}
err = fabs(v2[i].x - v[i].x) + fabs(v2[i].y - v[i].y);
if( err > max_err )
{
max_err = err;
merr_i = i;
}
pt_exceed += err > success_error_level;
sum_err += err;
pt_cmpd++;
}
else
{
if( !cvIsNaN( v[i].x ))
{
merr_i = i;
merr_k++;
ts->printf( cvtest::TS::LOG, "The algorithm lost the point #%d\n", i );
code = cvtest::TS::FAIL_BAD_ACCURACY;
goto _exit_;
}
}
}
if( pt_exceed > bad_points_max )
{
ts->printf( cvtest::TS::LOG,
"The number of poorly tracked points is too big (>=%d)\n", pt_exceed );
code = cvtest::TS::FAIL_BAD_ACCURACY;
goto _exit_;
}
if( max_err > 1 )
{
ts->printf( cvtest::TS::LOG, "Maximum tracking error is too big (=%g) at %d\n", max_err, merr_i );
code = cvtest::TS::FAIL_BAD_ACCURACY;
goto _exit_;
}
_exit_:
cvFree( &status );
cvReleaseMat( &_u );
cvReleaseMat( &_v );
cvReleaseMat( &_v2 );
cvReleaseImage( &imgI );
cvReleaseImage( &imgJ );
if( code < 0 )
ts->set_failed_test_info( code );
}
int rotatedRectangleIntersection( const RotatedRect& rect1, const RotatedRect& rect2, OutputArray intersectingRegion )
{
const float samePointEps = 0.00001; // used to test if two points are the same
Point2f vec1[4], vec2[4];
Point2f pts1[4], pts2[4];
std::vector <Point2f> intersection;
rect1.points(pts1);
rect2.points(pts2);
int ret = INTERSECT_FULL;
// Specical case of rect1 == rect2
{
bool same = true;
for( int i = 0; i < 4; i++ )
{
if( fabs(pts1[i].x - pts2[i].x) > samePointEps || (fabs(pts1[i].y - pts2[i].y) > samePointEps) )
{
same = false;
break;
}
}
if(same)
{
intersection.resize(4);
for( int i = 0; i < 4; i++ )
{
intersection[i] = pts1[i];
}
Mat(intersection).copyTo(intersectingRegion);
return INTERSECT_FULL;
}
}
// Line vector
// A line from p1 to p2 is: p1 + (p2-p1)*t, t=[0,1]
for( int i = 0; i < 4; i++ )
{
vec1[i].x = pts1[(i+1)%4].x - pts1[i].x;
vec1[i].y = pts1[(i+1)%4].y - pts1[i].y;
vec2[i].x = pts2[(i+1)%4].x - pts2[i].x;
vec2[i].y = pts2[(i+1)%4].y - pts2[i].y;
}
// Line test - test all line combos for intersection
for( int i = 0; i < 4; i++ )
{
for( int j = 0; j < 4; j++ )
{
// Solve for 2x2 Ax=b
float x21 = pts2[j].x - pts1[i].x;
float y21 = pts2[j].y - pts1[i].y;
float vx1 = vec1[i].x;
float vy1 = vec1[i].y;
float vx2 = vec2[j].x;
float vy2 = vec2[j].y;
float det = vx2*vy1 - vx1*vy2;
float t1 = (vx2*y21 - vy2*x21) / det;
float t2 = (vx1*y21 - vy1*x21) / det;
// This takes care of parallel lines
if( cvIsInf(t1) || cvIsInf(t2) || cvIsNaN(t1) || cvIsNaN(t2) )
{
continue;
}
if( t1 >= 0.0f && t1 <= 1.0f && t2 >= 0.0f && t2 <= 1.0f )
{
float xi = pts1[i].x + vec1[i].x*t1;
float yi = pts1[i].y + vec1[i].y*t1;
intersection.push_back(Point2f(xi,yi));
}
}
}
if( !intersection.empty() )
{
ret = INTERSECT_PARTIAL;
}
// Check for vertices from rect1 inside recct2
for( int i = 0; i < 4; i++ )
{
// We do a sign test to see which side the point lies.
// If the point all lie on the same sign for all 4 sides of the rect,
// then there's an intersection
int posSign = 0;
int negSign = 0;
float x = pts1[i].x;
float y = pts1[i].y;
for( int j = 0; j < 4; j++ )
{
// line equation: Ax + By + C = 0
// see which side of the line this point is at
float A = -vec2[j].y;
float B = vec2[j].x;
float C = -(A*pts2[j].x + B*pts2[j].y);
float s = A*x+ B*y+ C;
if( s >= 0 )
{
posSign++;
}
else
{
negSign++;
}
}
if( posSign == 4 || negSign == 4 )
{
intersection.push_back(pts1[i]);
}
}
// Reverse the check - check for vertices from rect2 inside recct1
for( int i = 0; i < 4; i++ )
{
// We do a sign test to see which side the point lies.
// If the point all lie on the same sign for all 4 sides of the rect,
// then there's an intersection
int posSign = 0;
int negSign = 0;
float x = pts2[i].x;
float y = pts2[i].y;
for( int j = 0; j < 4; j++ )
{
// line equation: Ax + By + C = 0
// see which side of the line this point is at
float A = -vec1[j].y;
float B = vec1[j].x;
float C = -(A*pts1[j].x + B*pts1[j].y);
float s = A*x + B*y + C;
if( s >= 0 )
{
posSign++;
}
else
{
negSign++;
}
}
if( posSign == 4 || negSign == 4 )
{
intersection.push_back(pts2[i]);
}
}
// Get rid of dupes
for( int i = 0; i < (int)intersection.size()-1; i++ )
{
for( size_t j = i+1; j < intersection.size(); j++ )
{
float dx = intersection[i].x - intersection[j].x;
float dy = intersection[i].y - intersection[j].y;
double d2 = dx*dx + dy*dy; // can be a really small number, need double here
if( d2 < samePointEps*samePointEps )
{
// Found a dupe, remove it
std::swap(intersection[j], intersection.back());
intersection.pop_back();
i--; // restart check
}
}
}
if( intersection.empty() )
{
return INTERSECT_NONE ;
}
// If this check fails then it means we're getting dupes, increase samePointEps
CV_Assert( intersection.size() <= 8 );
Mat(intersection).copyTo(intersectingRegion);
return ret;
}
inline bool isFlowCorrect(Point2f u)
{
return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
}
bool CameraInteraction::isFlowCorrect(float u) {
return !cvIsNaN(u) && (fabs(u) < 1e9);
}
CV_EXPORTS
inline bool
isValidDepth(const double & depth)
{
return !cvIsNaN(depth);
}
/** Checks if the value is a valid depth. For CV_16U or CV_16S, the convention is to be invalid if it is
* a limit. For a float/double, we just check if it is a NaN
* @param depth the depth to check for validity
*/
CV_EXPORTS
inline bool
isValidDepth(const float & depth)
{
return !cvIsNaN(depth);
}
// source point clouds are assumed to contain their normals
int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, Matx44d& pose)
{
int n = srcPC.rows;
const bool useRobustReject = m_rejectionScale>0;
Mat srcTemp = srcPC.clone();
Mat dstTemp = dstPC.clone();
Vec3d meanSrc, meanDst;
computeMeanCols(srcTemp, meanSrc);
computeMeanCols(dstTemp, meanDst);
Vec3d meanAvg = 0.5 * (meanSrc + meanDst);
subtractColumns(srcTemp, meanAvg);
subtractColumns(dstTemp, meanAvg);
double distSrc = computeDistToOrigin(srcTemp);
double distDst = computeDistToOrigin(dstTemp);
double scale = (double)n / ((distSrc + distDst)*0.5);
srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) *= scale;
dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) *= scale;
Mat srcPC0 = srcTemp;
Mat dstPC0 = dstTemp;
// initialize pose
pose = Matx44d::eye();
Mat M = Mat::eye(4,4,CV_64F);
double tempResidual = 0;
// walk the pyramid
for (int level = m_numLevels-1; level >=0; level--)
{
const double impact = 2;
double div = pow((double)impact, (double)level);
//double div2 = div*div;
const int numSamples = cvRound((double)(n/(div)));
const double TolP = m_tolerance*(double)(level+1)*(level+1);
const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
// Obtain the sampled point clouds for this level: Also rotates the normals
Mat srcPCT = transformPCPose(srcPC0, pose);
const int sampleStep = cvRound((double)n/(double)numSamples);
srcPCT = samplePCUniform(srcPCT, sampleStep);
/*
Tolga Birdal thinks that downsampling the scene points might decrease the accuracy.
Hamdi Sahloul, however, noticed that accuracy increased (pose residual decreased slightly).
*/
Mat dstPCS = samplePCUniform(dstPC0, sampleStep);
void* flann = indexPCFlann(dstPCS);
double fval_old=9999999999;
double fval_perc=0;
double fval_min=9999999999;
Mat Src_Moved = srcPCT.clone();
int i=0;
size_t numElSrc = (size_t)Src_Moved.rows;
int sizesResult[2] = {(int)numElSrc, 1};
float* distances = new float[numElSrc];
int* indices = new int[numElSrc];
Mat Indices(2, sizesResult, CV_32S, indices, 0);
Mat Distances(2, sizesResult, CV_32F, distances, 0);
// use robust weighting for outlier treatment
int* indicesModel = new int[numElSrc];
int* indicesScene = new int[numElSrc];
int* newI = new int[numElSrc];
int* newJ = new int[numElSrc];
Matx44d PoseX = Matx44d::eye();
while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
{
uint di=0, selInd = 0;
queryPCFlann(flann, Src_Moved, Indices, Distances);
for (di=0; di<numElSrc; di++)
{
newI[di] = di;
newJ[di] = indices[di];
}
if (useRobustReject)
{
int numInliers = 0;
float threshold = getRejectionThreshold(distances, Distances.rows, m_rejectionScale);
Mat acceptInd = Distances<threshold;
uchar *accPtr = (uchar*)acceptInd.data;
for (int l=0; l<acceptInd.rows; l++)
{
if (accPtr[l])
{
newI[numInliers] = l;
newJ[numInliers] = indices[l];
numInliers++;
}
}
numElSrc=numInliers;
}
// Step 2: Picky ICP
// Among the resulting corresponding pairs, if more than one scene point p_i
// is assigned to the same model point m_j, then select p_i that corresponds
// to the minimum distance
hashtable_int* duplicateTable = getHashtable(newJ, numElSrc, dstPCS.rows);
for (di=0; di<duplicateTable->size; di++)
{
hashnode_i *node = duplicateTable->nodes[di];
if (node)
{
// select the first node
size_t idx = reinterpret_cast<size_t>(node->data)-1, dn=0;
int dup = (int)node->key-1;
size_t minIdxD = idx;
float minDist = distances[idx];
while ( node )
{
idx = reinterpret_cast<size_t>(node->data)-1;
if (distances[idx] < minDist)
{
minDist = distances[idx];
minIdxD = idx;
}
node = node->next;
dn++;
}
indicesModel[ selInd ] = newI[ minIdxD ];
indicesScene[ selInd ] = dup ;
selInd++;
}
}
hashtableDestroy(duplicateTable);
if (selInd >= 6)
{
Mat Src_Match = Mat(selInd, srcPCT.cols, CV_64F);
Mat Dst_Match = Mat(selInd, srcPCT.cols, CV_64F);
for (di=0; di<selInd; di++)
{
const int indModel = indicesModel[di];
const int indScene = indicesScene[di];
const float *srcPt = srcPCT.ptr<float>(indModel);
const float *dstPt = dstPCS.ptr<float>(indScene);
double *srcMatchPt = Src_Match.ptr<double>(di);
double *dstMatchPt = Dst_Match.ptr<double>(di);
int ci=0;
for (ci=0; ci<srcPCT.cols; ci++)
{
srcMatchPt[ci] = (double)srcPt[ci];
dstMatchPt[ci] = (double)dstPt[ci];
}
}
Vec3d rpy, t;
minimizePointToPlaneMetric(Src_Match, Dst_Match, rpy, t);
if (cvIsNaN(cv::trace(rpy)) || cvIsNaN(cv::norm(t)))
break;
getTransformMat(rpy, t, PoseX);
Src_Moved = transformPCPose(srcPCT, PoseX);
double fval = cv::norm(Src_Match, Dst_Match)/(double)(Src_Moved.rows);
// Calculate change in error between iterations
fval_perc=fval/fval_old;
// Store error value
fval_old=fval;
if (fval < fval_min)
fval_min = fval;
}
else
break;
i++;
}
pose = PoseX * pose;
residual = tempResidual;
delete[] newI;
delete[] newJ;
delete[] indicesModel;
delete[] indicesScene;
delete[] distances;
delete[] indices;
tempResidual = fval_min;
destroyFlann(flann);
}
Matx33d Rpose;
Vec3d Cpose;
poseToRT(pose, Rpose, Cpose);
Cpose = Cpose / scale + meanAvg - Rpose * meanAvg;
rtToPose(Rpose, Cpose, pose);
residual = tempResidual;
return 0;
}
bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
{
CV_INSTRUMENT_REGION();
Mat img = _img.getMat(), cornersM = _corners.getMat();
int ncorners = cornersM.checkVector(2, CV_32F);
CV_Assert( ncorners >= 0 );
Point2f* corners = cornersM.ptr<Point2f>();
const int nbins = 256;
float ranges[] = {0, 256};
const float* _ranges = ranges;
Mat hist;
Mat black_comp, white_comp;
for(int i = 0; i < ncorners; i++)
{
int channels = 0;
Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height),
region_size.width*2 + 1, region_size.height*2 + 1);
Mat img_roi = img(roi);
calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges);
int black_thresh = 0, white_thresh = 0;
segment_hist_max(hist, black_thresh, white_thresh);
threshold(img, black_comp, black_thresh, 255.0, THRESH_BINARY_INV);
threshold(img, white_comp, white_thresh, 255.0, THRESH_BINARY);
const int erode_count = 1;
erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count);
erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);
std::vector<std::vector<Point> > white_contours, black_contours;
findContours(black_comp, black_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
findContours(white_comp, white_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
if(black_contours.size() < 5 || white_contours.size() < 5) continue;
// find two white and black blobs that are close to the input point
std::vector<std::pair<int, float> > white_order, black_order;
orderContours(black_contours, corners[i], black_order);
orderContours(white_contours, corners[i], white_order);
const float max_dist = 10.0f;
if(black_order[0].second > max_dist || black_order[1].second > max_dist ||
white_order[0].second > max_dist || white_order[1].second > max_dist)
{
continue; // there will be no improvement in this corner position
}
const std::vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first],
&white_contours[white_order[0].first], &white_contours[white_order[1].first]};
std::vector<Point2f> quads_approx[4];
Point2f quad_corners[4];
for(int k = 0; k < 4; k++)
{
std::vector<Point2f> temp;
for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]);
approxPolyDP(Mat(temp), quads_approx[k], 0.5, true);
findCorner(quads_approx[k], corners[i], quad_corners[k]);
quad_corners[k] += Point2f(0.5f, 0.5f);
}
// cross two lines
Point2f origin1 = quad_corners[0];
Point2f dir1 = quad_corners[1] - quad_corners[0];
Point2f origin2 = quad_corners[2];
Point2f dir2 = quad_corners[3] - quad_corners[2];
double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2)));
if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue;
findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]);
}
return true;
}
int CV_CompareHistTest::validate_test_results( int /*test_case_idx*/ )
{
int code = CvTS::OK;
int i;
double result0[MAX_METHOD+1];
double s0 = 0, s1 = 0, sq0 = 0, sq1 = 0, t;
for( i = 0; i < MAX_METHOD; i++ )
result0[i] = 0;
if( hist_type == CV_HIST_ARRAY )
{
float* ptr0 = (float*)cvPtr1D( hist[0]->bins, 0 );
float* ptr1 = (float*)cvPtr1D( hist[1]->bins, 0 );
for( i = 0; i < total_size; i++ )
{
double v0 = ptr0[i], v1 = ptr1[i];
result0[CV_COMP_CORREL] += v0*v1;
result0[CV_COMP_INTERSECT] += MIN(v0,v1);
if( fabs(v0 + v1) > DBL_EPSILON )
result0[CV_COMP_CHISQR] += (v0 - v1)*(v0 - v1)/(v0 + v1);
s0 += v0;
s1 += v1;
sq0 += v0*v0;
sq1 += v1*v1;
result0[CV_COMP_BHATTACHARYYA] += sqrt(v0*v1);
}
}
else
{
CvSparseMat* sparse0 = (CvSparseMat*)hist[0]->bins;
CvSparseMat* sparse1 = (CvSparseMat*)hist[1]->bins;
CvSparseMatIterator iterator;
CvSparseNode* node;
for( node = cvInitSparseMatIterator( sparse0, &iterator );
node != 0; node = cvGetNextSparseNode( &iterator ) )
{
const int* idx = CV_NODE_IDX(sparse0, node);
double v0 = *(float*)CV_NODE_VAL(sparse0, node);
double v1 = (float)cvGetRealND(sparse1, idx);
result0[CV_COMP_CORREL] += v0*v1;
result0[CV_COMP_INTERSECT] += MIN(v0,v1);
if( fabs(v0 + v1) > DBL_EPSILON )
result0[CV_COMP_CHISQR] += (v0 - v1)*(v0 - v1)/(v0 + v1);
s0 += v0;
sq0 += v0*v0;
result0[CV_COMP_BHATTACHARYYA] += sqrt(v0*v1);
}
for( node = cvInitSparseMatIterator( sparse1, &iterator );
node != 0; node = cvGetNextSparseNode( &iterator ) )
{
const int* idx = CV_NODE_IDX(sparse1, node);
double v1 = *(float*)CV_NODE_VAL(sparse1, node);
double v0 = (float)cvGetRealND(sparse0, idx);
if( fabs(v0) < DBL_EPSILON )
result0[CV_COMP_CHISQR] += v1;
s1 += v1;
sq1 += v1*v1;
}
}
t = (sq0 - s0*s0/total_size)*(sq1 - s1*s1/total_size);
result0[CV_COMP_CORREL] = fabs(t) > DBL_EPSILON ?
(result0[CV_COMP_CORREL] - s0*s1/total_size)/sqrt(t) : 1;
s1 *= s0;
s0 = result0[CV_COMP_BHATTACHARYYA];
s0 = 1. - s0*(s1 > FLT_EPSILON ? 1./sqrt(s1) : 1.);
result0[CV_COMP_BHATTACHARYYA] = sqrt(MAX(s0,0.));
for( i = 0; i < MAX_METHOD; i++ )
{
double v = result[i], v0 = result0[i];
const char* method_name =
i == CV_COMP_CHISQR ? "Chi-Square" :
i == CV_COMP_CORREL ? "Correlation" :
i == CV_COMP_INTERSECT ? "Intersection" :
i == CV_COMP_BHATTACHARYYA ? "Bhattacharyya" : "Unknown";
if( cvIsNaN(v) || cvIsInf(v) )
{
ts->printf( CvTS::LOG, "The comparison result using the method #%d (%s) is invalid (=%g)\n",
i, method_name, v );
code = CvTS::FAIL_INVALID_OUTPUT;
break;
}
else if( fabs(v0 - v) > FLT_EPSILON*10*MAX(fabs(v0),0.1) )
{
ts->printf( CvTS::LOG, "The comparison result using the method #%d (%s)\n\tis inaccurate (=%g, should be =%g)\n",
i, method_name, v, v0 );
code = CvTS::FAIL_BAD_ACCURACY;
break;
}
}
if( code < 0 )
ts->set_failed_test_info( code );
return code;
}
