void hsm_find_peaks_circ(int n, const double*f, double min_angle_deg, int unidir, int max_peaks,
int*peaks, int* npeaks)
{
sm_log_push("hsm_find_peaks_circ");
assert(max_peaks>0);
/* Find all local maxima for the function */
int maxima[n], nmaxima;
hsm_find_local_maxima_circ(n, f, maxima, &nmaxima);
sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
/* Sort based on value */
qsort_descending(maxima, (size_t) nmaxima, f);
*npeaks = 0;
sm_log_push("For each maximum");
/* Only retain a subset of these */
for(int m=0;m<nmaxima;m++) {
/* Here's a candidate maximum */
int candidate = maxima[m];
double candidate_angle = candidate * (2*M_PI/n);
/* Check that is not too close to the already accepted maxima */
int acceptable = 1;
for(int a=0;a<*npeaks;a++) {
int other = peaks[a];
double other_angle = other * (2*M_PI/n);
if(hsm_is_angle_between_smaller_than_deg(candidate_angle,other_angle,min_angle_deg)) {
acceptable = 0; break;
}
/* If unidir, check also +M_PI */
if(unidir)
if(hsm_is_angle_between_smaller_than_deg(candidate_angle+M_PI,other_angle,min_angle_deg)) {
acceptable = 0; break;
}
}
sm_debug("%saccepting candidate %d; lag = %d value = %f\n",
acceptable?"":"not ", m, maxima[m], f[maxima[m]]);
if(acceptable) {
peaks[*npeaks] = candidate;
(*npeaks) ++;
}
if(*npeaks>=max_peaks) break;
}
sm_log_pop();
sm_debug("found %d (max %d) maxima.\n", *npeaks, max_peaks);
sm_log_pop();
}
void hsm_find_peaks_linear(int n, const double*f, double min_dist, int max_peaks,
int*peaks, int* npeaks)
{
sm_log_push("hsm_find_peaks_linear");
assert(max_peaks>0);
/* Find all local maxima for the function */
int maxima[n], nmaxima;
hsm_find_local_maxima_linear(n,f,maxima,&nmaxima);
sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
/* Sort based on value */
qsort_descending(maxima, (size_t) nmaxima, f);
*npeaks = 0;
sm_log_push("for each maximum");
/* Only retain a subset of these */
for(int m=0;m<nmaxima;m++) {
/* Here's a candidate maximum */
int candidate = maxima[m];
/* Check that is not too close to the already accepted maxima */
int acceptable = 1;
for(int a=0;a<*npeaks;a++) {
int other = peaks[a];
if(abs(other-candidate) < min_dist) {
acceptable = 0; break;
}
}
sm_debug("%s accepting candidate %d; lag = %d value = %f\n",
acceptable?"":"not", m, maxima[m], f[maxima[m]]);
if(acceptable) {
peaks[*npeaks] = candidate;
(*npeaks) ++;
}
if(*npeaks >= max_peaks) break;
}
sm_log_pop("");
sm_debug("Found %d (max %d) maxima.\n", *npeaks, max_peaks);
sm_log_pop();
}
int main(int argc, char **argv)
{
const int dim = 128;
if (argc != 6 && argc != 7 && argc != 8) {
printf("Usage: %s <tree.in> <db.in> <query.in> <num_nbrs> "
"<matches.out> [distance_type:1] [normalize:1]\n", argv[0]);
return 1;
}
char *tree_in = argv[1];
char *db_in = argv[2];
char *query_in = argv[3];
int num_nbrs = atoi(argv[4]);
char *matches_out = argv[5];
DistanceType distance_type = DistanceMin;
bool normalize = true;
#if 0
if (argc >= 7)
output_html = argv[6];
#endif
if (argc >= 7)
distance_type = (DistanceType) atoi(argv[6]);
if (argc >= 8)
normalize = (atoi(argv[7]) != 0);
printf("[VocabMatch] Using tree %s\n", tree_in);
switch (distance_type) {
case DistanceDot:
printf("[VocabMatch] Using distance Dot\n");
break;
case DistanceMin:
printf("[VocabMatch] Using distance Min\n");
break;
default:
printf("[VocabMatch] Using no known distance!\n");
break;
}
/* Read the tree */
printf("[VocabMatch] Reading tree...\n");
fflush(stdout);
clock_t start = clock();
VocabTree tree;
tree.Read(tree_in);
clock_t end = clock();
printf("[VocabMatch] Read tree in %0.3fs\n",
(double) (end - start) / CLOCKS_PER_SEC);
#if 1
tree.Flatten();
#endif
tree.SetDistanceType(distance_type);
tree.SetInteriorNodeWeight(0, 0.0);
/* Read the database keyfiles */
FILE *f = fopen(db_in, "r");
std::vector<std::string> db_files;
char buf[256];
while (fgets(buf, 256, f)) {
/* Remove trailing newline */
if (buf[strlen(buf) - 1] == '\n')
buf[strlen(buf) - 1] = 0;
db_files.push_back(std::string(buf));
}
fclose(f);
/* Read the query keyfiles */
f = fopen(query_in, "r");
std::vector<std::string> query_files;
while (fgets(buf, 256, f)) {
/* Remove trailing newline */
if (buf[strlen(buf) - 1] == '\n')
buf[strlen(buf) - 1] = 0;
char keyfile[256];
sscanf(buf, "%s", keyfile);
query_files.push_back(std::string(keyfile));
}
fclose(f);
int num_db_images = db_files.size();
int num_query_images = query_files.size();
printf("[VocabMatch] Read %d database images\n", num_db_images);
/* Now score each query keyfile */
printf("[VocabMatch] Scoring %d query images...\n", num_query_images);
fflush(stdout);
#if 0
FILE *f_html = fopen(output_html, "w");
PrintHTMLHeader(f_html, num_nbrs);
#endif
float *scores = new float[num_db_images];
double *scores_d = new double[num_db_images];
int *perm = new int[num_db_images];
FILE *f_match = fopen(matches_out, "w");
if (f_match == NULL) {
printf("[VocabMatch] Error opening file %s for writing\n",
matches_out);
return 1;
}
for (int i = 0; i < num_query_images; i++) {
start = clock();
/* Clear scores */
for (int j = 0; j < num_db_images; j++)
scores[j] = 0.0;
int num_keys = 0;
unsigned char *keys = ReadDescriptorFile(query_files[i].c_str(),
dim, num_keys);
clock_t start_score = clock();
double mag = tree.ScoreQueryKeys(num_keys, normalize, keys, scores);
clock_t end_score = end = clock();
printf("[VocabMatch] Scored image %s in %0.3fs "
"( %0.3fs total, num_keys = %d, mag = %0.3f )\n",
query_files[i].c_str(),
(double) (end_score - start_score) / CLOCKS_PER_SEC,
(double) (end - start) / CLOCKS_PER_SEC, num_keys, mag);
/* Find the top scores */
for (int j = 0; j < num_db_images; j++) {
scores_d[j] = (double) scores[j];
}
qsort_descending();
qsort_perm(num_db_images, scores_d, perm);
int top = MIN(num_nbrs, num_db_images);
for (int j = 0; j < top; j++) {
// if (perm[j] == index_i)
// continue;
fprintf(f_match, "%d %d %0.4f\n", i, perm[j], scores_d[j]);
//fprintf(f_match, "%d %d %0.4f\n", i, perm[j], mag - scores_d[j]);
}
fflush(f_match);
fflush(stdout);
#if 0
PrintHTMLRow(f_html, query_files[i], scores_d,
perm, num_nbrs, db_files);
#endif
delete [] keys;
}
fclose(f_match);
#if 0
PrintHTMLFooter(f_html);
fclose(f_html);
#endif
delete [] scores;
delete [] scores_d;
delete [] perm;
return 0;
}
int main(int argc, char **argv)
{
const int dim = 128;
if (argc != 6 && argc != 7 && argc != 8 && argc != 9 && argc != 10 &&
argc != 11) {
printf("Usage: %s <tree.in> <db.in> <query.in> <num_nbrs> "
"<match-script.out> [leaves_only] [distance_type] [normalize] "
"[min_feature_scale] [max_keys]\n",
argv[0]);
return 1;
}
char *tree_in = argv[1];
char *db_in = argv[2];
char *query_in = argv[3];
int num_nbrs = atoi(argv[4]);
char *matches_out = argv[5];
bool leaves_only = false;
bool normalize = true;
double min_feature_scale = 0.0;
DistanceType distance_type = DistanceMin;
int max_keys = 0;
if (argc >= 7) {
if (atoi(argv[6]) != 0)
leaves_only = true;
}
if (argc >= 8)
distance_type = (DistanceType) atoi(argv[7]);
if (argc >= 9)
if (atoi(argv[8]) == 0)
normalize = false;
if (argc >= 10) {
min_feature_scale = atof(argv[9]);
}
if (argc >= 11) {
max_keys = atoi(argv[10]);
}
if (leaves_only) {
printf("[VocabMatch] Scoring with leaves only\n");
} else {
printf("[VocabMatch] Scoring with all nodes\n");
}
printf("[VocabMatch] Using tree %s\n", tree_in);
printf("[VocabMatch] min_feature_scale = %0.3f\n", min_feature_scale);
printf("[VocabMatch] max_keys = %d\n", max_keys);
switch (distance_type) {
case DistanceDot:
printf("[VocabMatch] Using distance Dot\n");
break;
case DistanceMin:
printf("[VocabMatch] Using distance Min\n");
break;
default:
printf("[VocabMatch] Using no known distance!\n");
break;
}
/* Read the tree */
printf("[VocabMatch] Reading tree...\n");
fflush(stdout);
clock_t start = clock();
VocabTree tree;
tree.Read(tree_in);
clock_t end = clock();
printf("[VocabMatch] Read tree in %0.3fs\n",
(double) (end - start) / CLOCKS_PER_SEC);
#if 1
tree.Flatten();
#endif
tree.SetDistanceType(distance_type);
if (leaves_only) {
tree.SetInteriorNodeWeight(atoi(argv[6]) - 1, 0.0);
// #define CONSTANT_WEIGHTS
#ifdef CONSTANT_WEIGHTS
tree.SetConstantLeafWeights();
#endif
}
/* Read the database keyfiles */
FILE *f = fopen(db_in, "r");
std::vector<std::string> db_files;
char buf[256];
while (fgets(buf, 256, f)) {
/* Remove trailing newline */
if (buf[strlen(buf) - 1] == '\n')
buf[strlen(buf) - 1] = 0;
db_files.push_back(std::string(buf));
}
fclose(f);
/* Read the query keyfiles */
f = fopen(query_in, "r");
std::vector<std::string> query_files;
std::vector<int> query_indices;
while (fgets(buf, 256, f)) {
/* Remove trailing newline */
if (buf[strlen(buf) - 1] == '\n')
buf[strlen(buf) - 1] = 0;
char keyfile[256];
int index;
sscanf(buf, "%d %s", &index, keyfile);
query_files.push_back(std::string(keyfile));
query_indices.push_back(index);
}
fclose(f);
/* Populate the database */
printf("[VocabMatch] Populating database...\n");
fflush(stdout);
int num_db_images = db_files.size();
int num_query_images = query_files.size();
/* Now score each query keyfile */
printf("[VocabMatch] Scoring query images...\n");
fflush(stdout);
float *scores = new float[num_db_images];
double *scores_d = new double[num_db_images];
int *perm = new int[num_db_images];
FILE *f_match = fopen(matches_out, "w");
if (f_match == NULL) {
printf("[VocabMatch] Error opening file %s for writing\n",
matches_out);
return 1;
}
for (int i = 0; i < num_query_images; i++) {
int index_i = query_indices[i];
start = clock();
/* Clear scores */
for (int j = 0; j < num_db_images; j++)
scores[j] = 0.0;
unsigned char *keys;
int num_keys;
keys = ReadAndFilterKeys(query_files[i].c_str(), dim,
min_feature_scale, max_keys, num_keys);
tree.ScoreQueryKeys(num_keys, /*i,*/ true, keys, scores);
end = clock();
printf("[VocabMatch] Scored image %s (%d keys) in %0.3fs\n",
query_files[i].c_str(), num_keys,
(double) (end - start) / CLOCKS_PER_SEC);
#if 0
for (int j = 0; j < num_db_images; j++) {
/* Normalize scores */
if (magnitudes[j] > 0.0)
scores[j] /= magnitudes[j];
else
scores[j] = 0.0;
}
#endif
/* Find the top scores */
for (int j = 0; j < num_db_images; j++) {
scores_d[j] = (double) scores[j];
}
qsort_descending();
qsort_perm(num_db_images, scores_d, perm);
// assert(is_sorted(num_db_images, scores_d));
int top = MIN(num_nbrs+1, num_db_images);
for (int j = 0; j < top; j++) {
if (perm[j] == index_i)
continue;
fprintf(f_match, "%d %d %0.5e\n", index_i, perm[j], scores_d[j]);
fflush(f_match);
}
fflush(stdout);
delete [] keys;
}
fclose(f_match);
delete [] scores;
delete [] scores_d;
delete [] perm;
return 0;
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn_3D(int n, v3_t *right_pts, v3_t *left_pts, int scale_xform,
double *Tout) {
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double M[3][3] = { { 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, } };
double MT[3][3];
double MTM[3][3];
double eval[3], sqrteval_inv[3];
double evec[3][3], evec_tmp[3][3];
double Sinv[3][3], U[3][3];
double Tcenter[4][4] = { { 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 } };
double Ttmp[4][4];
double T[16], R[16];
double sum_num, sum_den, scale, RMS_sum;
int perm[3];
/* Compute the centroid of both point sets */
right_centroid = v3_mean(n, right_pts);
left_centroid = v3_mean(n, left_pts);
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num += v3_magsq(r);
sum_den += v3_magsq(l);
}
scale = sqrt(sum_num / sum_den);
/* Fill in the matrix M */
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[0][2] += Vx(r) * Vz(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
M[1][2] += Vy(r) * Vz(l);
M[2][0] += Vz(r) * Vx(l);
M[2][1] += Vz(r) * Vy(l);
M[2][2] += Vz(r) * Vz(l);
}
/* Compute MTM */
matrix_transpose(3, 3, (double *)M, (double *)MT);
matrix_product(3, 3, 3, 3, (double *)MT, (double *)M, (double *)MTM);
/* Calculate Sinv, the inverse of the square root of MTM */
dgeev_driver(3, (double *)MTM, (double *)evec, eval);
/* Sort the eigenvalues */
qsort_descending();
qsort_perm(3, eval, perm);
memcpy(evec_tmp[0], evec[perm[0]], sizeof(double) * 3);
memcpy(evec_tmp[1], evec[perm[1]], sizeof(double) * 3);
memcpy(evec_tmp[2], evec[perm[2]], sizeof(double) * 3);
memcpy(evec, evec_tmp, sizeof(double) * 9);
sqrteval_inv[0] = 1.0 / sqrt(eval[0]);
sqrteval_inv[1] = 1.0 / sqrt(eval[1]);
if (eval[2] < 1.0e-8 * eval[0]) {
sqrteval_inv[2] = 0.0;
} else {
sqrteval_inv[2] = 1.0 / sqrt(eval[2]);
}
Sinv[0][0] =
sqrteval_inv[0] * evec[0][0] * evec[0][0] +
sqrteval_inv[1] * evec[1][0] * evec[1][0] +
sqrteval_inv[2] * evec[2][0] * evec[2][0];
Sinv[0][1] =
sqrteval_inv[0] * evec[0][0] * evec[0][1] +
sqrteval_inv[1] * evec[1][0] * evec[1][1] +
sqrteval_inv[2] * evec[2][0] * evec[2][1];
Sinv[0][2] =
sqrteval_inv[0] * evec[0][0] * evec[0][2] +
sqrteval_inv[1] * evec[1][0] * evec[1][2] +
sqrteval_inv[2] * evec[2][0] * evec[2][2];
Sinv[1][0] =
sqrteval_inv[0] * evec[0][1] * evec[0][0] +
sqrteval_inv[1] * evec[1][1] * evec[1][0] +
sqrteval_inv[2] * evec[2][1] * evec[2][0];
Sinv[1][1] =
sqrteval_inv[0] * evec[0][1] * evec[0][1] +
sqrteval_inv[1] * evec[1][1] * evec[1][1] +
sqrteval_inv[2] * evec[2][1] * evec[2][1];
Sinv[1][2] =
sqrteval_inv[0] * evec[0][1] * evec[0][2] +
sqrteval_inv[1] * evec[1][1] * evec[1][2] +
sqrteval_inv[2] * evec[2][1] * evec[2][2];
Sinv[2][0] =
sqrteval_inv[0] * evec[0][2] * evec[0][0] +
sqrteval_inv[1] * evec[1][2] * evec[1][0] +
sqrteval_inv[2] * evec[2][2] * evec[2][0];
Sinv[2][1] =
sqrteval_inv[0] * evec[0][2] * evec[0][1] +
sqrteval_inv[1] * evec[1][2] * evec[1][1] +
sqrteval_inv[2] * evec[2][2] * evec[2][1];
Sinv[2][2] =
sqrteval_inv[0] * evec[0][2] * evec[0][2] +
sqrteval_inv[1] * evec[1][2] * evec[1][2] +
sqrteval_inv[2] * evec[2][2] * evec[2][2];
/* U = M * Sinv */
matrix_product(3, 3, 3, 3, (double *)M, (double *)Sinv, (double *)U);
if (eval[2] < 1.0e-8 * eval[0]) {
double u3u3[9], Utmp[9];
matrix_transpose_product2(3, 1, 3, 1, evec[2], evec[2], u3u3);
matrix_sum(3, 3, 3, 3, (double *) U, u3u3, Utmp);
if (matrix_determinant3(Utmp) < 0.0) {
printf("[align_horn_3D] Recomputing matrix...\n");
matrix_diff(3, 3, 3, 3, (double *) U, u3u3, Utmp);
}
memcpy(U, Utmp, 9 * sizeof(double));
}
/* Fill in the rotation matrix */
R[0] = U[0][0]; R[1] = U[0][1]; R[2] = U[0][2]; R[3] = 0.0;
R[4] = U[1][0]; R[5] = U[1][1]; R[6] = U[1][2]; R[7] = 0.0;
R[8] = U[2][0]; R[9] = U[2][1]; R[10] = U[2][2]; R[11] = 0.0;
R[12] = 0.0; R[13] = 0.0; R[14] = 0.0; R[15] = 1.0;
/* Fill in the translation matrix */
matrix_ident(4, T);
T[3] = Vx(right_centroid);
T[7] = Vy(right_centroid);
T[11] = Vz(right_centroid);
if (scale_xform == 0)
scale = 1.0;
Tcenter[0][0] = scale;
Tcenter[1][1] = scale;
Tcenter[2][2] = scale;
Tcenter[0][3] = -scale * Vx(left_centroid);
Tcenter[1][3] = -scale * Vy(left_centroid);
Tcenter[2][3] = -scale * Vz(left_centroid);
matrix_product(4, 4, 4, 4, T, R, (double *) Ttmp);
matrix_product(4, 4, 4, 4, (double *)Ttmp, (double *)Tcenter, Tout);
#if 0
T[2] = Vx(v3_sub(right_centroid, left_centroid));
T[5] = Vy(v3_sub(right_centroid, left_centroid));
T[8] = Vz(v3_sub(right_centroid, left_centroid));
#endif
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
double left[4] = { Vx(left_pts[i]),
Vy(left_pts[i]),
Vz(left_pts[i]), 1.0 };
double left_prime[3];
double dx, dy, dz;
matrix_product(4, 4, 4, 1, Tout, left, left_prime);
dx = left_prime[0] - Vx(right_pts[i]);
dy = left_prime[1] - Vy(right_pts[i]);
dz = left_prime[2] - Vz(right_pts[i]);
RMS_sum += dx * dx + dy * dy + dz * dz;
#if 0
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
v3_t resid;
/* Rotate, scale l */
v3_t Rl, SRl;
Vx(Rl) = R[0] * Vx(l) + R[1] * Vy(l) + R[2] * Vz(l);
Vy(Rl) = R[3] * Vx(l) + R[4] * Vy(l) + R[5] * Vz(l);
Vz(Rl) = R[6] * Vx(l) + R[7] * Vy(l) + R[8] * Vz(l);
SRl = v3_scale(scale, Rl);
resid = v3_sub(r, SRl);
RMS_sum += v3_magsq(resid);
#endif
}
return sqrt(RMS_sum / n);
}
void hsm_match(struct hsm_params*p, hsm_buffer b1, hsm_buffer b2) {
sm_log_push("hsm_match");
/* Let's measure the time */
clock_t hsm_match_start = clock();
assert(b1->num_angular_cells == b2->num_angular_cells);
assert(p->max_translation > 0);
assert(b1->linear_cell_size > 0);
b1->num_valid_results = 0;
/* Compute cross-correlation of spectra */
hsm_circular_cross_corr_stupid(b1->num_angular_cells, b2->hs, b1->hs, b1->hs_cross_corr);
/* Find peaks in cross-correlation */
int peaks[p->num_angular_hypotheses], npeaks;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->angular_hyp_min_distance_deg, 0, p->num_angular_hypotheses, peaks, &npeaks);
sm_debug("Found %d peaks (max %d) in cross correlation.\n", npeaks, p->num_angular_hypotheses);
if(npeaks == 0) {
sm_error("Cross correlation of spectra has 0 peaks.\n");
sm_log_pop();
return;
}
sm_log_push("loop on theta hypotheses");
/* lag e' quanto 2 si sposta a destra rispetto a 1 */
for(int np=0;np<npeaks;np++) {
int lag = peaks[np];
double theta_hypothesis = lag * (2*M_PI/b1->num_angular_cells);
sm_debug("Theta hyp#%d: lag %d, angle %fdeg\n", np, lag, rad2deg(theta_hypothesis));
/* Superimpose the two spectra */
double mult[b1->num_angular_cells];
for(int r=0;r<b1->num_angular_cells;r++)
mult[r] = b1->hs[r] * b2->hs[pos_mod(r-lag, b1->num_angular_cells)];
/* Find directions where both are intense */
int directions[p->xc_ndirections], ndirections;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->xc_directions_min_distance_deg, 1, p->xc_ndirections, directions, &ndirections);
if(ndirections<2) {
sm_error("Too few directions.\n");
}
struct {
/* Direction of cross correlation */
double angle;
int nhypotheses;
struct {
double delta;
double value;
} hypotheses[p->linear_xc_max_npeaks];
} dirs[ndirections];
sm_debug("Using %d (max %d) correlations directions.\n", ndirections, p->xc_ndirections);
int max_lag = (int) ceil(p->max_translation / b1->linear_cell_size);
int min_lag = -max_lag;
sm_debug("Max lag: %d cells (max t: %f, cell size: %f)\n",
max_lag, p->max_translation, b1->linear_cell_size);
sm_log_push("loop on xc direction");
/* For each correlation direction */
for(int cd=0;cd<ndirections;cd++) {
dirs[cd].angle = theta_hypothesis + (directions[cd]) * (2*M_PI/b1->num_angular_cells);
printf(" cd %d angle = %d deg\n", cd, (int) rad2deg(dirs[cd].angle));
/* Do correlation */
int lags [2*max_lag + 1];
double xcorr [2*max_lag + 1];
int i1 = pos_mod(directions[cd] , b1->num_angular_cells);
int i2 = pos_mod(directions[cd] + lag , b1->num_angular_cells);
double *f1 = b1->ht[i1];
double *f2 = b2->ht[i2];
hsm_linear_cross_corr_stupid(
b2->num_linear_cells,f2,
b1->num_linear_cells,f1,
xcorr, lags, min_lag, max_lag);
/* Find peaks of cross-correlation */
int linear_peaks[p->linear_xc_max_npeaks], linear_npeaks;
hsm_find_peaks_linear(
2*max_lag + 1, xcorr, p->linear_xc_peaks_min_distance/b1->linear_cell_size,
p->linear_xc_max_npeaks, linear_peaks, &linear_npeaks);
sm_debug("theta hyp #%d: Found %d (max %d) peaks for correlation.\n",
cd, linear_npeaks, p->linear_xc_max_npeaks);
dirs[cd].nhypotheses = linear_npeaks;
sm_log_push("Considering each peak of linear xc");
for(int lp=0;lp<linear_npeaks;lp++) {
int linear_xc_lag = lags[linear_peaks[lp]];
double value = xcorr[linear_peaks[lp]];
double linear_xc_lag_m = linear_xc_lag * b1->linear_cell_size;
sm_debug("lag: %d delta: %f value: %f \n", linear_xc_lag, linear_xc_lag_m, value);
dirs[cd].hypotheses[lp].delta = linear_xc_lag_m;
dirs[cd].hypotheses[lp].value = value;
}
sm_log_pop();
if(p->debug_true_x_valid) {
double true_delta = cos(dirs[cd].angle) * p->debug_true_x[0] +
sin(dirs[cd].angle) * p->debug_true_x[1];
sm_debug("true_x delta = %f \n", true_delta );
}
} /* xc direction */
sm_log_pop();
sm_debug("Now doing all combinations. How many are there?\n");
int possible_choices[ndirections];
int num_combinations = 1;
for(int cd=0;cd<ndirections;cd++) {
possible_choices[cd] = dirs[cd].nhypotheses;
num_combinations *= dirs[cd].nhypotheses;
}
sm_debug("Total: %d combinations\n", num_combinations);
sm_log_push("For each combination..");
for(int comb=0;comb<num_combinations;comb++) {
int choices[ndirections];
hsm_generate_combinations(ndirections, possible_choices, comb, choices);
/* Linear least squares */
double M[2][2]={{0,0},{0,0}}; double Z[2]={0,0};
/* heuristic quality value */
double sum_values = 0;
for(int cd=0;cd<ndirections;cd++) {
double angle = dirs[cd].angle;
double c = cos(angle), s = sin(angle);
double w = dirs[cd].hypotheses[choices[cd]].value;
double y = dirs[cd].hypotheses[choices[cd]].delta;
M[0][0] += c * c * w;
M[1][0] += c * s * w;
M[0][1] += c * s * w;
M[1][1] += s * s * w;
Z[0] += w * c * y;
Z[1] += w * s * y;
sum_values += w;
}
double det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
double Minv[2][2];
Minv[0][0] = M[1][1] * (1/det);
Minv[1][1] = M[0][0] * (1/det);
Minv[0][1] = -M[0][1] * (1/det);
Minv[1][0] = -M[1][0] * (1/det);
double t[2] = {
Minv[0][0]*Z[0] + Minv[0][1]*Z[1],
Minv[1][0]*Z[0] + Minv[1][1]*Z[1]};
/* copy result in results slot */
int k = b1->num_valid_results;
b1->results[k][0] = t[0];
b1->results[k][1] = t[1];
b1->results[k][2] = theta_hypothesis;
b1->results_quality[k] = sum_values;
b1->num_valid_results++;
}
sm_log_pop();
} /* theta hypothesis */
sm_log_pop();
/* for(int i=0;i<b1->num_valid_results;i++) {
printf("#%d %.0fdeg %.1fm %.1fm quality %f \n",i,
rad2deg(b1->results[i][2]),
b1->results[i][0],
b1->results[i][1],
b1->results_quality[i]);
}*/
/* Sorting based on values */
int indexes[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++)
indexes[i] = i;
qsort_descending(indexes, (size_t) b1->num_valid_results, b1->results_quality);
/* copy in the correct order*/
double*results_tmp[b1->num_valid_results];
double results_quality_tmp[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++) {
results_tmp[i] = b1->results[i];
results_quality_tmp[i] = b1->results_quality[i];
}
for(int i=0;i<b1->num_valid_results;i++) {
b1->results[i] = results_tmp[indexes[i]];
b1->results_quality[i] = results_quality_tmp[indexes[i]];
}
for(int i=0;i<b1->num_valid_results;i++) {
char near[256]="";
double *x = b1->results[i];
if(p->debug_true_x_valid) {
double err_th = rad2deg(fabs(angleDiff(p->debug_true_x[2],x[2])));
double err_m = hypot(p->debug_true_x[0]-x[0],
p->debug_true_x[1]-x[1]);
const char * ast = (i == 0) && (err_th > 2) ? " ***** " : "";
sprintf(near, "th err %4d err_m %5f %s",(int)err_th ,err_m,ast);
}
if(i<10)
printf("after #%d %3.1fm %.1fm %3.0fdeg quality %5.0f \t%s\n",i,
x[0],
x[1], rad2deg(x[2]), b1->results_quality[i], near);
}
/* How long did it take? */
clock_t hsm_match_stop = clock();
int ticks = hsm_match_stop-hsm_match_start;
double ctime = ((double)ticks) / CLOCKS_PER_SEC;
sm_debug("Time: %f sec (%d ticks)\n", ctime, ticks);
sm_log_pop();
}
