END_TEST
START_TEST(test_pixel_to_metric)
{
/* input */
double xc = 0.0; // [mm]
double yc = 0.0; // [mm]
control_par cpar;
/* output */
double xp, yp;
cpar.imx = 1024;
cpar.imy = 1008;
cpar.pix_x = 0.01;
cpar.pix_y = 0.01;
cpar.chfield = 0;
/* compare the xc, yc to the original */
double xc1, yc1;
metric_to_pixel (&xp, &yp, xc, yc, &cpar);
pixel_to_metric (&xc1, &yc1, xp, yp, &cpar);
ck_assert_msg( fabs(xc1 - xc) < EPS &&
fabs(yc1 - yc) < EPS,
"Expected %f, %f but got %f %f\n", xc, yc, xc1, yc1);
xc = 1.0;
yc = 0.0;
metric_to_pixel (&xp, &yp, xc, yc, &cpar);
pixel_to_metric (&xc1, &yc1, xp, yp, &cpar);
ck_assert_msg( fabs(xc1 - xc) < EPS &&
fabs(yc1 - yc) < EPS,
"Expected %f, %f, but got %f %f\n", xc,yc,xc1, yc1);
xc = 0.0;
yc = -1.0;
metric_to_pixel (&xp, &yp, xc, yc, &cpar);
pixel_to_metric (&xc1, &yc1, xp, yp, &cpar);
ck_assert_msg( fabs(xc1 - xc) < EPS &&
fabs(yc1 - yc) < EPS,
"Expected %f, %f, but got %f %f\n", xc,yc,xc1, yc1);
}
void prepare_eval (control_par *cpar, int *n_fix) {
int i_img, i, filenumber, step_shake, count = 0;
double dummy;
sequence_par *seq_par;
FILE *fpp;
int part_pointer; /* Holds index of particle later */
frame frm;
frame_init(&frm, cpar->num_cams, MAX_TARGETS);
seq_par = read_sequence_par("parameters/sequence.par", cpar->num_cams);
fpp = fopen ("parameters/dumbbell.par", "r");
if (fpp){
fscanf (fpp, "%lf", &dummy);
fscanf (fpp, "%lf", &dummy);
fscanf (fpp, "%lf", &dummy);
fscanf (fpp, "%lf", &dummy);
fscanf (fpp, "%d", &step_shake);
fclose (fpp);
}
for (filenumber = seq_par->first; filenumber <= seq_par->last; \
filenumber += step_shake)
{
read_frame(&frm, "res/db_is", NULL, NULL, seq_par->img_base_name,
filenumber);
for (i = 0; i < frm.num_parts; i++) {
for (i_img = 0; i_img < cpar->num_cams; i_img++) {
part_pointer = frm.correspond[i].p[i_img];
if (part_pointer != CORRES_NONE) {
pix[i_img][count].x = frm.targets[i_img][part_pointer].x;
pix[i_img][count].y = frm.targets[i_img][part_pointer].y;
} else {
pix[i_img][count].x = -999;
pix[i_img][count].y = -999;
}
if(pix[i_img][count].x>-999 && pix[i_img][count].y>-999){
pixel_to_metric (&crd[i_img][count].x, &crd[i_img][count].y,
pix[i_img][count].x, pix[i_img][count].y, cpar);
}
else{
crd[i_img][count].x=-1e10;
crd[i_img][count].y=-1e10;
}
crd[i_img][count].pnr=count;
}
count ++;
}
}
free_frame(&frm);
nfix=count;
}
/* asses_new_position() determines the nearest target on each camera around a
* search position and prepares the data structures accordingly with the
* determined target info or the unused flag value.
*
* Arguments:
* vec3d pos - the position around which to search.
* vec2d targ_pos[] - the determined targets' respective positions.
* int cand_inds[][MAX_CANDS] - output buffer, the determined targets' index in
* the respective camera's target list
* frame *frm - the frame holdin target data for the search position.
* tracking_run *run - scene information struct.
*
* Returns:
* the number of cameras where a suitable target was found.
*/
int assess_new_position(vec3d pos, vec2d targ_pos[],
int cand_inds[][MAX_CANDS], frame *frm, tracking_run *run)
{
int cam, num_cands, valid_cams, _ix;
vec2d pixel;
double right, left, down, up; /* search rectangle limits */
left = right = up = down = ADD_PART;
for (cam = 0; cam < run->cpar->num_cams; cam++) {
point_to_pixel(pixel, pos, run->cal[cam], run->cpar);
targ_pos[cam][0] = targ_pos[cam][1] = COORD_UNUSED;
/* here we shall use only the 1st neigbhour */
num_cands = candsearch_in_pix (frm->targets[cam], frm->num_targets[cam],
pixel[0], pixel[1], left, right, up, down,
cand_inds[cam], run->cpar);
if (num_cands > 0) {
_ix = cand_inds[cam][0]; // first nearest neighbour
targ_pos[cam][0] = frm->targets[cam][_ix].x;
targ_pos[cam][1] = frm->targets[cam][_ix].y;
}
}
valid_cams = 0;
for (cam = 0; cam < run->cpar->num_cams; cam++) {
if ((targ_pos[cam][0] != COORD_UNUSED) && \
(targ_pos[cam][1] != COORD_UNUSED))
{
pixel_to_metric(&(targ_pos[cam][0]), &(targ_pos[cam][1]),
targ_pos[cam][0], targ_pos[cam][1], run->cpar);
dist_to_flat(targ_pos[cam][0], targ_pos[cam][1], run->cal[cam],
&(targ_pos[cam][0]), &(targ_pos[cam][1]), run->flatten_tol);
valid_cams++;
}
}
return valid_cams;
}
/* This is very similar to prepare_eval, but it is sufficiently different in
small but devious ways, not only parameters, that for now it'll be a
different function. */
void prepare_eval_shake(control_par *cpar) {
FILE *fpp;
char* target_file_base[4];
int i_img, i, filenumber, step_shake, count = 0, frame_count = 0;
int seq_first, seq_last;
int frame_used, part_used;
int max_shake_points, max_shake_frames;
double dummy;
sequence_par *seq_par;
int part_pointer; /* Holds index of particle later */
frame frm;
frame_init(&frm, cpar->num_cams, MAX_TARGETS);
seq_par = read_sequence_par("parameters/sequence.par", cpar->num_cams);
fpp = fopen ("parameters/shaking.par", "r");
fscanf (fpp,"%d\n", &seq_first);
fscanf (fpp,"%d\n", &seq_last);
fscanf (fpp,"%d\n", &max_shake_points);
fscanf (fpp,"%d\n", &max_shake_frames);
fclose (fpp);
step_shake = (int)((double)(seq_last - seq_first + 1) /
(double)max_shake_frames + 0.5);
printf ("shake step is %d\n", step_shake);
for (filenumber = seq_first + 2; filenumber < seq_last - 1; \
filenumber += step_shake)
{
frame_used = 0;
read_frame(&frm, "res/rt_is", "res/ptv_is", NULL,
seq_par->img_base_name, filenumber);
printf("read frame %d\n", filenumber);
printf("found %d particles \n", frm.num_parts);
for (i = 0; i < frm.num_parts; i++) {
part_used = 0;
for (i_img = 0; i_img < cpar->num_cams; i_img++) {
part_pointer = frm.correspond[i].p[i_img];
if ((part_pointer != CORRES_NONE) && \
(frm.path_info[i].prev != PREV_NONE) && \
(frm.path_info[i].next != NEXT_NONE) )
{
pix[i_img][count].x = frm.targets[i_img][part_pointer].x;
pix[i_img][count].y = frm.targets[i_img][part_pointer].y;
pix[i_img][count].pnr = count;
pixel_to_metric(&crd[i_img][count].x, &crd[i_img][count].y,
pix[i_img][count].x, pix[i_img][count].y, cpar);
crd[i_img][count].pnr = count;
frame_used = 1;
part_used = 1;
}
}
if (part_used == 1) count++;
if (count >= max_shake_points) break;
}
if (frame_used == 1) frame_count++;
if ((count >= max_shake_points) || (frame_count > max_shake_frames))
break;
}
free_frame(&frm);
nfix = count;
printf("End of prepare_eval_shake, nfix = %d \n", nfix);
}
/* track backwards */
void trackback_c ()
{
char buf[256];
int i, j, h, k, step, invol=0;
int zaehler1, philf[4][MAX_CANDS];
int count1=0, count2=0, zusatz=0;
int quali=0;
double x2[4], y2[4], angle, acc, lmax, dl;
double xr[4], xl[4], yd[4], yu[4];
vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */
double xn[4], yn[4];
double rr, Ymin=0, Ymax=0;
double npart=0, nlinks=0;
foundpix *w, p16[4*MAX_CANDS];
sequence_par *seq_par;
track_par *tpar;
volume_par *vpar;
control_par *cpar;
framebuf *fb;
/* Shortcuts to inside current frame */
P *curr_path_inf, *ref_path_inf;
corres *ref_corres;
target **ref_targets;
int _ix; /* For use in any of the complex index expressions below */
int _frame_parts; /* number of particles in a frame */
display = 1;
/* read data */
seq_par = read_sequence_par("parameters/sequence.par", 4);
tpar = read_track_par("parameters/track.par");
vpar = read_volume_par("parameters/criteria.par");
cpar = read_control_par("parameters/ptv.par");
fb = (framebuf *) malloc(sizeof(framebuf));
fb_init(fb, 4, cpar->num_cams, MAX_TARGETS,
"res/rt_is", "res/ptv_is", "res/added", seq_par->img_base_name);
/* Prime the buffer with first frames */
for (step = seq_par->last; step > seq_par->last - 4; step--) {
fb_read_frame_at_end(fb, step, 1);
fb_next(fb);
}
fb_prev(fb);
lmax = norm((tpar->dvxmin - tpar->dvxmax), (tpar->dvymin - tpar->dvymax),
(tpar->dvzmin - tpar->dvzmax));
volumedimension (glob_cal, &(vpar->X_lay[1]), &(vpar->X_lay[0]), &Ymax,
&Ymin, &(vpar->Zmax_lay[1]), &(vpar->Zmin_lay[0]), cpar);
/* sequence loop */
for (step = seq_par->last - 1; step > seq_par->first; step--) {
sprintf (buf, "Time step: %d, seqnr: %d, Particle info:",
step - seq_par->first, step);
for (h = 0; h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
/* We try to find link only if the forward search failed to. */
if ((curr_path_inf->next < 0) || (curr_path_inf->prev != -1)) continue;
for (j = 0; j < 7; j++) vec_init(X[j]);
curr_path_inf->inlist = 0;
reset_foundpix_array(p16, 16, fb->num_cams);
/* 3D-position of current particle */
vec_copy(X[1], curr_path_inf->x);
/* use information from previous to locate new search position
and to calculate values for search area */
ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->next]);
vec_copy(X[0], ref_path_inf->x);
search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]);
for (j=0; j < fb->num_cams; j++) {
img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j],
*(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar);
}
/* calculate searchquader and reprojection in image space */
searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar);
for (j = 0; j < fb->num_cams; j++) {
zaehler1 = candsearch_in_pix (
fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j],
xl[j], xr[j], yu[j], yd[j], philf[j], cpar);
for(k=0; k<4; k++) {
if( zaehler1>0) {
if (philf[j][k] == -999){
p16[j*4+k].ftnr=-1;
} else {
p16[j*4+k].ftnr=fb->buf[3]->targets[j][philf[j][k]].tnr;
p16[j*4+k].whichcam[j]=1;
}
}
}
}
/* search in pix for candidates in next time step */
//for (j = 0; j < fb->num_cams; j++) {
// register_closest_neighbs(fb->buf[2]->targets[j],
// fb->buf[2]->num_targets[j], j, xn[j], yn[j],
// xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS]);
//}
/* fill and sort candidate struct */
sortwhatfound(p16, &zaehler1, fb->num_cams);
w = (foundpix *) calloc (zaehler1, sizeof (foundpix));
/*end of candidate struct */
if (zaehler1 > 0) count2++;
copy_foundpix_array(w, p16, zaehler1, fb->num_cams);
for (i = 0; i < zaehler1; i++) {
ref_path_inf = &(fb->buf[2]->path_info[w[i].ftnr]);
vec_copy(X[3], ref_path_inf->x);
vec_subt(X[1], X[3], diff_pos);
if (pos3d_in_bounds(diff_pos, tpar)) {
angle_acc(X[1], X[2], X[3], &angle, &acc);
/* *********************check link *****************************/
if ((acc < tpar->dacc && angle < tpar->dangle) || \
(acc < tpar->dacc/10))
{
dl = (vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[0], X[1]) )/2;
quali=w[i].freq;
rr = (dl/lmax + acc/tpar->dacc + angle/tpar->dangle)/quali;
register_link_candidate(curr_path_inf, rr, w[i].ftnr);
}
}
}
free(w);
/******************/
quali=0;
/* reset img coord because num_cams < 4 */
for (j=0;j<4;j++) { x2[j]=-1e10; y2[j]=-1e10;}
/* if old wasn't found try to create new particle position from rest */
if (tpar->add) {
if ( curr_path_inf->inlist == 0) {
for (j = 0; j < fb->num_cams; j++) {
/* use fix distance to define xl, xr, yu, yd instead of searchquader */
xl[j]= xr[j]= yu[j]= yd[j] = 3.0;
zaehler1 = candsearch_in_pixrest (fb->buf[2]->targets[j],
fb->buf[2]->num_targets[j], xn[j], yn[j],
xl[j], xr[j], yu[j], yd[j], philf[j], cpar);
if(zaehler1 > 0) {
_ix = philf[j][0];
x2[j] = fb->buf[2]->targets[j][_ix].x;
y2[j] = fb->buf[2]->targets[j][_ix].y;
}
}
for (j = 0; j < fb->num_cams; j++) {
if (x2[j] !=-1e10 && y2[j] != -1e10) {
pixel_to_metric(&x2[j], &y2[j], x2[j],y2[j], cpar);
quali++;
}
}
if (quali>=2) {
vec_copy(X[3], X[2]);
invol=0;
det_lsq_3d (glob_cal, *(cpar->mm),
x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3],
&(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams);
/* volume check */
if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] &&
Ymin < X[3][1] && X[3][1] < Ymax &&
vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1])
{invol = 1;}
vec_subt(X[1], X[3], diff_pos);
if (invol == 1 && pos3d_in_bounds(diff_pos, tpar)) {
angle_acc(X[1], X[2], X[3], &angle, &acc);
if ( (acc<tpar->dacc && angle<tpar->dangle) || \
(acc<tpar->dacc/10) )
{
dl = (vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[0], X[1]) )/2;
rr =(dl/lmax+acc/tpar->dacc + angle/tpar->dangle)/(quali);
ref_path_inf = &(fb->buf[2]->path_info[
fb->buf[2]->num_parts]);
vec_copy(ref_path_inf->x, X[3]);
reset_links(ref_path_inf);
_frame_parts = fb->buf[2]->num_parts;
register_link_candidate(curr_path_inf, rr,
_frame_parts);
ref_corres = &(fb->buf[2]->correspond[_frame_parts]);
ref_targets = fb->buf[2]->targets;
for (j = 0;j < fb->num_cams; j++) {
ref_corres->p[j]=-1;
if(philf[j][0]!=-999) {
_ix = philf[j][0];
ref_targets[j][_ix].tnr = _frame_parts;
ref_corres->p[j] = _ix;
ref_corres->nr = _frame_parts;
}
}
fb->buf[2]->num_parts++;
}
}
invol=0;
}
}
} /* end of if old wasn't found try to create new particle position from rest */
} /* end of h-loop */
for (h = 0; h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
if(curr_path_inf->inlist > 0 ) {
sort(curr_path_inf->inlist, (float *)curr_path_inf->decis,
curr_path_inf->linkdecis);
}
}
/* create links with decision check */
count1=0; zusatz=0;
for (h = 0; h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
if (curr_path_inf->inlist > 0 ) {
/* if old/new and unused prev == -1 and next == -2 link is created */
ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->linkdecis[0]]);
if ( ref_path_inf->prev == PREV_NONE && \
ref_path_inf->next == NEXT_NONE )
{
curr_path_inf->finaldecis = curr_path_inf->decis[0];
curr_path_inf->prev = curr_path_inf->linkdecis[0];
fb->buf[2]->path_info[curr_path_inf->prev].next = h;
zusatz++;
}
/* old which link to prev has to be checked */
if ((ref_path_inf->prev != PREV_NONE) && \
(ref_path_inf->next == NEXT_NONE) )
{
vec_copy(X[0], fb->buf[0]->path_info[curr_path_inf->next].x);
vec_copy(X[1], curr_path_inf->x);
vec_copy(X[3], ref_path_inf->x);
vec_copy(X[4], fb->buf[3]->path_info[ref_path_inf->prev].x);
for (j = 0; j < 3; j++)
X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]);
angle_acc(X[3], X[4], X[5], &angle, &acc);
if ( (acc<tpar->dacc && angle<tpar->dangle) || (acc<tpar->dacc/10) ) {
curr_path_inf->finaldecis = curr_path_inf->decis[0];
curr_path_inf->prev = curr_path_inf->linkdecis[0];
fb->buf[2]->path_info[curr_path_inf->prev].next = h;
zusatz++;
}
}
}
if (curr_path_inf->prev != -1 ) count1++;
} /* end of creation of links with decision check */
sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d",
step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1,
fb->buf[1]->num_parts - count1, zusatz);
/* for the average of particles and links */
npart = npart + fb->buf[1]->num_parts;
nlinks = nlinks + count1;
fb_next(fb);
fb_write_frame_from_start(fb, step);
if(step > seq_par->first + 2) { fb_read_frame_at_end(fb, step - 3, 1); }
} /* end of sequence loop */
/* average of all steps */
npart /= (seq_par->last - seq_par->first - 1);
nlinks /= (seq_par->last - seq_par->first - 1);
printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n",
npart, nlinks, npart-nlinks);
fb_next(fb);
fb_write_frame_from_start(fb, step);
fb_free(fb);
free(fb);
free(tpar);
/* reset of display flag */
display = 1;
}
void trackcorr_c_loop (tracking_run *run_info, int step, int display)
{
/* sequence loop */
char val[256], buf[256];
int j, h, k, mm, kk, invol=0;
int zaehler1, zaehler2, philf[4][MAX_CANDS];
int count1=0, count2=0, count3=0, zusatz=0;
int intx0, intx1, inty0, inty1;
int intx2, inty2;
int quali=0;
vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */
double x1[4], y1[4], x2[4], y2[4], angle, acc, angle0, acc0, dl;
double xr[4], xl[4], yd[4], yu[4], angle1, acc1;
double xp[4], yp[4], xc[4], yc[4], xn[4], yn[4];
double rr;
int flag_m_tr=0;
/* Shortcuts to inside current frame */
P *curr_path_inf, *ref_path_inf;
corres *curr_corres, *ref_corres;
target **curr_targets, **ref_targets;
int _ix; /* For use in any of the complex index expressions below */
int _frame_parts; /* number of particles in a frame */
/* Shortcuts into the tracking_run struct */
framebuf *fb;
track_par *tpar;
volume_par *vpar;
control_par *cpar;
/* Remaining globals:
all those in trackcorr_c_init.
calibration globals.
*/
foundpix *w, *wn, p16[4*MAX_CANDS];
sprintf (buf, "Time step: %d, seqnr: %d, Particle info:",
step - run_info->seq_par->first, step);
count1=0; zusatz=0;
fb = run_info->fb;
tpar = run_info->tpar;
vpar = run_info->vpar;
cpar = run_info->cpar;
curr_targets = fb->buf[1]->targets;
/* try to track correspondences from previous 0 - corp, variable h */
for (h = 0; h < fb->buf[1]->num_parts; h++) {
for (j = 0; j < 7; j++) vec_init(X[j]);
curr_path_inf = &(fb->buf[1]->path_info[h]);
curr_corres = &(fb->buf[1]->correspond[h]);
curr_path_inf->inlist = 0;
reset_foundpix_array(p16, 16, fb->num_cams);
/* 3D-position */
vec_copy(X[1], curr_path_inf->x);
/* use information from previous to locate new search position
and to calculate values for search area */
if (curr_path_inf->prev >= 0) {
ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->prev]);
vec_copy(X[0], ref_path_inf->x);
search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]);
for (j = 0; j < fb->num_cams; j++) {
img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j],
*(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar);
}
} else {
vec_copy(X[2], X[1]);
for (j=0; j < fb->num_cams; j++) {
if (curr_corres->p[j] == -1) {
img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j],
ap[j], *(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar);
} else {
_ix = curr_corres->p[j];
x1[j] = curr_targets[j][_ix].x;
y1[j] = curr_targets[j][_ix].y;
}
}
}
/* calculate searchquader and reprojection in image space */
searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar);
/* search in pix for candidates in next time step */
for (j = 0; j < fb->num_cams; j++) {
register_closest_neighbs(fb->buf[2]->targets[j],
fb->buf[2]->num_targets[j], j, x1[j], y1[j],
xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS], cpar);
}
/* fill and sort candidate struct */
sortwhatfound(p16, &zaehler1, fb->num_cams);
w = (foundpix *) calloc (zaehler1, sizeof (foundpix));
if (zaehler1 > 0) count2++;
copy_foundpix_array(w, p16, zaehler1, fb->num_cams);
/*end of candidate struct */
/* check for what was found */
for (mm=0; mm<zaehler1;mm++) { /* zaehler1-loop */
/* search for found corr of current the corr in next
with predicted location */
reset_foundpix_array(p16, 16, fb->num_cams);
/* found 3D-position */
ref_path_inf = &(fb->buf[2]->path_info[w[mm].ftnr]);
vec_copy(X[3], ref_path_inf->x);
if (curr_path_inf->prev >= 0) {
for (j = 0; j < 3; j++)
X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]);
} else {
search_volume_center_moving(X[1], X[3], X[5]);
}
searchquader(X[5][0], X[5][1], X[5][2], xr, xl, yd, yu, tpar, cpar);
for (j = 0; j < fb->num_cams; j++) {
img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j],
*(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&x2[j], &y2[j], xn[j], yn[j], cpar);
}
/* search for candidates in next time step */
for (j=0; j < fb->num_cams; j++) {
zaehler2 = candsearch_in_pix (fb->buf[3]->targets[j],
fb->buf[3]->num_targets[j], x1[j], y1[j],
xl[j], xr[j], yu[j], yd[j], philf[j], cpar);
for(k = 0; k < 4; k++) {
if (philf[j][k] == -999) {
p16[j*4+k].ftnr=-1;
} else {
if (fb->buf[3]->targets[j][philf[j][k]].tnr != -1) {
_ix = philf[j][k];
p16[j*4+k].ftnr = fb->buf[3]->targets[j][_ix].tnr;
p16[j*4+k].whichcam[j] = 1;
}
}
}
}
/* end of search in pix */
/* fill and sort candidate struct */
sortwhatfound(p16, &zaehler2, fb->num_cams);
wn = (foundpix *) calloc (zaehler2, sizeof (foundpix));
if (zaehler2 > 0) count3++;
copy_foundpix_array(wn, p16, zaehler2, fb->num_cams);
/*end of candidate struct */
/* ************************************************ */
for (kk=0; kk < zaehler2; kk++) { /* zaehler2-loop */
ref_path_inf = &(fb->buf[3]->path_info[wn[kk].ftnr]);
vec_copy(X[4], ref_path_inf->x);
vec_subt(X[4], X[3], diff_pos);
if ( pos3d_in_bounds(diff_pos, tpar)) {
angle_acc(X[3], X[4], X[5], &angle1, &acc1);
if (curr_path_inf->prev >= 0) {
angle_acc(X[1], X[2], X[3], &angle0, &acc0);
} else {
acc0=acc1; angle0=angle1;
}
acc=(acc0+acc1)/2; angle=(angle0+angle1)/2;
quali=wn[kk].freq+w[mm].freq;
if ((acc < tpar->dacc && angle < tpar->dangle) || \
(acc < tpar->dacc/10))
{
dl = (vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[4], X[3]) )/2;
rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali);
register_link_candidate(curr_path_inf, rr, w[mm].ftnr);
}
}
} /* end of zaehler2-loop */
/* creating new particle position */
/* *************************************************************** */
for (j = 0;j < fb->num_cams; j++) {
img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j],
*(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar);
}
/* reset img coord because of num_cams < 4 */
for (j=0;j < fb->num_cams; j++) { x2[j]=-1e10; y2[j]=-1e10; }
/* search for unused candidates in next time step */
for (j = 0;j < fb->num_cams; j++) {
/* use fix distance to define xl, xr, yu, yd instead of searchquader */
xl[j]= xr[j]= yu[j]= yd[j] = 3.0;
zaehler2 = candsearch_in_pixrest (fb->buf[3]->targets[j],
fb->buf[3]->num_targets[j], xn[j], yn[j],
xl[j], xr[j], yu[j], yd[j], philf[j], cpar);
if(zaehler2>0 ) {
_ix = philf[j][0];
x2[j] = fb->buf[3]->targets[j][_ix].x;
y2[j] = fb->buf[3]->targets[j][_ix].y;
}
}
quali=0;
for (j = 0; j < fb->num_cams; j++) {
if (x2[j] != -1e10 && y2[j] != -1e10) {
pixel_to_metric(&x2[j],&y2[j], x2[j],y2[j], cpar);
quali++;
}
}
if ( quali >= 2) {
vec_copy(X[4], X[5]);
invol=0;
det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1],
x2[2], y2[2], x2[3], y2[3],
&(X[4][0]), &(X[4][1]), &(X[4][2]), fb->num_cams);
/* volume check */
if ( vpar->X_lay[0] < X[4][0] && X[4][0] < vpar->X_lay[1] &&
run_info->ymin < X[4][1] && X[4][1] < run_info->ymax &&
vpar->Zmin_lay[0] < X[4][2] && X[4][2] < vpar->Zmax_lay[1]) {invol=1;}
vec_subt(X[3], X[4], diff_pos);
if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) {
angle_acc(X[3], X[4], X[5], &angle, &acc);
if ((acc < tpar->dacc && angle < tpar->dangle) || \
(acc < tpar->dacc/10))
{
dl=(vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[4], X[3]) )/2;
rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle) /
(quali+w[mm].freq);
register_link_candidate(curr_path_inf, rr, w[mm].ftnr);
if (tpar->add) {
ref_path_inf = &(fb->buf[3]->path_info[
fb->buf[3]->num_parts]);
vec_copy(ref_path_inf->x, X[4]);
reset_links(ref_path_inf);
_frame_parts = fb->buf[3]->num_parts;
ref_corres = &(fb->buf[3]->correspond[_frame_parts]);
ref_targets = fb->buf[3]->targets;
for (j = 0; j < fb->num_cams; j++) {
ref_corres->p[j]=-1;
if(philf[j][0]!=-999) {
_ix = philf[j][0];
ref_targets[j][_ix].tnr = _frame_parts;
ref_corres->p[j] = _ix;
ref_corres->nr = _frame_parts;
}
}
fb->buf[3]->num_parts++;
zusatz++;
}
}
}
invol=0;
}
quali=0;
/* end of creating new particle position */
/* *************************************************************** */
/* try to link if kk is not found/good enough and prev exist */
if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) {
vec_subt(X[3], X[1], diff_pos);
if (pos3d_in_bounds(diff_pos, tpar)) {
angle_acc(X[1], X[2], X[3], &angle, &acc);
if ( (acc < tpar->dacc && angle < tpar->dangle) || \
(acc < tpar->dacc/10) )
{
quali = w[mm].freq;
dl = (vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[0], X[1]) )/2;
rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali);
register_link_candidate(curr_path_inf, rr, w[mm].ftnr);
}
}
}
free(wn);
} /* end of zaehler1-loop */
/* begin of inlist still zero */
if (tpar->add) {
if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) {
for (j = 0; j < fb->num_cams; j++) {
img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j],
ap[j], *(cpar->mm), &xn[j], &yn[j]);
metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar);
x2[j]=-1e10;
y2[j]=-1e10;
}
/* search for unused candidates in next time step */
for (j = 0; j < fb->num_cams; j++) {
/*use fix distance to define xl, xr, yu, yd instead of searchquader */
xl[j]= xr[j]= yu[j]= yd[j] = 3.0;
zaehler2 = candsearch_in_pixrest (fb->buf[2]->targets[j],
fb->buf[2]->num_targets[j], xn[j], yn[j],
xl[j], xr[j], yu[j], yd[j], philf[j], cpar);
if(zaehler2 > 0) {
_ix = philf[j][0];
x2[j] = fb->buf[2]->targets[j][_ix].x;
y2[j] = fb->buf[2]->targets[j][_ix].y;
}
}
quali=0;
for (j = 0; j < fb->num_cams; j++) {
if (x2[j] !=-1e10 && y2[j] != -1e10) {
pixel_to_metric(&x2[j], &y2[j], x2[j], y2[j], cpar);
quali++;
}
}
if (quali>=2) {
vec_copy(X[3], X[2]);
invol=0;
det_lsq_3d (glob_cal, *(cpar->mm),
x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3],
&(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams);
/* in volume check */
if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] &&
run_info->ymin < X[3][1] && X[3][1] < run_info->ymax &&
vpar->Zmin_lay[0] < X[3][2] &&
X[3][2] < vpar->Zmax_lay[1]) {invol = 1;}
vec_subt(X[2], X[3], diff_pos);
if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) {
angle_acc(X[1], X[2], X[3], &angle, &acc);
if ( (acc < tpar->dacc && angle < tpar->dangle) || \
(acc < tpar->dacc/10) )
{
dl = (vec_diff_norm(X[1], X[3]) +
vec_diff_norm(X[0], X[1]) )/2;
rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali);
ref_path_inf = &(fb->buf[2]->path_info[
fb->buf[2]->num_parts]);
vec_copy(ref_path_inf->x, X[3]);
reset_links(ref_path_inf);
_frame_parts = fb->buf[2]->num_parts;
register_link_candidate(curr_path_inf, rr, _frame_parts);
ref_corres = &(fb->buf[2]->correspond[_frame_parts]);
ref_targets = fb->buf[2]->targets;
for (j = 0;j < fb->num_cams; j++) {
ref_corres->p[j]=-1;
if(philf[j][0]!=-999) {
_ix = philf[j][0];
ref_targets[j][_ix].tnr = _frame_parts;
ref_corres->p[j] = _ix;
ref_corres->nr = _frame_parts;
}
}
fb->buf[2]->num_parts++;
zusatz++;
}
}
invol=0;
} // if quali >= 2
}
}
/* end of inlist still zero */
/***********************************/
free(w);
} /* end of h-loop */
/* sort decis and give preliminary "finaldecis" */
for (h = 0; h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
if(curr_path_inf->inlist > 0 ) {
sort(curr_path_inf->inlist, (float *) curr_path_inf->decis,
curr_path_inf->linkdecis);
curr_path_inf->finaldecis = curr_path_inf->decis[0];
curr_path_inf->next = curr_path_inf->linkdecis[0];
}
}
/* create links with decision check */
for (h = 0;h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
if(curr_path_inf->inlist > 0 ) {
ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->next]);
if (ref_path_inf->prev == -1) {
/* best choice wasn't used yet, so link is created */
ref_path_inf->prev = h;
} else {
/* best choice was already used by mega[2][mega[1][h].next].prev */
/* check which is the better choice */
if ( fb->buf[1]->path_info[ref_path_inf->prev].finaldecis > \
curr_path_inf->finaldecis)
{
/* remove link with prev */
fb->buf[1]->path_info[ref_path_inf->prev].next = NEXT_NONE;
ref_path_inf->prev = h;
} else {
curr_path_inf->next = NEXT_NONE;
}
}
}
if (curr_path_inf->next != -2 ) count1++;
}
/* end of creation of links with decision check */
/* ******** Draw links now ******** */
m1_tr = 0;
if (display) {
for (h = 0; h < fb->buf[1]->num_parts; h++) {
curr_path_inf = &(fb->buf[1]->path_info[h]);
curr_corres = &(fb->buf[1]->correspond[h]);
ref_corres = &(fb->buf[2]->correspond[curr_path_inf->next]);
if (curr_path_inf->next != -2 ) {
strcpy(buf,"");
sprintf(buf ,"green");
for (j = 0; j < fb->num_cams; j++) {
if (curr_corres->p[j] > 0 && ref_corres->p[j] > 0) {
flag_m_tr=1;
xp[j] = curr_targets[j][curr_corres->p[j]].x;
yp[j] = curr_targets[j][curr_corres->p[j]].y;
xc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].x;
yc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].y;
predict (xp[j], yp[j], xc[j], yc[j], &xn[j], &yn[j]);
if ( ( fabs(xp[j]-zoom_x[j]) < cpar->imx/(2*zoom_f[j]))
&& (fabs(yp[j]-zoom_y[j]) < cpar->imy/(2*zoom_f[j])))
{
strcpy(val,"");
sprintf(val ,"orange");
intx0 = (int)(cpar->imx/2 + zoom_f[j]*(xp[j] - zoom_x[j]));
inty0 = (int)(cpar->imy/2 + zoom_f[j]*(yp[j] - zoom_y[j]));
intx1 = (int)(cpar->imx/2 + zoom_f[j]*(xc[j] - zoom_x[j]));
inty1 = (int)(cpar->imy/2 + zoom_f[j]*(yc[j] - zoom_y[j]));
intx2 = (int)(cpar->imx/2 + zoom_f[j]*(xn[j] - zoom_x[j]));
inty2 = (int)(cpar->imy/2 + zoom_f[j]*(yn[j] - zoom_y[j]));
intx0_tr[j][m1_tr]=intx0;
inty0_tr[j][m1_tr]=inty0;
intx1_tr[j][m1_tr]=intx1;
inty1_tr[j][m1_tr]=inty1;
intx2_tr[j][m1_tr]=intx2;
inty2_tr[j][m1_tr]=inty2;
pnr1_tr[j][m1_tr]=-1;
pnr2_tr[j][m1_tr]=-1;
pnr3_tr[j][m1_tr]=-1;
if (curr_path_inf->finaldecis > 0.2) {
pnr1_tr[j][m1_tr] = h;
pnr2_tr[j][m1_tr] = curr_path_inf->next;
pnr3_tr[j][m1_tr] = curr_path_inf->finaldecis;
}
}
}
if (flag_m_tr==0) {
intx0_tr[j][m1_tr]=0;
inty0_tr[j][m1_tr]=0;
intx1_tr[j][m1_tr]=0;
inty1_tr[j][m1_tr]=0;
intx2_tr[j][m1_tr]=0;
inty2_tr[j][m1_tr]=0;
pnr1_tr[j][m1_tr]=-1;
pnr2_tr[j][m1_tr]=-1;
pnr3_tr[j][m1_tr]=-1;
}
flag_m_tr=0;
}
m1_tr++;
}
}
}
/* ******** End of Draw links now ******** */
sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d",
step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1,
fb->buf[1]->num_parts - count1, zusatz);
/* for the average of particles and links */
npart = npart + fb->buf[1]->num_parts;
nlinks = nlinks + count1;
fb_next(fb);
fb_write_frame_from_start(fb, step);
if(step < run_info->seq_par->last - 2) {
fb_read_frame_at_end(fb, step + 3, 0);
}
} /* end of sequence loop */
int calibration_proc_c (/*ClientData clientData, Tcl_Interp* interp,*/ int argc, const char** argv)
{
int i, j, sel, i_img, k, n, sup;
int intx1, inty1, intx2, inty2;
coord_2d apfig1[11][11]; /* regular grid for ap figures */
coord_2d apfig2[11][11]; /* ap figures */
coord_3d fix4[4]; /* object points for preorientation */
coord_2d crd0[4][4]; /* image points for preorientation */
char filename[256], val[256];
const char *valp;
//Tk_PhotoHandle img_handle;
//Tk_PhotoImageBlock img_block;
/* read support of unsharp mask */
fp1 = fopen ("parameters/unsharp_mask.par", "r");
if (! fp1) sup = 12;
else { fscanf (fp1, "%d\n", &sup); fclose (fp1); }
/* Get Selection value from TclTk */
// ChrisB: what does this do?? Set a value......
//valp = Tcl_GetVar(interp, "sel", TCL_GLOBAL_ONLY);
//sel = atoi (valp);
sel = 1; // set a value....
switch (sel)
{
case 1: /* read calibration parameter file */
fp1 = fopen_r ("parameters/cal_ori.par");
fscanf (fp1,"%s\n", fixp_name);
for (i=0; i<4; i++)
{
fscanf (fp1, "%s\n", img_name[i]);
fscanf (fp1, "%s\n", img_ori0[i]);
}
fscanf (fpp, "%d\n", &tiff_flag);
fscanf (fp1, "%d\n", &chfield);
fclose (fp1);
/* create file names */
for (i=0; i<n_img; i++)
{
strcpy (img_ori[i], img_name[i]);
strcat (img_ori[i], ".ori");
strcpy (img_addpar0[i], img_name[i]);
strcat (img_addpar0[i], ".addpar0");
strcpy (img_addpar[i], img_name[i]);
strcat (img_addpar[i], ".addpar");
strcpy (img_hp_name[i], img_name[i]);
strcat (img_hp_name[i], "_hp");
}
for (i=0; i<n_img; i++)
{
zoom_x[i] = imx/2, zoom_y[i] = imy/2, zoom_f[i] = 1;
read_image (/*interp,*/ img_name[i], img[i]);
sprintf(val, "camcanvas %d", i+1);
//Tcl_Eval(interp, val);
//img_handle = Tk_FindPhoto( interp, "temp");
//Tk_PhotoGetImage (img_handle, &img_block);
//tclimg2cimg (interp, img[i], &img_block);
sprintf(val, "newimage %d", i+1);
//Tcl_Eval(interp, val);
}
break;
case 2: puts ("Detection procedure"); strcpy(val,"");
/* Highpass Filtering */
pre_processing_c (/*clientData, interp,*/ argc, argv);
/* reset zoom values */
for (i=0; i<n_img; i++)
{
zoom_x[i] = imx/2; zoom_y[i] = imy/2; zoom_f[i] = 1;
}
/* copy images because the target recognition
will set greyvalues to zero */
for (i=0; i<n_img; i++)
{
copy_images (img[i], img0[i]);
}
/* target recognition */
for (i=0; i<n_img; i++)
{
targ_rec (/*interp,*/ img[i], img0[i], "parameters/detect_plate.par",
0, imx, 1, imy, pix[i], i, &num[i]);
sprintf (buf,"image %d: %d, ", i+1, num[i]);
strcat(val, buf);
if (num[i] > nmax) exit (1);
}
/* save pixel coord as approx. for template matching */
if (examine) for (i=0; i<n_img; i++)
{
sprintf (filename, "%s_pix", img_name[i]);
fp1 = fopen (filename, "w");
for (j=0; j<num[i]; j++)
fprintf (fp1, "%4d %8.3f %8.3f\n",
pix[i][j].pnr, pix[i][j].x, pix[i][j].y);
fclose (fp1);
}
sprintf(buf,"Number of detected targets, interaction enabled");
//Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text delete 2");
//Tcl_Eval(interp, ".text insert 2 $tbuf");
//Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text delete 3");
//Tcl_Eval(interp, ".text insert 3 $tbuf");
break;
case 3: pp1=0; pp2=0; pp3=0; pp4=0;
for (i=0; i<n_img; i++)
{
sprintf (buf, "%d targets remain", num[i]);
puts (buf);
}
fp1 = fopen_r ("parameters/man_ori.par");
for (i=0; i<n_img; i++)
{
fscanf (fp1, "%d %d %d %d\n", &nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]);
}
fclose (fp1);
for (i=0; i<n_img; i++)
{
sprintf(val, "measure %d %d %d %d %d", nr[i][0], nr[i][1], nr[i][2], nr[i][3], i+1);
//Tcl_Eval(interp, val);
#if 0
// ChrisB: do we need this?
valp = Tcl_GetVar(interp, "px0", TCL_GLOBAL_ONLY);
pix0[i][0].x = atoi (valp);
valp = Tcl_GetVar(interp, "py0", TCL_GLOBAL_ONLY);
pix0[i][0].y = atoi (valp);
valp = Tcl_GetVar(interp, "px1", TCL_GLOBAL_ONLY);
pix0[i][1].x = atoi (valp);
valp = Tcl_GetVar(interp, "py1", TCL_GLOBAL_ONLY);
pix0[i][1].y = atoi (valp);
valp = Tcl_GetVar(interp, "px2", TCL_GLOBAL_ONLY);
pix0[i][2].x = atoi (valp);
valp = Tcl_GetVar(interp, "py2", TCL_GLOBAL_ONLY);
pix0[i][2].y = atoi (valp);
valp = Tcl_GetVar(interp, "px3", TCL_GLOBAL_ONLY);
pix0[i][3].x = atoi (valp);
valp = Tcl_GetVar(interp, "py3", TCL_GLOBAL_ONLY);
pix0[i][3].y = atoi (valp);
#endif
}
/* write measured coordinates to file for next trial */
fp1 = fopen ("man_ori.dat", "w");
for (i=0; i<n_img; i++)
for (j=0; j<4; j++)
fprintf (fp1, "%f %f\n", pix0[i][j].x, pix0[i][j].y);
fclose (fp1);
break;
case 4: /* read pixel coordinates of older pre-orientation */
/* read point numbers of pre-clicked points */
fp1 = fopen_r ("parameters/man_ori.par");
for (i=0; i<n_img; i++)
{
fscanf (fp1, "%d %d %d %d\n",
&nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]);
}
fclose (fp1);
/* read coordinates of pre-clicked points */
fp1 = fopen ("man_ori.dat", "r");
if (! fp1) break;
for (i_img=0; i_img<n_img; i_img++) for (i=0; i<4; i++)
{
#if 0
fscanf (fp1, "%lf %lf\n",
&pix0[i_img][i].x, &pix0[i_img][i].y);
drawcross (interp, (int) pix0[i_img][i].x,
(int) pix0[i_img][i].y, cr_sz+2, i_img, "red");
draw_pnr (interp, (int) pix0[i_img][i].x, (int) pix0[i_img][i].y,
nr[i_img][i], i_img, "red");
#endif
}
fclose (fp1);
break;
case 5: puts ("Sort grid points");
for (i=0; i<n_img; i++)
{
/* read control point coordinates for man_ori points */
fp1 = fopen_r (fixp_name);
k = 0;
while ( fscanf (fp1, "%d %lf %lf %lf", &fix[k].pnr,
&fix[k].x, &fix[k].y, &fix[k].z) != EOF) k++;
fclose (fp1);
nfix = k;
/* take clicked points from control point data set */
for (j=0; j<4; j++) for (k=0; k<nfix; k++)
{
if (fix[k].pnr == nr[i][j]) fix4[j] = fix[k];
}
/* get approx for orientation and ap */
read_ori (&Ex[i], &I[i], img_ori0[i]);
fp1 = fopen (img_addpar0[i], "r");
if (! fp1) fp1 = fopen ("addpar.raw", "r");
if (fp1) {
fscanf (fp1, "%lf %lf %lf %lf %lf %lf %lf",
&ap[i].k1,&ap[i].k2,&ap[i].k3,
&ap[i].p1,&ap[i].p2,
&ap[i].scx,&ap[i].she);
fclose (fp1);} else {
printf("no addpar.raw\n");
ap[i].k1=ap[i].k2=ap[i].k3=ap[i].p1=ap[i].p2=ap[i].she=0.0;
ap[i].scx=1.0;
}
/* transform clicked points */
for (j=0; j<4; j++)
{
pixel_to_metric (pix0[i][j].x, pix0[i][j].y,
imx,imy, pix_x, pix_y,
&crd0[i][j].x, &crd0[i][j].y,
chfield);
correct_brown_affin (crd0[i][j].x, crd0[i][j].y, ap[i],
&crd0[i][j].x, &crd0[i][j].y);
}
/* raw orientation with 4 points */
raw_orient (Ex[i], I[i], ap[i], mmp, 4, fix4, crd0[i], &Ex[i]);
sprintf (filename, "raw%d.ori", i);
write_ori (Ex[i], I[i], filename);
/* sorting of detected points by back-projection */
sortgrid_man (/*interp,*/ Ex[i], I[i], ap[i], mmp,
imx,imy, pix_x,pix_y,
nfix, fix, num[i], pix[i], chfield, i);
/* adapt # of detected points */
num[i] = nfix;
for (j=0; j<nfix; j++)
{
#if 0
if (pix[i][j].pnr < 0) continue;
intx1 = (int) pix[i][j].x ;
inty1 = (int) pix[i][j].y ;
drawcross (interp, intx1, inty1, cr_sz, i, "white");
draw_pnr (interp, intx1, inty1, fix[j].pnr, i, "white");
#endif
}
}
/* dump dataset for rdb */
if (examine == 4)
{
/* create filename for dumped dataset */
sprintf (filename, "dump_for_rdb");
fp1 = fopen (filename, "w");
/* write # of points to file */
fprintf (fp1, "%d\n", nfix);
/* write point and image coord to file */
for (i=0; i<nfix; i++)
{
fprintf (fp1, "%4d %10.3f %10.3f %10.3f %d ",
fix[i].pnr, fix[i].x, fix[i].y, fix[i].z, 0);
for (i_img=0; i_img<n_img; i_img++)
{
if (pix[i_img][i].pnr >= 0)
{
/* transform pixel coord to metric */
pixel_to_metric (pix[i_img][i].x,
pix[i_img][i].y, imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y,
chfield);
fprintf (fp1, "%4d %8.5f %8.5f ",
pix[i_img][i].pnr,
crd[i_img][i].x, crd[i_img][i].y);
}
else
{
fprintf (fp1, "%4d %8.5f %8.5f ",
pix[i_img][i].pnr, 0.0, 0.0);
}
}
fprintf (fp1, "\n");
}
fclose (fp1);
printf ("dataset dumped into %s\n", filename);
}
break;
case 6: puts ("Orientation"); strcpy(buf, "");
for (i_img=0; i_img<n_img; i_img++)
{
for (i=0; i<nfix ; i++)
{
pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y,
imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y,
chfield);
crd[i_img][i].pnr = pix[i_img][i].pnr;
}
/* save data for special use of resection routine */
if (examine == 4)
{
printf ("try write resection data to disk\n");
/* point coordinates */
sprintf (filename, "resect_%s.fix", img_name[i_img]);
write_ori (Ex[i_img], I[i_img], img_ori[i_img]);
fp1 = fopen (filename, "w");
for (i=0; i<nfix; i++)
fprintf (fp1, "%3d %10.5f %10.5f %10.5f\n",
fix[i].pnr, fix[i].x, fix[i].y, fix[i].z);
fclose (fp1);
/* metric image coordinates */
sprintf (filename, "resect_%s.crd", img_name[i_img]);
fp1 = fopen (filename, "w");
for (i=0; i<nfix; i++)
fprintf (fp1,
"%3d %9.5f %9.5f\n", crd[i_img][i].pnr,
crd[i_img][i].x, crd[i_img][i].y);
fclose (fp1);
/* orientation and calibration approx data */
write_ori (Ex[i_img], I[i_img], "resect.ori0");
fp1 = fopen ("resect.ap0", "w");
fprintf (fp1, "%f %f %f %f %f %f %f",
ap[i_img].k1, ap[i_img].k2, ap[i_img].k3,
ap[i_img].p1, ap[i_img].p2,
ap[i_img].scx, ap[i_img].she);
fclose (fp1);
printf ("resection data written to disk\n");
}
/* resection routine */
/* ================= */
if (examine != 4)
orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp,
nfix, fix, crd[i_img],
&Ex[i_img], &I[i_img], &ap[i_img], i_img);
/* ================= */
/* resection with dumped datasets */
if (examine == 4)
{
printf("Resection with dumped datasets? (y/n)");
scanf("%s",buf);
if (buf[0] != 'y') continue;
strcpy (buf, "");
/* read calibration frame datasets */
for (n=0, nfix=0, dump_for_rdb=0; n<100; n++)
{
sprintf (filename, "resect.fix%d", n);
fp1 = fopen (filename, "r");
if (! fp1) continue;
printf("reading file: %s\n", filename);
printf ("reading dumped resect data #%d\n", n);
k = 0;
while ( fscanf (fp1, "%d %lf %lf %lf",
&fix[nfix+k].pnr, &fix[nfix+k].x,
&fix[nfix+k].y, &fix[nfix+k].z)
!= EOF) k++;
fclose (fp1);
/* read metric image coordinates */
sprintf (filename, "resect_%d.crd%d", i_img, n);
printf("reading file: %s\n", filename);
fp1 = fopen (filename, "r");
for (i=nfix; i<nfix+k; i++)
fscanf (fp1, "%d %lf %lf",
&crd[i_img][i].pnr,
&crd[i_img][i].x, &crd[i_img][i].y);
nfix += k;
}
/* resection */
orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp,
nfix, fix, crd[i_img],
&Ex[i_img], &I[i_img], &ap[i_img], i_img);
}
/* save orientation and additional parameters */
write_ori (Ex[i_img], I[i_img], img_ori[i_img]);
fp1 = fopen (img_addpar[i_img], "w");
fprintf (fp1, "%f %f %f %f %f %f %f",
ap[i_img].k1, ap[i_img].k2, ap[i_img].k3,
ap[i_img].p1, ap[i_img].p2,
ap[i_img].scx, ap[i_img].she);
fclose (fp1);
}
//Tcl_Eval(interp, ".text delete 3");
//Tcl_Eval(interp, ".text delete 1");
//Tcl_Eval(interp, ".text insert 1 \"Orientation and self calibration \"");
//Tcl_Eval(interp, ".text delete 2");
//Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \"");
//Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text insert 3 $tbuf");
break;
case 7: checkpoint_proc (/*interp*/);
#if 0
sprintf(val,"blue: planimetry, yellow: height");
Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 3");
Tcl_Eval(interp, ".text insert 3 $tbuf");
#endif
break;
case 8: /* draw additional parameter figures */
//Tcl_Eval(interp, "clearcam");
/* read orientation and additional parameters */
for (i=0; i<n_img; i++) read_ori (&Ex[i], &I[i], img_ori[i]);
for (i=0; i<n_img; i++)
{
fp1 = fopen_r (img_addpar[i]);
fscanf (fp1,"%lf %lf %lf %lf %lf %lf %lf",
&ap[i].k1, &ap[i].k2, &ap[i].k3,
&ap[i].p1, &ap[i].p2, &ap[i].scx, &ap[i].she);
fclose (fp1);
}
for (i_img=0; i_img<n_img; i_img++)
{
/* create undistorted grid */
for (i=0; i<11; i++) for (j=0; j<11; j++)
{
apfig1[i][j].x = i * imx/10;
apfig1[i][j].y = j * imy/10;
}
/* draw undistorted grid */
for (i=0; i<10; i++) for (j=0; j<10; j++)
{
intx1 = (int) apfig1[i][j].x;
inty1 = (int) apfig1[i][j].y;
intx2 = (int) apfig1[i+1][j].x;
inty2 = (int) apfig1[i][j+1].y;
//drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black");
//drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black");
}
for (j=0; j<10; j++)
{
intx1 = (int) apfig1[10][j].x;
inty1 = (int) apfig1[10][j].y;
inty2 = (int) apfig1[10][j+1].y;
//drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black");
}
for (i=0; i<10; i++)
{
intx1 = (int) apfig1[i][10].x;
inty1 = (int) apfig1[i][10].y;
intx2 = (int) apfig1[i+1][10].x;
//drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black");
}
/* distort grid */
for (i=0; i<11; i++) for (j=0; j<11; j++)
{
/* transform to metric, distort and re-transform */
pixel_to_metric (apfig1[i][j].x, apfig1[i][j].y,
imx,imy, pix_x,pix_y,
&apfig2[i][j].x, &apfig2[i][j].y, chfield);
distort_brown_affin (apfig2[i][j].x, apfig2[i][j].y,
ap[i_img], &apfig2[i][j].x, &apfig2[i][j].y);
metric_to_pixel (apfig2[i][j].x, apfig2[i][j].y,
imx,imy, pix_x,pix_y,
&apfig2[i][j].x, &apfig2[i][j].y, chfield);
/* exaggerate distortion by factor 5 */
apfig2[i][j].x = 5*apfig2[i][j].x - 4*apfig1[i][j].x;
apfig2[i][j].y = 5*apfig2[i][j].y - 4*apfig1[i][j].y;
}
/* draw distorted grid */
for (i=0; i<10; i++) for (j=0; j<10; j++)
{
intx1 = (int) apfig2[i][j].x;
inty1 = (int) apfig2[i][j].y;
intx2 = (int) apfig2[i+1][j].x;
inty2 = (int) apfig2[i+1][j].y;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
intx2 = (int) apfig2[i][j+1].x ;
inty2 = (int) apfig2[i][j+1].y ;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
for (j=0; j<10; j++)
{
intx1 = (int) apfig2[10][j].x;
inty1 = (int) apfig2[10][j].y;
intx2 = (int) apfig2[10][j+1].x;
inty2 = (int) apfig2[10][j+1].y;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
for (i=0; i<10; i++)
{
intx1 = (int) apfig2[i][10].x;
inty1 = (int) apfig2[i][10].y;
intx2 = (int) apfig2[i+1][10].x;
inty2 = (int) apfig2[i+1][10].y ;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
}
break;
}
return TCL_OK;
}
int correspondences_proc_c()
{
int i, i_img;
double x,y;
if( verbose )puts ("\nTransformation to metric coordinates\n");
/* rearrange point numbers after manual deletion of points */
for (i_img=0; i_img<n_img; i_img++)
for (i=0; i<num[i_img]; i++) pix[i_img][i].pnr = i;
/* transformations pixel coordinates -> metric coordinates */
/* transformations metric coordinates -> corrected metric coordinates */
for (i_img=0; i_img<n_img; i_img++)
{
for (i=0; i<num[i_img]; i++)
{
pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y,
imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y, chfield);
crd[i_img][i].pnr = pix[i_img][i].pnr;
x = crd[i_img][i].x - I[i_img].xh;
y = crd[i_img][i].y - I[i_img].yh;
correct_brown_affin (x, y, ap[i_img], &geo[i_img][i].x, &geo[i_img][i].y);
geo[i_img][i].pnr = crd[i_img][i].pnr;
}
}
/* sort coordinates for binary search in correspondences_proc */
for (i_img=0; i_img<n_img; i_img++)
{
quicksort_coord2d_x (geo[i_img], num[i_img]);
}
/* read illuminated layer data for demo version */
fpp = fopen_r ("parameters/criteria.par");
fscanf (fpp, "%lf\n", &X_lay[0]);
fscanf (fpp, "%lf\n", &Zmin_lay[0]);
fscanf (fpp, "%lf\n", &Zmax_lay[0]);
fscanf (fpp, "%lf\n", &X_lay[1]);
fscanf (fpp, "%lf\n", &Zmin_lay[1]);
fscanf (fpp, "%lf\n", &Zmax_lay[1]);
/* read criteria for accepted match (shape, tolerance), globals */
fscanf (fpp, "%lf", &cnx);
fscanf (fpp, "%lf", &cny);
fscanf (fpp, "%lf", &cn);
fscanf (fpp, "%lf", &csumg);
fscanf (fpp, "%lf", &corrmin);
fscanf (fpp, "%lf", &eps0);
fclose (fpp);
/* init multimedia radial displacement LUTs */
/* ======================================== */
if ( !mmp.lut && (mmp.n1 != 1 || mmp.n2[0] != 1 || mmp.n3 != 1))
{
if( verbose )puts ("Init multimedia displacement LUTs");
for (i_img=0; i_img<n_img; i_img++) init_mmLUT(i_img);
mmp.lut = 1;
}
correspondences_4 (/* interp*/);
/* --------------- */
/* save pixel coords for tracking */
for (i_img=0; i_img<n_img; i_img++)
{
if( useCompression ) {
// ChrisB: write compressed output:
sprintf (filename, "%s_targets.gz", img_name[i_img]);
fgz = gzopen (filename, "wb");
gzprintf(fgz,"%d\n", num[i_img]);
for (i=0; i<num[i_img]; i++)
{
gzprintf (fgz, "%4d %9.4f %9.4f %5d %5d %5d %5d %5d\n", pix[i_img][i].pnr, pix[i_img][i].x,
pix[i_img][i].y, pix[i_img][i].n ,
pix[i_img][i].nx ,pix[i_img][i].ny,
pix[i_img][i].sumg, pix[i_img][i].tnr);
}
gzclose( fgz );
} else {
// ChrisB: write uncompressed output:
sprintf (filename, "%s_targets", img_name[i_img]);
fp1 = fopen (filename, "w");
fprintf(fp1,"%d\n", num[i_img]);
for (i=0; i<num[i_img]; i++) {
fprintf (fp1, "%4d %9.4f %9.4f %5d %5d %5d %5d %5d\n", pix[i_img][i].pnr, pix[i_img][i].x,
pix[i_img][i].y, pix[i_img][i].n ,
pix[i_img][i].nx ,pix[i_img][i].ny,
pix[i_img][i].sumg, pix[i_img][i].tnr);
}
fclose (fp1);
}
}
return TCL_OK;
}
int mouse_proc_c (int click_x, int click_y, int kind, int num_image, volume_par *vpar, \
control_par *cpar){
int i, j, n, zf;
double x, y;
double xa12, xb12, ya12, yb12;
int k, pt1, intx1, inty1, count, intx2, inty2, pt2;
candidate cand[maxcand];
printf("entered mouse_proc_c \n");
if (zoom_f[0] == 1) {zf = 2;} else { zf = zoom_f[0];}
n=num_image;
if (examine) zf *= 2;
switch (kind)
{
/* -------------------------- MIDDLE MOUSE BUTTON ---------------------------------- */
case 3: /* generate epipolar line segments */
/* get geometric coordinates of nearest point in img[n] */
x = (float) (click_x - cpar->imx/2)/zoom_f[n] + zoom_x[n];
y = (float) (click_y - cpar->imy/2)/zoom_f[n] + zoom_y[n];
pixel_to_metric (&x, &y, x,y, cpar);
x -= I[n].xh; y -= I[n].yh;
correct_brown_affin (x, y, ap[n], &x, &y);
k = nearest_neighbour_geo (geo[n], num[n], x, y, 0.05);
if (k == -999){
printf ("No point near click coord! Click again! \n");
return -1;
}
pt1 = geo[n][k].pnr;
intx1 = (int) ( cpar->imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n]));
inty1 = (int) ( cpar->imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n]));
rclick_points_intx1=intx1;
rclick_points_inty1=inty1;
//drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet");
printf ( "pt1,nx,ny,n,sumg: %d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny,
pix[n][pt1].n, pix[n][pt1].sumg);
for (i = 0; i < cpar->num_cams; i++) if (i != n) {
/* calculate epipolar band in img[i] */
epi_mm (i, geo[n][k].x,geo[n][k].y,
Ex[n],I[n], G[n], Ex[i],I[i], G[i], *(cpar->mm), vpar,
&xa12, &ya12, &xb12, &yb12);
/* search candidate in img[i] */
printf("\ncandidates in img: %d\n", i);
find_candidate_plus_msg (geo[i], pix[i], num[i],
xa12, ya12, xb12, yb12,
pix[n][pt1].n, pix[n][pt1].nx, pix[n][pt1].ny,
pix[n][pt1].sumg, cand, &count, i, vpar, cpar);
distort_brown_affin (xa12,ya12, ap[i], &xa12,&ya12);
distort_brown_affin (xb12,yb12, ap[i], &xb12,&yb12);
xa12 += I[i].xh; ya12 += I[i].yh;
xb12 += I[i].xh; yb12 += I[i].yh;
metric_to_pixel(&xa12, &ya12, xa12, ya12, cpar);
metric_to_pixel(&xb12, &yb12, xb12, yb12, cpar);
intx1 = (int) ( cpar->imx/2 + zoom_f[i] * (xa12 - zoom_x[i]));
inty1 = (int) ( cpar->imy/2 + zoom_f[i] * (ya12 - zoom_y[i]));
intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (xb12 - zoom_x[i]));
inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (yb12 - zoom_y[i]));
rclick_intx1[i]=intx1;
rclick_inty1[i]=inty1;
rclick_intx2[i]=intx2;
rclick_inty2[i]=inty2;
// drawvector ( interp, intx1, inty1, intx2, inty2, 1, i, val);
rclick_count[i]=count;
for (j=0; j<count; j++)
{
pt2 = cand[j].pnr;
intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (pix[i][pt2].x - zoom_x[i]));
inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (pix[i][pt2].y - zoom_y[i]));
rclick_points_x1[i][j]=intx2;
rclick_points_y1[i][j]=inty2;
//drawcross (interp, intx2, inty2, cr_sz+2, i, "orange");
}
}
break;
case 4: /* delete points, which should not be used for orientation */
j = kill_in_list (n, num[n], click_x, click_y);
if (j != -1)
{
num[n] -= 1;
printf ("point %d deleted", j);
}
else {
printf ("no point near click coord !");
}
break;
}
printf("finished mouse_proc_c \n");
return 0;
}
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;
double dm = 0.0001, drad = 0.0001;
double xp, yp, xc, yc;
vec3d pos;
/* init X, y (set to zero) */
for (i = 0; i < 10; i++) {
for (j = 0; j < 6; j++)
X[i][j] = 0;
y[i] = 0;
}
cal->added_par.k1 = 0;
cal->added_par.k2 = 0;
cal->added_par.k3 = 0;
cal->added_par.p1 = 0;
cal->added_par.p2 = 0;
cal->added_par.scx = 1;
cal->added_par.she = 0;
/* main loop, program runs through it, until none of the beta values
comes over a threshold and no more points are thrown out
because of their residuals */
itnum = 0;
stopflag = 0;
while ((stopflag == 0) && (itnum < 20)) {
++itnum;
for (i = 0, n = 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
*/
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
/* no corrections as additional parameters are neglected
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
*/
/* every calibration dot is projected to the mm position, xp, yp */
vec_set(pos, fix[i][0], fix[i][1], fix[i][2]);
rotation_matrix(&(cal->ext_par));
img_coord (pos, cal, cpar->mm, &xp, &yp);
/* numeric derivatives of internal camera coefficients */
num_deriv_exterior(cal, cpar, dm, drad, pos, X[n], X[n + 1]);
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
/* Gauss Markoff Model */
ata ((double *) X, (double *) XPX, n, 6, 6);
matinv ((double *) XPX, 6, 6);
atl ((double *) XPy, (double *) X, y, n, 6, 6);
matmul ((double *) beta, (double *) XPX, (double *) XPy, 6,6,1,6,6);
stopflag = 1;
for (i = 0; i < 6; i++) {
if (fabs (beta[i]) > 0.1 )
stopflag = 0;
}
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
}
if (stopflag) {
rotation_matrix(&(cal->ext_par));
}
return stopflag;
}
/* orient() calculates orientation of the camera, updating its calibration
structure using the definitions and algorithms well described in [1].
Arguments:
Calibration* cal_in - camera calibration object
control_par *cpar - control parameters
int nfix - number of 3D known points
vec3d fix[] - each of nfix items is one 3D position of known point on
the calibration object.
target pix[] - image coordinates corresponding to each point in ``fix``.
can be obtained from the set of detected 2D points using
sortgrid(). The points which are associated with fix[] have real
pointer (.pnr attribute), others have -999.
orient_par flags - structure of all the flags of the parameters to be
(un)changed, read from orient.par parameter file using
read_orient_par(), defaults are zeros except for x_scale which is
by default 1.
Output:
Calibration *cal_in - if the orientation routine converged, this structure
is updated, otherwise, returned untouched. The routine works on a copy of
the calibration structure, cal.
double sigmabeta[] - array of deviations for each of the interior and
exterior parameters and glass interface vector (19 in total).
Returns:
On success, a pointer to an array of residuals. For each observation point
i = 0..n-1, residual 2*i is the Gauss-Markof residual for the x coordinate
and residual 2*i + 1 is for the y. Then come 10 cells with the delta
between initial guess and final solution for internal and distortion
parameters, which are also part of the G-M model and described in it.
On failure returns NULL.
*/
double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
target pix[], orient_par *flags, double sigmabeta[20])
{
int i,j,n, itnum, stopflag, n_obs=0, maxsize;
double ident[IDT], XPX[NPAR][NPAR], XPy[NPAR], beta[NPAR], omega=0;
double xp, yp, xpd, ypd, xc, yc, r, qq, p, sumP;
int numbers;
double al,be,ga,nGl,e1_x,e1_y,e1_z,e2_x,e2_y,e2_z,safety_x,safety_y,safety_z;
double *P, *y, *yh, *Xbeta, *resi;
vec3d glass_dir, tmp_vec, e1, e2;
Calibration *cal;
/* small perturbation for translation/rotation in meters and in radians */
double dm = 0.00001, drad = 0.0000001;
cal = malloc (sizeof (Calibration));
memcpy(cal, cal_in, sizeof (Calibration));
maxsize = nfix*2 + IDT;
P = (double *) calloc(maxsize, sizeof(double));
y = (double *) calloc(maxsize, sizeof(double));
yh = (double *) calloc(maxsize, sizeof(double));
Xbeta = (double *) calloc(maxsize, sizeof(double));
resi = (double *) calloc(maxsize, sizeof(double));
double (*X)[NPAR] = malloc(sizeof (*X) * maxsize);
double (*Xh)[NPAR] = malloc(sizeof (*Xh) * maxsize);
for(i = 0; i < maxsize; i++) {
for(j = 0; j < NPAR; j++) {
X[i][j] = 0.0;
Xh[i][j] = 0.0;
}
y[i] = 0;
P[i] = 1;
}
for(i = 0; i < NPAR; i++)
sigmabeta[j] = 0.0;
if(flags->interfflag){
numbers = 18;
} else{
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);
e1_x = 2*cal->glass_par.vec_z - 3*cal->glass_par.vec_x;
e1_y = 3*cal->glass_par.vec_x - 1*cal->glass_par.vec_z;
e1_z = 1*cal->glass_par.vec_y - 2*cal->glass_par.vec_y;
vec_set(tmp_vec, e1_x, e1_y, e1_z);
unit_vector(tmp_vec, e1);
e2_x = e1_y*cal->glass_par.vec_z - e1_z*cal->glass_par.vec_x;
e2_y = e1_z*cal->glass_par.vec_x - e1_x*cal->glass_par.vec_z;
e2_z = e1_x*cal->glass_par.vec_y - e1_y*cal->glass_par.vec_y;
vec_set(tmp_vec, e2_x, e2_y, e2_z);
unit_vector(tmp_vec, e2);
al = 0;
be = 0;
ga = 0;
/* init identities */
ident[0] = cal->int_par.cc;
ident[1] = cal->int_par.xh;
ident[2] = cal->int_par.yh;
ident[3] = cal->added_par.k1;
ident[4] = cal->added_par.k2;
ident[5] = cal->added_par.k3;
ident[6] = cal->added_par.p1;
ident[7] = cal->added_par.p2;
ident[8] = cal->added_par.scx;
ident[9] = cal->added_par.she;
safety_x = cal->glass_par.vec_x;
safety_y = cal->glass_par.vec_y;
safety_z = cal->glass_par.vec_z;
/* main loop, program runs through it, until none of the beta values
comes over a threshold and no more points are thrown out
because of their residuals */
itnum = 0;
stopflag = 0;
while ((stopflag == 0) && (itnum < NUM_ITER)) {
itnum++;
for (i = 0, n = 0; i < nfix; i++) {
/* check for correct correspondence
note that we do not use anymore pointer in fix, the points are read by
the order of appearance and if we want to use every other point
we use 'i', just check it is not -999 */
if(pix[i].pnr != i) continue;
switch (flags->useflag) {
case 1: if ((i % 2) == 0) continue; break;
case 2: if ((i % 2) != 0) continue; break;
case 3: if ((i % 3) == 0) continue; break;
}
/* get metric flat-image coordinates of the detected point */
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
/* Projected 2D position on sensor of corresponding known point */
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xp, &yp);
/* derivatives of distortion parameters */
r = sqrt (xp*xp + yp*yp);
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][9] = cal->added_par.scx * xp * r*r;
X[n+1][9] = yp * r*r;
X[n][10] = cal->added_par.scx * xp * pow(r,4.0);
X[n+1][10] = yp * pow(r,4.0);
X[n][11] = cal->added_par.scx * xp * pow(r,6.0);
X[n+1][11] = yp * pow(r,6.0);
X[n][12] = cal->added_par.scx * (2*xp*xp + r*r);
X[n+1][12] = 2 * xp * yp;
X[n][13] = 2 * cal->added_par.scx * xp * yp;
X[n+1][13] = 2*yp*yp + r*r;
qq = cal->added_par.k1*r*r; qq += cal->added_par.k2*pow(r,4.0);
qq += cal->added_par.k3*pow(r,6.0);
qq += 1;
X[n][14] = xp * qq + cal->added_par.p1 * (r*r + 2*xp*xp) + \
2*cal->added_par.p2*xp*yp;
X[n+1][14] = 0;
X[n][15] = -cos(cal->added_par.she) * yp;
X[n+1][15] = -sin(cal->added_par.she) * yp;
/* numeric derivatives of projection coordinates over external
parameters, 3D position and the angles */
num_deriv_exterior(cal, cpar, dm, drad, fix[i], X[n], X[n + 1]);
/* Num. deriv. of projection coords over sensor distance from PP */
cal->int_par.cc += dm;
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][6] = (xpd - xp) / dm;
X[n+1][6] = (ypd - yp) / dm;
cal->int_par.cc -= dm;
/* ditto, over water-glass-air interface position vector */
al += dm;
cal->glass_par.vec_x += e1[0]*nGl*al;
cal->glass_par.vec_y += e1[1]*nGl*al;
cal->glass_par.vec_z += e1[2]*nGl*al;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][16] = (xpd - xp) / dm;
X[n+1][16] = (ypd - yp) / dm;
al -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
be += dm;
cal->glass_par.vec_x += e2[0]*nGl*be;
cal->glass_par.vec_y += e2[1]*nGl*be;
cal->glass_par.vec_z += e2[2]*nGl*be;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][17] = (xpd - xp) / dm;
X[n+1][17] = (ypd - yp) / dm;
be -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
ga += dm;
cal->glass_par.vec_x += cal->glass_par.vec_x*nGl*ga;
cal->glass_par.vec_y += cal->glass_par.vec_y*nGl*ga;
cal->glass_par.vec_z += cal->glass_par.vec_z*nGl*ga;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][18] = (xpd - xp) / dm;
X[n+1][18] = (ypd - yp) / dm;
ga -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
n_obs = n;
/* identities */
for (i = 0; i < IDT; i++)
X[n_obs + i][6 + i] = 1;
y[n_obs+0] = ident[0] - cal->int_par.cc;
y[n_obs+1] = ident[1] - cal->int_par.xh;
y[n_obs+2] = ident[2] - cal->int_par.yh;
y[n_obs+3] = ident[3] - cal->added_par.k1;
y[n_obs+4] = ident[4] - cal->added_par.k2;
y[n_obs+5] = ident[5] - cal->added_par.k3;
y[n_obs+6] = ident[6] - cal->added_par.p1;
y[n_obs+7] = ident[7] - cal->added_par.p2;
y[n_obs+8] = ident[8] - cal->added_par.scx;
y[n_obs+9] = ident[9] - cal->added_par.she;
/* weights */
for (i = 0; i < n_obs; i++)
P[i] = 1;
P[n_obs+0] = ( ! flags->ccflag) ? POS_INF : 1;
P[n_obs+1] = ( ! flags->xhflag) ? POS_INF : 1;
P[n_obs+2] = ( ! flags->yhflag) ? POS_INF : 1;
P[n_obs+3] = ( ! flags->k1flag) ? POS_INF : 1;
P[n_obs+4] = ( ! flags->k2flag) ? POS_INF : 1;
P[n_obs+5] = ( ! flags->k3flag) ? POS_INF : 1;
P[n_obs+6] = ( ! flags->p1flag) ? POS_INF : 1;
P[n_obs+7] = ( ! flags->p2flag) ? POS_INF : 1;
P[n_obs+8] = ( ! flags->scxflag) ? POS_INF : 1;
P[n_obs+9] = ( ! flags->sheflag) ? POS_INF : 1;
n_obs += IDT;
sumP = 0;
for (i = 0; i < n_obs; i++) { /* homogenize */
p = sqrt (P[i]);
for (j = 0; j < NPAR; j++)
Xh[i][j] = p * X[i][j];
yh[i] = p * y[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 */
ata ((double *) Xh, (double *) XPX, n_obs, numbers, NPAR );
matinv ((double *) XPX, numbers, NPAR);
atl ((double *) XPy, (double *) Xh, yh, n_obs, numbers, NPAR);
matmul ((double *) beta, (double *) XPX, (double *) XPy,
numbers, numbers,1, NPAR, NPAR);
stopflag = 1;
for (i = 0; i < numbers; i++) {
if (fabs (beta[i]) > CONVERGENCE) stopflag = 0;
}
if ( ! flags->ccflag) beta[6] = 0.0;
if ( ! flags->xhflag) beta[7] = 0.0;
if ( ! flags->yhflag) beta[8] = 0.0;
if ( ! flags->k1flag) beta[9] = 0.0;
if ( ! flags->k2flag) beta[10] = 0.0;
if ( ! flags->k3flag) beta[11] = 0.0;
if ( ! flags->p1flag) beta[12] = 0.0;
if ( ! flags->p2flag) beta[13] = 0.0;
if ( ! flags->scxflag)beta[14] = 0.0;
if ( ! flags->sheflag) beta[15] = 0.0;
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
cal->int_par.cc += beta[6];
cal->int_par.xh += beta[7];
cal->int_par.yh += beta[8];
cal->added_par.k1 += beta[9];
cal->added_par.k2 += beta[10];
cal->added_par.k3 += beta[11];
cal->added_par.p1 += beta[12];
cal->added_par.p2 += beta[13];
cal->added_par.scx += beta[14];
cal->added_par.she += beta[15];
if (flags->interfflag) {
cal->glass_par.vec_x += e1[0]*nGl*beta[16];
cal->glass_par.vec_y += e1[1]*nGl*beta[16];
cal->glass_par.vec_z += e1[2]*nGl*beta[16];
cal->glass_par.vec_x += e2[0]*nGl*beta[17];
cal->glass_par.vec_y += e2[1]*nGl*beta[17];
cal->glass_par.vec_z += e2[2]*nGl*beta[17];
}
}
/* compute residuals etc. */
matmul ( (double *) Xbeta, (double *) X, (double *) beta, n_obs,
numbers, 1, n_obs, NPAR);
omega = 0;
for (i = 0; i < n_obs; i++) {
resi[i] = Xbeta[i] - y[i];
omega += resi[i] * P[i] * resi[i];
}
sigmabeta[NPAR] = sqrt (omega / (n_obs - numbers));
for (i = 0; i < numbers; i++) {
sigmabeta[i] = sigmabeta[NPAR] * sqrt(XPX[i][i]);
}
free(X);
free(P);
free(y);
free(Xbeta);
free(Xh);
if (stopflag){
rotation_matrix(&(cal->ext_par));
memcpy(cal_in, cal, sizeof (Calibration));
return resi;
}
else {
free(resi);
return NULL;
}
}