void
test_query_from_file(int verbose)
{
VlKDForest *forest;
int i;
float *data, *query;
vl_size dim;
vl_size num;
/*
* load
*/
data = load_data("data.bin", &dim, &num);
forest = load_VlKDForest("forest.bin");
forest->data = data;
if((query = (float *)malloc(dim * sizeof(float))) == NULL){
printf("not enough memoey\n");
exit(1);
}
for(i = 0;i < dim; i++) query[i] = 0.5;
if(verbose) printf("has created a query\n");
/*
* search neighbors
*/
vl_size numNeighbors = 10;
unsigned int numComparisons = 0 ;
unsigned int maxNumComparisons = 0 ;
VlKDForestNeighbor * neighbors ;
vl_kdforest_set_max_num_comparisons (forest, maxNumComparisons) ;
neighbors = vl_malloc (sizeof(VlKDForestNeighbor) * numNeighbors) ;
numComparisons = vl_kdforest_query (forest, neighbors, numNeighbors,
query);
for(i = 0;i < numNeighbors; i++){
printf("%d %f\n",
neighbors[i].index + 1, neighbors[i].distance);
/* check distance */
if(fabs(
dist_l2(dim, query, &data[neighbors[i].index * dim]) -
neighbors[i].distance) > 1e-6){
printf("%d distance is different. %f\n",
dist_l2(dim, query, &data[neighbors[i].index * dim]) );
}
/* check order */
if(i != 0 && neighbors[i-1].distance > neighbors[i].distance){
printf("order is wrong.\n");
}
}
vl_free(neighbors);
vl_kdforest_delete(forest);
free(data);
free(query);
}
/**
* @brief Camera Response Function Correction
*/
ImageFloat crf_correction (ImageFloat in, ThinPlate tp)
{
/*Iterative adjustment to generate an image of mean(noise_part) = 0.5
* This module supposes that the image is normalized between 0 and 1, where
* 0 is the black value and 1 the white value.
* */
float mean, power, a;
int i;
float center_sharp[2], center_blur[2];
float top_sharp[2], top_blur[2];
int wsize;
ImageFloat out;
out = new_imageFloat (in->ncol, in->nrow);
/*Calculating the center of the pattern in the blur image */
pattern_center (center_sharp);
evaluate_thinPlate (tp, center_sharp, center_blur, 1);
pattern_top_center(top_sharp);
evaluate_thinPlate (tp, top_sharp, top_blur, 1);
/* approx 0.85 half the size of the noise region in the blur image*/
wsize = (int)(0.85 * dist_l2(top_blur[0],top_blur[1],
center_blur[0],center_blur[1]));
/*A parabolic function is estimated as:
* y = ax^2 + (1-a)x
* This parabola maps 0 -> 0, 1-> 1
* a is set in order that yM -> 0.5
* so a = (0.5-mean(x))/(mean(x^2) - mean(x))
*/
mean = mean_subpx_window (in, wsize, center_blur[0], center_blur[1]);
power = power_subpx_window (in, wsize, center_blur[0], center_blur[1]);
a = (0.5 - mean) / (power - mean);
for (i = 0; i < in->ncol * in->nrow; i++)
out->val[i] = a * in->val[i] * in->val[i] + (1 - a) * in->val[i];
return out;
}
/**
* @brief Coarse detection of the pattern by using segments detected from LSD
* @param in - input image float
* @return Array of floats containing the OPQR 2D point positions of the
* pattern
*/
static float *detect_pattern_coarse(ImageFloat in)
{
image_double in_lsd;
ntuple_list seg;
float min_val, max_val;
double *seg_length, *seg_midpoint_x, *seg_midpoint_y;
char is_in_distance;
double xO, xP, xQ, xR, yO, yQ, yP, yR, l;
int *has_seg, *has_all_seg, point;
/* To keep track of the error from the optimal segment position
* just to choose the closest segment to the optimal location
*/
double seg_error[NUM_SEG];
double xi1, xi2, yi1, yi2, xj1, xj2, yj1, yj2, xjN1, xjN2, yjN1, yjN2;
double xoMp, xpMq, xqMr, xrMo, yoMp, ypMq, yqMr, yrMo;
double xC, yC;
/* Structure of the Pattern Seg0,Seg1,...,Seg10,Seg_Orient0,Seg_Orient1 */
const double x1S[] = { 3, 6, 7, 7, 7, 6, 3, 0, -2, -2, -2, 1, 8 };
const double y1S[] = { 0, 0, -1, -4, -7, -9, -9, -9, -7, -4, -1, 1, 0 };
const double x2S[] = { 3, 6, 8, 8, 8, 6, 3, 0, -1, -1, -1, 1, 7 };
const double y2S[] = { 1, 1, -1, -4, -7, -8, -8, -8, -7, -4, -1, 0, 0 };
int i = 0, j = 0, k = 0, c = 0;
double errorc;
double actual_scale;
char ready;
float *opqr = (float *) malloc(8 * sizeof(float));
/* Execute LSD */
/* Convert between images types and renormalize the image to [0,255]*/
min_val = BIG_NUMBER;
max_val = 0;
for (i=0; i< in->ncol *in->nrow;i++)
{
if(in->val[i] < min_val) min_val = in->val[i];
if(in->val[i] > max_val) max_val = in->val[i];
}
in_lsd = new_image_double((unsigned int) in->ncol,
(unsigned int) in->nrow);
for (i = 0; i < in->ncol * in->nrow; i++)
in_lsd->data[i] = (double) 255/(max_val-min_val)*(in->val[i]-min_val);
/* We do a LOOP from INITIAL_SCALE_LSD to MAX_SCALE_LSD */
actual_scale = INITIAL_SCALE_LSD;
ready = 0;
while (actual_scale < MAX_SCALE_LSD && !ready)
{
printf(" -->LSD scale =%f\n",actual_scale);
seg = lsd_scale(in_lsd, actual_scale);
/* allocate the array or exit with an error */
if ((seg_length = (double *) malloc(seg->size * sizeof(double)))
== NULL
|| (seg_midpoint_x = (double *) malloc(seg->size * sizeof(double)))
== NULL
|| (seg_midpoint_y = (double *) malloc(seg->size * sizeof(double)))
== NULL)
{
error("PSF_ESTIM - Unable to allocate double array space");
exit(EXIT_FAILURE);
}
/*
The i component, of the n-tuple number j, of an n-tuple list 'ntl'
is accessed with:
*/
for (i = 0; i < (int) seg->size; i++)
{
/* segment length */
seg_length[i] = dist_l2(seg->values[i * seg->dim],
seg->values[i * seg->dim + 1],
seg->values[i * seg->dim + 2],
seg->values[i * seg->dim + 3]);
/* segment midpoint */
seg_midpoint_x[i] =
0.5 * (seg->values[i * seg->dim]
+ seg->values[i * seg->dim + 2]);
seg_midpoint_y[i] =
0.5 * (seg->values[i * seg->dim + 1]
+ seg->values[i * seg->dim + 3]);
}
/* Accessing to segment j=0...12 associated to segment i
* has_seg[NUM_SEG*i+j], initialization default to 0
*/
if ((has_seg =
(int *) malloc(NUM_SEG * seg->size * sizeof(int))) == NULL
|| (has_all_seg =
(int *) malloc(seg->size * sizeof(int))) == NULL)
{
error("PSF_ESTIM - Unable to allocate double array space");
exit(EXIT_FAILURE);
}
/* has_seg[], Initialization default to -1 */
for (i = 0; i < NUM_SEG * (int) seg->size; i++)
{
has_seg[i] = -1;
}
/* has_all_seg[], Initialization default to 0 */
/* First pass */
for (i = 0; i < (int) seg->size; i++)
{
xi1 = seg->values[i * seg->dim];
xi2 = seg->values[i * seg->dim + 2];
yi1 = seg->values[i * seg->dim + 1];
yi2 = seg->values[i * seg->dim + 3];
/* Reinitialize the error track */
for (j = 0; j < NUM_SEG; j++)
{
seg_error[j] = BIG_NUMBER;
}
for (j = 0; j < (int) seg->size; j++)
{
xj1 = seg->values[j * seg->dim];
xj2 = seg->values[j * seg->dim + 2];
yj1 = seg->values[j * seg->dim + 1];
yj2 = seg->values[j * seg->dim + 3];
/* Convert the (x,y) coordinates to a new Coordinate System
* (xN, yN) having:
* (xi1,yi1) at (0,0)
* (xi2,yi2) at (0,1)
* The vectors x1s,y1s,x2s,y2s are given within this
* new (xN,yN) coordinate system
*/
l = seg_length[i];
xjN1 =
1 / (l * l) * ((yi2 - yi1) * (xj1 - xi1)
- (xi2 - xi1) * (yj1 - yi1));
yjN1 =
1 / (l * l) * ((xi2 - xi1) * (xj1 - xi1)
+ (yi2 - yi1) * (yj1 - yi1));
xjN2 =
1 / (l * l) * ((yi2 - yi1) * (xj2 - xi1)
- (xi2 - xi1) * (yj2 - yi1));
yjN2 =
1 / (l * l) * ((xi2 - xi1) * (xj2 - xi1)
+ (yi2 - yi1) * (yj2 - yi1));
for (c = 0; c < NUM_SEG; c++)
{
is_in_distance =
(fabs(xjN1 - x1S[c]) < TOL * (2 + fabs(x1S[c])))
&& (fabs(yjN1 - y1S[c]) < TOL * (2 + fabs(y1S[c])))
&& (fabs(xjN2 - x2S[c]) < TOL * (2 + fabs(x2S[c])))
&& (fabs(yjN2 - y2S[c]) < TOL * (2 + fabs(y2S[c])));
if (is_in_distance)
{
/* Need to check that there isn't a previous segment
* closer to the optimal location and already marked
* as good (errorc). I just keep the segment with
* minimum total error l1.
*/
errorc = fabs(xjN1 - x1S[c]) + fabs(yjN1 - y1S[c])
+ fabs(xjN2 - x2S[c]) + fabs(yjN2 - y2S[c]);
if (errorc < seg_error[c])
{
has_seg[i * NUM_SEG + c] = j;
seg_error[c] = errorc;
}
}
}
}
/*has_all_seg[i] will be one if all segments are present */
has_all_seg[i] = 1;
for (j=0;j< NUM_SEG;j++)
{
has_all_seg[i] =
has_all_seg[i] && (has_seg[i * NUM_SEG + j] >= 0);
}
if (has_all_seg[i])
{
point = i;
k++;
}
}
ready = (k==1);
actual_scale *= 1.15;
}
if (k > 1)
{
printf("More than one pattern was detected.");
printf("\nCrop the image surounding the desired pattern and re-run.");
exit(EXIT_SEVERAL_PATTERNS_DETECTED);
}
else if(k<1)
{
printf("No pattern was detected. Use another image");
exit(EXIT_NO_PATTERN_DETECTED);
}
/*Calculate C - center, u = unit_length, theta = angle */
/* 1/3*(DET + 0 + 1) = oMp */
xoMp = 0.33333 * (seg_midpoint_x[point]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 0]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 1]]);
yoMp = 0.33333 * (seg_midpoint_y[point]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 0]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 1]]);
/* 1/3*(2 + 3 + 4) = pMq */
xpMq = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 2]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 3]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 4]]);
ypMq = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 2]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 3]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 4]]);
/* 1/3*(5 + 6 + 7) = qMr */
xqMr = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 5]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 6]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 7]]);
yqMr = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 5]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 6]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 7]]);
/* 1/3*(8 + 9 + 10) = rMo */
xrMo = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 8]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 9]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 10]]);
yrMo = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 8]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 9]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 10]]);
/*Center */
xC = 0.25 * (xoMp + xpMq + xqMr + xrMo);
yC = 0.25 * (yoMp + ypMq + yqMr + yrMo);
/*O = C + CoMr + CoMp */
xO = xC + (xrMo - xC) + (xoMp - xC);
yO = yC + (yrMo - yC) + (yoMp - yC);
/*P = C + CpMq + CoMp */
xP = xC + (xpMq - xC) + (xoMp - xC);
yP = yC + (ypMq - yC) + (yoMp - yC);
/*Q = C + CqMr + CpMq */
xQ = xC + (xqMr - xC) + (xpMq - xC);
yQ = yC + (yqMr - yC) + (ypMq - yC);
/*R = C + CrMo + CqMr */
xR = xC + (xrMo - xC) + (xqMr - xC);
yR = yC + (yrMo - yC) + (yqMr - yC);
/*Array of OPQR coordinates*/
opqr[0] = (float) xO;
opqr[1] = (float) yO;
opqr[2] = (float) xP;
opqr[3] = (float) yP;
opqr[4] = (float) xQ;
opqr[5] = (float) yQ;
opqr[6] = (float) xR;
opqr[7] = (float) yR;
/* free memory */
free_image_double(in_lsd);
free_ntuple_list(seg);
free(seg_length);
free(seg_midpoint_y);
free(seg_midpoint_x);
free(has_seg);
free(has_all_seg);
return opqr;
}
void
test_simple(int verbose)
{
VlKDForest *forest;
int i, j;
float *data, *query;
vl_size dim = 128;
vl_size num = 10000;
vl_size numTrees = 1;
/*
* create a test data
*/
if((data = create_data(dim ,num)) == NULL){
printf("not enough memoey\n");
exit(1);
}
if(verbose) printf("has created a test data\n");
if((query = (float *)malloc(dim * sizeof(float))) == NULL){
printf("not enough memoey\n");
exit(1);
}
for(i = 0;i < dim; i++) query[i] = 0.5;
if(verbose) printf("has created a query\n");
/*
* build a kd-tree forest
*/
forest = kdtreebuild(1, dim, numTrees, VL_KDTREE_MEDIAN, num, data);
if(verbose) printf("has created a forest\n");
if(verbose && 0){
for(j = 0;j < numTrees; j++){
printf("dataIndex[%d] = [", j);
for(i = 0;i < forest->numData; i++){
printf("%d ",
forest->trees[j]->dataIndex[i].index + 1);
}
printf("]\n");
}
}
/*
* save
*/
save_data("data.bin", data, dim, num);
save_VlKDForest("forest.bin", forest);
/*
* search neighbors
*/
vl_size numNeighbors = 10;
unsigned int numComparisons = 0 ;
unsigned int maxNumComparisons = 0 ;
VlKDForestNeighbor * neighbors ;
vl_kdforest_set_max_num_comparisons (forest, maxNumComparisons) ;
neighbors = vl_malloc (sizeof(VlKDForestNeighbor) * numNeighbors) ;
numComparisons = vl_kdforest_query (forest, neighbors, numNeighbors,
query);
for(i = 0;i < numNeighbors; i++){
printf("%d %f\n",
neighbors[i].index + 1, neighbors[i].distance);
/* check distance */
if(fabs(
dist_l2(dim, query, &data[neighbors[i].index * dim]) -
neighbors[i].distance) > 1e-6){
printf("%d distance is different. %f\n",
dist_l2(dim, query, &data[neighbors[i].index * dim]) );
}
/* check order */
if(i != 0 && neighbors[i-1].distance > neighbors[i].distance){
printf("order is wrong.\n");
}
}
vl_free(neighbors);
vl_kdforest_delete(forest);
free(data);
free(query);
}
/**
* @brief Detection of a X corner in the imput image
* @param in - input image float
* @param ptx - approximate x coord. of the X corner; (output) refined position
* @param pty - approximate y coord. of the X corner; (output) refined position
* @return int - 0 if no error
*/
static int detect_xcorner(ImageFloat in, float *ptx, float *pty)
{
ImageFloat mask, src_buff, gx_buff, gy_buff;
float coeff;
int i, j, k, y, x;
float cx = *ptx;
float cy = *pty;
float c2x, c2y;
int max_iters = 400;
float eps = 0.00001;
int wsize = 3;
float a11, a12, a22, p, q, d;
int iter = 0;
float err;
float py, px;
float tgx, tgy, gxx, gxy, gyy, m;
float *mask1D;
/*mask1D = new_vector(2*wsize+1); */
mask1D = (float *) malloc((2 * wsize + 1) * sizeof(float));
mask = new_imageFloat(2 * wsize + 1, 2 * wsize + 1);
coeff = 1. / (mask->ncol * mask->nrow);
/* calculate mask */
for (i = -wsize, k = 0; i <= wsize; i++, k++)
{
mask1D[k] = exp(-i * i * coeff);
}
for (i = 0; i < (int) mask->nrow; i++)
{
for (j = 0; j < (int) mask->ncol; j++)
{
mask->val[i * mask->nrow + j] = mask1D[j] * mask1D[i];
}
}
do {
src_buff = extract_subpx_window(in, wsize, cx, cy);
gx_buff = gradx(src_buff);
gy_buff = grady(src_buff);
a11 = a12 = a22 = p = q = 0;
/* process gradient */
for (y = -wsize, k = 0; y <= wsize; y++)
{
py = cy + (float) y;
for (x = -wsize; x <= wsize; x++, k++)
{
m = mask->val[k];
tgx = gx_buff->val[k];
tgy = gy_buff->val[k];
gxx = tgx * tgx * m;
gxy = tgx * tgy * m;
gyy = tgy * tgy * m;
px = cx + (float) x;
a11 += gxx;
a12 += gxy;
a22 += gyy;
p += gxx * px + gxy * py;
q += gxy * px + gyy * py;
}
}
d = a11 * a22 - a12 * a12;
c2x = 1 / d * (a22 * p - a12 * q);
c2y = 1 / d * (-a12 * p + a11 * q);
err = dist_l2(cx, cy, c2x, c2y);
cx = c2x;
cy = c2y;
free_imageFloat(src_buff);
free_imageFloat(gx_buff);
free_imageFloat(gy_buff);
} while (++iter < max_iters && err > eps);
*ptx = cx;
*pty = cy;
free_imageFloat(mask);
free((void *) mask1D);
return 0;
}
