Skip to content

Commit

Permalink
more print statements and sub-tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexlib
alexlib committed Dec 7, 2023
1 parent 6b0d941 commit 0bd6812
Show file tree
Hide file tree
Showing 17 changed files with 573 additions and 148 deletions.
16 changes: 15 additions & 1 deletion liboptv/src/correspondences.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,7 @@ int safely_allocate_adjacency_lists(correspond* lists[4][4], int num_cams,
Arguments:
correspond *list[4][4] - the pairwise adjacency lists.
int base_target_count - number of turgets in the base camera (first camera)
int base_target_count - number of targets in the base camera (first camera)
double accept_corr - minimal correspondence grade for acceptance.
n_tupel *scratch - scratch buffer to fill with candidate clique data.
int scratch_size - size of the scratch space. Upon reaching it, the search
Expand Down Expand Up @@ -300,6 +300,7 @@ int four_camera_matching(correspond *list[4][4], int base_target_count,
if (p4 != p41) continue;
for (o = 0; o < list[2][3][p3].n; o++) {
p42 = list[2][3][p3].p2[o];
// printf(" p42 %d p4 %d\n", p42, p4);
if (p4 != p42) continue;

corr = (list[0][1][i].corr[j]
Expand All @@ -315,6 +316,7 @@ int four_camera_matching(correspond *list[4][4], int base_target_count,
+ list[1][3][p2].dist[n]
+ list[2][3][p3].dist[o]);

// printf("corr %f\n", corr);
if (corr <= accept_corr)
continue;

Expand All @@ -326,6 +328,9 @@ int four_camera_matching(correspond *list[4][4], int base_target_count,
scratch[matched].corr = corr;

matched++;

// printf("matched %d [%d %d %d %d]\n", matched, p1, p2, p3, p4);

if (matched == scratch_size) {
printf ("Overflow in correspondences.\n");
return matched;
Expand Down Expand Up @@ -371,18 +376,23 @@ int three_camera_matching(correspond *list[4][4], int num_cams,
for (i2 = i1 + 1; i2 < num_cams - 1; i2++) {
p1 = list[i1][i2][i].p1;
if (p1 > nmax || tusage[i1][p1] > 0) continue;
// printf("p1 %d candidates = %d\n", p1, list[i1][i2][i].n);

for (j = 0; j < list[i1][i2][i].n; j++) {
p2 = list[i1][i2][i].p2[j];
if (p2 > nmax || tusage[i2][p2] > 0) continue;
// printf("p2 %d\n", p2);

for (i3 = i2 + 1; i3 < num_cams; i3++)
for (k = 0; k < list[i1][i3][i].n; k++) {
p3 = list[i1][i3][i].p2[k];
if (p3 > nmax || tusage[i3][p3] > 0) continue;
// printf("p3 %d\n", p3);

for (m = 0; m < list[i2][i3][p2].n; m++) {
if (p3 != list[i2][i3][p2].p2[m]) continue;

// printf("p3 equal to lists %d = %d\n", p3,list[i2][i3][p2].p2[m]);

/* accept as preliminary match */
corr = (list[i1][i2][i].corr[j]
Expand All @@ -391,6 +401,8 @@ int three_camera_matching(correspond *list[4][4], int num_cams,
/ (list[i1][i2][i].dist[j]
+ list[i1][i3][i].dist[k]
+ list[i2][i3][p2].dist[m]);

// printf("corr %f\n", corr);

if (corr <= accept_corr)
continue;
Expand All @@ -405,6 +417,8 @@ int three_camera_matching(correspond *list[4][4], int num_cams,
scratch[matched].corr = corr;

matched++;
printf("matched %d [%d %d %d]\n", matched, p1, p2, p3);

if (matched == scratch_size) {
printf ("Overflow in correspondences.\n");
return matched;
Expand Down
10 changes: 5 additions & 5 deletions liboptv/src/epi.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,12 +177,12 @@ int find_candidate (coord_2d *crd, target *pix, int num,
j0 -= dj;
}

printf("j0 before : %d\n", j0);
// printf("j0 before : %d\n", j0);
/* due to truncation error we might shift to smaller x */
j0 -= 12;
if (j0 < 0) j0 = 0;

printf("j0: %d\n", j0);
// printf("j0: %d\n", j0);

for (j = j0; j < num; j++) { /* candidate search */

Expand All @@ -205,10 +205,10 @@ int find_candidate (coord_2d *crd, target *pix, int num,

p2 = crd[j].pnr;

printf("j: %d p2: %d %8.6f %8.6f\n", j, p2, d, tol_band_width);
// printf("j: %d p2: %d %8.6f %8.6f\n", j, p2, d, tol_band_width);

if (p2 >= num) {
printf("pnr out of range: %d\n", p2);
// printf("pnr out of range: %d\n", p2);
return -1;
}

Expand Down Expand Up @@ -241,7 +241,7 @@ int find_candidate (coord_2d *crd, target *pix, int num,
cand[count].corr = corr;
count++;

printf("cand[%d].pnr:%d %f %f \n", count-1, cand[count-1].pnr, cand[count-1].tol, cand[count-1].corr);
// printf("cand[%d].pnr:%d %f %f \n", count-1, cand[count-1].pnr, cand[count-1].tol, cand[count-1].corr);
}
return count;
}
Expand Down
17 changes: 16 additions & 1 deletion liboptv/src/imgcoord.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,21 +39,35 @@ void flat_image_coord (vec3d orig_pos, Calibration *cal, mm_np *mm,
*/
trans_Cam_Point(cal->ext_par, *mm, cal->glass_par, orig_pos, \
&(cal_t.ext_par), pos_t, cross_p, cross_c);
multimed_nlay (&cal_t, mm, pos_t, &X_t,&Y_t);

// printf("pos_t: %f, %f, %f\n", pos_t[0], pos_t[1], pos_t[2]);
// printf("cross_p: %f, %f, %f\n", cross_p[0], cross_p[1], cross_p[2]);
// printf("cross_c: %f, %f, %f\n", cross_c[0], cross_c[1], cross_c[2]);

multimed_nlay (&cal_t, mm, pos_t, &X_t, &Y_t);
// printf("X_t: %f, Y_t: %f\n", X_t, Y_t);

vec_set(pos_t,X_t,Y_t,pos_t[2]);

back_trans_Point(pos_t, *mm, cal->glass_par, cross_p, cross_c, pos);

// printf("pos: %f, %f, %f\n", pos[0], pos[1], pos[2]);

deno = cal->ext_par.dm[0][2] * (pos[0]-cal->ext_par.x0)
+ cal->ext_par.dm[1][2] * (pos[1]-cal->ext_par.y0)
+ cal->ext_par.dm[2][2] * (pos[2]-cal->ext_par.z0);

// printf("deno: %f\n", deno);

*x = - cal->int_par.cc * (cal->ext_par.dm[0][0] * (pos[0]-cal->ext_par.x0)
+ cal->ext_par.dm[1][0] * (pos[1]-cal->ext_par.y0)
+ cal->ext_par.dm[2][0] * (pos[2]-cal->ext_par.z0)) / deno;

*y = - cal->int_par.cc * (cal->ext_par.dm[0][1] * (pos[0]-cal->ext_par.x0)
+ cal->ext_par.dm[1][1] * (pos[1]-cal->ext_par.y0)
+ cal->ext_par.dm[2][1] * (pos[2]-cal->ext_par.z0)) / deno;

// printf("x: %f, y: %f\n", *x, *y);
}

/* img_coord() uses flat_image_coord() to estimate metric coordinates in image space
Expand All @@ -70,6 +84,7 @@ void flat_image_coord (vec3d orig_pos, Calibration *cal, mm_np *mm,
*/
void img_coord (vec3d pos, Calibration *cal, mm_np *mm, double *x, double *y) {
flat_image_coord (pos, cal, mm, x, y);
// printf("x: %f, y: %f\n", *x, *y);
flat_to_dist(*x, *y, cal, x, y);
}

12 changes: 7 additions & 5 deletions liboptv/src/lsqadj.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,13 @@
*
* Arguments:
* a - matrix of doubles of the size (m x n_large).
* ata - matrix of the result multiply(a.T,a) of size (n x n)
* at - matrix of the result multiply(a.T,a) of size (n x n)
* m - number of rows in matrix a
* n - number of rows and columns in the output ata - the size of the sub-matrix
* n_large - number of columns in matrix a
*/

void ata (double *a, double *ata, int m, int n, int n_large ) {
void ata (double *a, double *at, int m, int n, int n_large ) {
/* matrix a and result matrix ata = at a
a is m * n, ata is n * n */

Expand All @@ -29,9 +29,11 @@ void ata (double *a, double *ata, int m, int n, int n_large ) {
{
for (j = 0; j < n; j++)
{
*(ata+i*n_large+j) = 0.0;
for (k = 0; k < m; k++)
*(ata+i*n_large+j) += *(a+k*n_large+i) * *(a+k*n_large+j);
// *(at+i*n_large+j) = 0.0; //I think it's a serious bug, AL
*(at+i*n+j) = 0.0;
for (k = 0; k < m; k++){
*(at+i*n+j) += *(a+k*n_large+i) * *(a+k*n_large+j);
}
}
}
}
Expand Down
9 changes: 7 additions & 2 deletions liboptv/src/multimed.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,11 @@ void multimed_nlay (Calibration *cal, mm_np *mm, vec3d pos,
double radial_shift;

radial_shift = multimed_r_nlay (cal, mm, pos);
// printf("radial shift is %f\n", radial_shift);
// printf("pos is %f, %f, %f\n", pos[0], pos[1], pos[2]);
// printf("radial shift is %f\n", radial_shift);
// if (radial_shift == NAN || radial_shift == 0 || radial_shift == 1.0){
// printf(" Error in multimed_r_nlay\n");
// }

/* if radial_shift == 1.0, this degenerates to Xq = X, Yq = Y */
*Xq = cal->ext_par.x0 + (pos[0] - cal->ext_par.x0) * radial_shift;
Expand Down Expand Up @@ -85,6 +89,7 @@ double multimed_r_nlay (Calibration *cal, mm_np *mm, vec3d pos) {
zout += mm->d[i];

r = norm((X - cal->ext_par.x0), (Y - cal->ext_par.y0), 0);
// printf("X=%f, Y=%f, Z=%f\n", X, Y, Z);
// printf("r is %f\n", r);
rq = r;

Expand Down Expand Up @@ -333,7 +338,7 @@ void init_mmlut (volume_par *vpar, control_par *cpar, Calibration *cal) {

/* round values (-> enlarge) */
Rmax += (rw - fmod (Rmax, rw));
// printf("inside init_mmlut: Rmax=%f, Zmin_t=%f, Zmax_t=%f\n", Rmax, Zmin_t, Zmax_t);
printf("inside init_mmlut: Rmax=%f, Zmin_t=%f, Zmax_t=%f\n", Rmax, Zmin_t, Zmax_t);

/* get # of rasterlines in r,z */
nr = (int)(Rmax/rw + 1);
Expand Down
56 changes: 49 additions & 7 deletions liboptv/src/orientation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,7 +268,7 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
Calibration *cal;

/* small perturbation for translation/rotation in meters and in radians */
double dm = 0.00001, drad = 0.0000001;
double dm = 0.000000000001, drad = 0.000000000001;

cal = malloc (sizeof (Calibration));
memcpy(cal, cal_in, sizeof (Calibration));
Expand Down Expand Up @@ -302,6 +302,7 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
numbers = 16;
}


vec_set(glass_dir,
cal->glass_par.vec_x, cal->glass_par.vec_y, cal->glass_par.vec_z);
nGl = vec_norm(glass_dir);
Expand Down Expand Up @@ -344,6 +345,7 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],

itnum = 0;
stopflag = 0;

while ((stopflag == 0) && (itnum < NUM_ITER)) {
itnum++;

Expand All @@ -361,22 +363,32 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
}

/* get metric flat-image coordinates of the detected point */
// printf("itnum = %d, pnr = %d i = %d, pix[i].x = %f, pix[i].y = %f\n", itnum, pix[i].pnr, i, pix[i].x, pix[i].y);
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
// printf("xc = %f, yc = %f\n", xc, yc);
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
// printf("after brown affin xc = %f, yc = %f\n", xc, yc);

/* Projected 2D position on sensor of corresponding known point */
rotation_matrix(&(cal->ext_par));
// printf("fix[i].x = %f, fix[i].y = %f, fix[i].z = %f\n", fix[i][0], fix[i][1], fix[i][2]);

img_coord (fix[i], cal, cpar->mm, &xp, &yp);

printf("xp = %f, yp = %f\n", xp, yp);

/* derivatives of distortion parameters */

r = sqrt (xp*xp + yp*yp);
printf("r = %f\n", r);
printf("cal->added_par.scx = %f\n", cal->added_par.scx);
printf("cal->added_par.she = %f\n", cal->added_par.she);

X[n][7] = cal->added_par.scx;
X[n+1][7] = sin(cal->added_par.she);

X[n][8] = 0;
X[n+1][8] = 1;
X[n][8] = 0.0;
X[n+1][8] = 1.0;

X[n][9] = cal->added_par.scx * xp * r*r;
X[n+1][9] = yp * r*r;
Expand Down Expand Up @@ -501,24 +513,50 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
sumP = 0;
for (i = 0; i < n_obs; i++) { /* homogenize */
p = sqrt (P[i]);
// printf("p = %f\n", p);
for (j = 0; j < NPAR; j++)
Xh[i][j] = p * X[i][j];
// printf("Xh[%d][%d] = %f\n", i, j, Xh[i][j]);

yh[i] = p * y[i];
// printf("yh[%d] = %f\n", i, yh[i]);
sumP += P[i];
}

/* Gauss Markoff Model it is the least square adjustment
of the redundant information contained both in the spatial
intersection and the resection, see [1], eq. 23 */
intersection and the resection, see [1], eq. 23 */

for (i = 0; i < n_obs; i++) { /* homogenize */
for (j = 0; j < NPAR; j++){
printf("Xh[%d][%d] = %f\n", i, j, Xh[i][j]);
}
}

ata ((double *) Xh, (double *) XPX, n_obs, numbers, NPAR );

for (i = 0; i < n_obs; i++) { /* homogenize */
for (j = 0; j < NPAR; j++){
printf("XPX[%d][%d] = %f\n", i, j, XPX[i][j]);
}
}


matinv ((double *) XPX, numbers, NPAR);
// for (i = 0; i < n_obs; i++) { /* homogenize */
// for (j = 0; j < NPAR; j++){
// printf("XPX[%d][%d] = %f\n", i, j, XPX[i][j]);
// }
// }


atl ((double *) XPy, (double *) Xh, yh, n_obs, numbers, NPAR);
matmul ((double *) beta, (double *) XPX, (double *) XPy,
numbers, numbers,1, NPAR, NPAR);
numbers, numbers, 1, NPAR, NPAR);

stopflag = 1;
for (i = 0; i < numbers; i++) {
printf("beta[%d] = %f\n", i, beta[i]);
if (fabs (beta[i]) > CONVERGENCE) stopflag = 0;
}

Expand Down Expand Up @@ -572,6 +610,7 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],

for (i = 0; i < numbers; i++) {
sigmabeta[i] = sigmabeta[NPAR] * sqrt(XPX[i][i]);
printf("sigmabeta[%d] = %f\n", i, sigmabeta[i]);
}

free(X);
Expand Down Expand Up @@ -614,7 +653,7 @@ double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
int raw_orient (Calibration* cal, control_par *cpar, int nfix, vec3d fix[], target pix[])
{
double X[10][6], y[10], XPX[6][6], XPy[6], beta[6];
int i, j, n, itnum, stopflag;
int i, j, n=0, itnum, stopflag;
double dm = 0.0001, drad = 0.0001;
double xp, yp, xc, yc;
vec3d pos;
Expand Down Expand Up @@ -644,7 +683,7 @@ int raw_orient (Calibration* cal, control_par *cpar, int nfix, vec3d fix[], targ
while ((stopflag == 0) && (itnum < 20)) {
++itnum;

for (i = 0, n = 0; i < nfix; i++) {
for (i = 0; i < nfix; i++) {
/* we do not check the order - trust the user to click the points
in the correct order of appearance in man_ori and in the calibration
parameters GUI
Expand All @@ -661,12 +700,15 @@ int raw_orient (Calibration* cal, control_par *cpar, int nfix, vec3d fix[], targ

/* numeric derivatives of internal camera coefficients */
num_deriv_exterior(cal, cpar, dm, drad, pos, X[n], X[n + 1]);
printf("num_deriv_exterior: i=%d n= %d %f %f \n", i, n, *X[n], *X[n+1]);

y[n] = xc - xp;
y[n+1] = yc - yp;

n += 2;
}



/* Gauss Markoff Model */

Expand Down
Loading

0 comments on commit 0bd6812

Please sign in to comment.