/* num_deriv_exterior() calculates the partial numerical derivative of image
coordinates of a given 3D position, over each of the 6 exterior orientation
parameters (3 position parameters, 3 rotation angles).
Arguments:
Calibration* cal_in - camera calibration object
control_par *cpar - control parameters
double dpos, double dang - the step size for numerical differentiation,
dpos for the metric variables, dang for the angle variables. Units
are same as the units of the variables derived.
vec3d pos - the current 3D position represented on the image.
Return parameters:
double x_ders[6], y_ders[6] respectively the derivatives of the x and y
image coordinates as function of each of the orientation parameters.
*/
void num_deriv_exterior(Calibration* cal, control_par *cpar, double dpos, double dang, vec3d pos,
double x_ders[6], double y_ders[6])
{
int pd;
double step, xs, ys, xpd, ypd;
double* vars[6];
vars[0] = &(cal->ext_par.x0);
vars[1] = &(cal->ext_par.y0);
vars[2] = &(cal->ext_par.z0);
vars[3] = &(cal->ext_par.omega);
vars[4] = &(cal->ext_par.phi);
vars[5] = &(cal->ext_par.kappa);
/* Starting image position */
rotation_matrix(&(cal->ext_par));
img_coord (pos, cal, cpar->mm, &xs, &ys);
for (pd = 0; pd < 6; pd++) {
step = (pd > 2) ? dang : dpos;
*(vars[pd]) += step;
if (pd > 2) rotation_matrix(&(cal->ext_par));
img_coord (pos, cal, cpar->mm, &xpd, &ypd);
x_ders[pd] = (xpd - xs) / step;
y_ders[pd] = (ypd - ys) / step;
*(vars[pd]) -= step;
}
rotation_matrix(&(cal->ext_par));
}
void searchquader(double X, double Y, double Z,
double xr[4], double xl[4], double yd[4], double yu[4])
{
int k, i;
double x, y, xz, yz;
coord_3d quader[8], point;
/* project quader in image space to define search area */
for (k=0; k<8; k++)
{
quader[k].pnr=k;
}
/* calculation of quader points */
point.pnr=0; point.x=X; point.y=Y; point.z=Z;
quader[0].x=X+tpar.dvxmin; quader[0].y=Y+tpar.dvymin; quader[0].z=Z+tpar.dvzmin; /* --- */
quader[1].x=X+tpar.dvxmax; quader[1].y=Y+tpar.dvymin; quader[1].z=Z+tpar.dvzmin; /* +-- */
quader[2].x=X+tpar.dvxmin; quader[2].y=Y+tpar.dvymax; quader[2].z=Z+tpar.dvzmin; /* -+- */
quader[3].x=X+tpar.dvxmin; quader[3].y=Y+tpar.dvymin; quader[3].z=Z+tpar.dvzmax; /* --+ */ //changed by Beat and JuliAn Nov 2008
quader[4].x=X+tpar.dvxmax; quader[4].y=Y+tpar.dvymax; quader[4].z=Z+tpar.dvzmin; /* ++- */
quader[5].x=X+tpar.dvxmax; quader[5].y=Y+tpar.dvymin; quader[5].z=Z+tpar.dvzmax; /* +-+ */
quader[6].x=X+tpar.dvxmin; quader[6].y=Y+tpar.dvymax; quader[6].z=Z+tpar.dvzmax; /* -++ */
quader[7].x=X+tpar.dvxmax; quader[7].y=Y+tpar.dvymax; quader[7].z=Z+tpar.dvzmax; /* +++ */
/* calculation of search area */
for (i=0; i<n_img; i++)
{
xr[i]=0;
xl[i]=imx;
yd[i]=0;
yu[i]=imy;
img_coord (point.x, point.y, point.z, Ex[i], I[i], G[i], ap[i], mmp, &xz,&yz);
metric_to_pixel (xz,yz, imx,imy, pix_x,pix_y, &xz,&yz, chfield);
for (k=0; k<8; k++)
{
img_coord (quader[k].x, quader[k].y, quader[k].z, Ex[i], I[i], G[i], ap[i], mmp, &x,&y);
metric_to_pixel (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield);
if (x <xl[i] ) xl[i]=x;
if (y <yu[i] ) yu[i]=y;
if (x >xr[i] ) xr[i]=x;
if (y >yd[i] ) yd[i]=y;
}
if (xl[i] < 0 ) xl[i]=0;
if (yu[i] < 0 ) yu[i]=0;
if (xr[i] > imx) xr[i]=imx;
if (yd[i] > imy) yd[i]=imy;
xr[i]=xr[i]-xz;
xl[i]=xz-xl[i];
yd[i]=yd[i]-yz;
yu[i]=yz-yu[i];
}
return;
}
END_TEST
START_TEST(test_point_position)
{
/* Generate target information based on a known point. In the simple case,
calculated point must equal known point and average ray distance == 0.
Then jigg the cameras symmetrically to get the same points again but
with an analytically derivable ray distance. */
int cam, num_cams = 4;
vec2d targs_plain[4], targs_jigged[4];
Calibration *calib[4];
char ori_tmpl[] = "cal/sym_cam%d.tif.ori";
char ori_name[25];
mm_np media_par = {1, 1., {1., 0., 0.}, {1., 0., 0.}, 1.};
vec3d point = {17, 42, 0}; /* Something in the FOV, non-symmetric. */
vec3d res, jigged;
double jigg_amp = 0.5, skew_dist, jigged_correct;
/* Target generation requires an existing calibration. */
chdir("testing_fodder/");
/* Four cameras on 4 quadrants. */
for (cam = 0; cam < num_cams; cam++) {
sprintf(ori_name, ori_tmpl, cam + 1);
calib[cam] = read_calibration(ori_name, "cal/cam1.tif.addpar", NULL);
img_coord(point, calib[cam], &media_par,
&(targs_plain[cam][0]), &(targs_plain[cam][1]));
vec_copy(jigged, point);
jigged[1] += ((cam % 2) ? jigg_amp : -jigg_amp);
img_coord(jigged, calib[cam], &media_par,
&(targs_jigged[cam][0]), &(targs_jigged[cam][1]));
}
skew_dist = point_position(targs_plain, num_cams, &media_par, calib, res);
fail_unless(skew_dist < 1e-10);
vec_subt(point, res, res);
fail_unless(vec_norm(res) < 1e-10);
skew_dist = point_position(targs_jigged, num_cams, &media_par, calib, res);
jigged_correct = 4*(2*jigg_amp)/6;
/* two thirds, but because of details: each disagreeing pair (4 out of 6)
has a 2*jigg_amp error because the cameras are moved in opposite
directions. */
fail_unless(fabs(skew_dist - jigged_correct) < 0.05);
vec_subt(point, res, res);
fail_unless(vec_norm(res) < 0.01);
}
END_TEST
START_TEST(test_convergence_measure)
{
/* Generate target information based on known points. In the simple case,
convergence will be spot-on. Then generate other targets based on a
per-camera jigging of 3D points and get convergence measure compatible
with the jig amplitude. */
vec3d known[16], jigged;
vec2d* targets[16];
Calibration *calib[4];
int num_cams = 4, num_pts = 16;
int cam, cpt_vert, cpt_horz, cpt_ix;
double jigg_amp = 0.5, jigged_skew_dist, jigged_correct;
char ori_tmpl[] = "cal/sym_cam%d.tif.ori";
char ori_name[25];
/* Using a neutral medium, this isn't what's tested. */
mm_np media_par = {1, 1., {1., 0., 0.}, {1., 0., 0.}, 1.};
control_par *cpar;
/* Target generation requires an existing calibration. */
chdir("testing_fodder/");
cpar = read_control_par("parameters/ptv.par");
/* Four cameras on 4 quadrants looking down into a calibration target.
Calibration taken from an actual experimental setup */
for (cam = 0; cam < num_cams; cam++) {
sprintf(ori_name, ori_tmpl, cam + 1);
calib[cam] = read_calibration(ori_name, "cal/cam1.tif.addpar", NULL);
}
/* Reference points before jigging: */
for (cpt_horz = 0; cpt_horz < 4; cpt_horz++) {
for (cpt_vert = 0; cpt_vert < 4; cpt_vert++) {
cpt_ix = cpt_horz*4 + cpt_vert;
vec_set(known[cpt_ix], cpt_vert * 10, cpt_horz * 10, 0);
}
}
/* Plain case: */
for (cpt_ix = 0; cpt_ix < num_pts; cpt_ix++) {
targets[cpt_ix] = (vec2d *) calloc(num_cams, sizeof(vec2d));
for (cam = 0; cam < num_cams; cam++) {
img_coord(known[cpt_ix], calib[cam], &media_par,
&(targets[cpt_ix][cam][0]),
&(targets[cpt_ix][cam][1]));
}
}
fail_unless(fabs(weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 1, 0)) < 1e-10);
/* With dumbbell length: */
fail_unless(fabs(weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 10, 10)) < 1e-10);
/* Jigged case (reusing target memory), moving the points and cameras
in parallel to create a known shift between parallel rays. */
for (cam = 0; cam < num_cams; cam++) {
calib[cam]->ext_par.y0 += ((cam % 2) ? jigg_amp : -jigg_amp);
for (cpt_ix = 0; cpt_ix < num_pts; cpt_ix++) {
vec_copy(jigged, known[cpt_ix]);
jigged[1] += ((cam % 2) ? jigg_amp : -jigg_amp);
img_coord(jigged, calib[cam], &media_par,
&(targets[cpt_ix][cam][0]),
&(targets[cpt_ix][cam][1]));
}
}
jigged_skew_dist = weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 1, 0);
jigged_correct = 16*4*(2*jigg_amp)/(16*6);
/* two thirds, but because of details: each disagreeing pair (4 out of 6)
has a 2*jigg_amp error because the cameras are moved in opposite
directions. */
fail_unless(fabs(jigged_skew_dist - jigged_correct) < 0.05 );
}
/* point_to_pixel is just a shortcut to two lines that repeat every so in track loop
* img_coord (from 3d point to a 2d vector in metric units), followed by
* metric_to_pixel (from 2d vector in metric units to the pixel position in the camera)
* Arguments:
* vec3d point in 3D space
* Calibration *cal parameters
* Control parameters (num cams, multimedia parameters, cpar->mm, etc.)
* Returns (as a first argument):
* vec2d with pixel positions (x,y) in the camera.
*/
void point_to_pixel (vec2d v1, vec3d point, Calibration *cal, control_par *cpar){
img_coord(point, cal, cpar->mm, &v1[0], &v1[1]);
metric_to_pixel(&v1[0], &v1[1], v1[0], v1[1], cpar);
}
/* 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 flow_demo_c (ClientData clientData, Tcl_Interp* interp, int argc, const char** argv)
{
int i, i_seq, nr, j,pft_version=3,num_points,dumy;
char name[128];
unsigned char *imgf;
Tk_PhotoHandle img_handle;
Tk_PhotoImageBlock img_block;
nr = atoi(argv[1]);
fpp = fopen_r ("parameters/sequence.par");
for (i=0; i<4; i++)
fscanf (fpp, "%s\n", seq_name[i]); /* name of sequence */
fscanf (fpp,"%d\n", &seq_first);
fscanf (fpp,"%d\n", &seq_last);
fclose (fpp);
/* allocate memory */
imgf = (unsigned char *) calloc (imgsize, 1);
fpp = fopen ("parameters/pft_version.par", "r");
if (fpp){
fscanf (fpp, "%d\n", &pft_version);
pft_version=pft_version+3;
fclose (fpp);
}
else{
fpp = fopen ("parameters/pft_version.par", "w");
fprintf(fpp,"%d\n", 0);
fclose(fpp);
}
/* load and display images */
for (i_seq=seq_first; i_seq<=seq_last; i_seq++){
compose_name_plus_nr (seq_name[nr], "", i_seq, name);
fp1 = fopen_r (name); if (! fp1) return TCL_OK;
sprintf (buf, "display camera %d, image %d", nr+1, i_seq);
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
read_image (interp, name, imgf);
fclose (fp1);
img_handle = Tk_FindPhoto( interp, "temp");
Tk_PhotoGetImage (img_handle, &img_block);
sprintf(buf, "newimage %d", nr+1);
Tcl_Eval(interp, buf);
if(pft_version==4){
sprintf (filename, "%s%s", name,"_targets");
/* read targets of camera nr*/
nt4[3][nr]=0;
fp1= fopen (filename, "r");
if (! fp1) printf("Can't open ascii file: %s\n", filename);
fscanf (fp1, "%d\n", &nt4[3][nr]);
for (j=0; j<nt4[3][nr]; j++){
fscanf (fp1, "%4d %lf %lf %d %d %d %d %d\n",
&pix[nr][j].pnr, &pix[nr][j].x,
&pix[nr][j].y, &pix[nr][j].n ,
&pix[nr][j].nx ,&pix[nr][j].ny,
&pix[nr][j].sumg, &pix[nr][j].tnr);
}
fclose (fp1);
num[nr] = nt4[3][nr];
if (display){
for (j=0; j<num[nr]; j++){
drawcross (interp, (int) pix[nr][j].x, (int) pix[nr][j].y,cr_sz, nr, "blue");
}
printf ("drawing %d 2d ", num[nr]);
}
sprintf (filename, "res/rt_is.%d", i_seq);
fp1= fopen (filename, "r");
if (fp1){
fscanf (fp1, "%d\n", &num_points);
for (j=0; j<num_points; j++){
if (n_img==4){
fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n",
&dumy, &fix[j].x, &fix[j].y, &fix[j].z,
&geo[0][j].pnr, &geo[1][j].pnr, &geo[2][j].pnr, &geo[3][j].pnr);
}
if (n_img==3){
fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n",
&dumy, &fix[j].x, &fix[j].y, &fix[j].z,
&geo[0][j].pnr, &geo[1][j].pnr, &geo[2][j].pnr);
}
if (n_img==2){ // Alex's patch. 24.09.09. Working on Wesleyan data of 2 cameras only
fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n",
&dumy, &fix[j].x, &fix[j].y, &fix[j].z,
&geo[0][j].pnr, &geo[1][j].pnr);
}
}
fclose (fp1);
if (display){
for (j=0; j<num_points; j++){
img_coord (fix[j].x,fix[j].y,fix[j].z, Ex[nr], I[nr], G[nr], ap[nr], mmp, &pix[nr][j].x,&pix[nr][j].y);
metric_to_pixel (pix[nr][j].x,pix[nr][j].y, imx,imy, pix_x,pix_y, &pix[nr][j].x,&pix[nr][j].y, chfield);
//if(geo[nr][j].pnr>-1){
// drawcross (interp, (int) pix[nr][geo[nr][j].pnr].x, (int) pix[nr][geo[nr][j].pnr].y,cr_sz, nr, "yellow");
//}
drawcross (interp, (int) pix[nr][j].x, (int) pix[nr][j].y,cr_sz, nr, "red");
}
printf ("and %d corresponding 3d positions for frame %d\n", num_points,i_seq);
}
}
}
Tcl_Eval(interp, "update idletasks");
}
printf ("done\n\n");
free (imgf);
return TCL_OK;
}
int trajectories_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv)
/* draws crosses for detected points in a displayed image */
{
int k, intx1, inty1, intx2, inty2;
int i, anz1, anz2, m, j;
FILE *fp1;
char val[256];
vector *line1, *line2;
double color;
coord_2d p1[4], p2[4];
cr_sz = atoi(Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY));
fpp = fopen_r ("parameters/sequence.par");
/* name of sequence */
fscanf (fpp,"%d\n", &seq_first);
fscanf (fpp,"%d\n", &seq_last);
fclose (fpp);
sprintf (buf, "Show trajectories "); puts (buf);
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
for (i=seq_first; i<seq_last;i++)
{
if (i < 10) sprintf (val, "res/ptv_is.%1d", i);
else if (i < 100) sprintf (val, "res/ptv_is.%2d", i);
else sprintf (val, "res/ptv_is.%3d", i);
fp1 = fopen (val, "r");
color = ((double)(i-seq_first))/((double)(seq_last-2-seq_first));
fscanf (fp1,"%d\n", &anz1);
line1 = (vector *) malloc (anz1 * sizeof (vector));
for (j=0;j<anz1;j++) {
fscanf (fp1, "%d\n", &line1[j].p);
fscanf (fp1, "%d\n", &line1[j].n);
fscanf (fp1, "%lf\n", &line1[j].x1);
fscanf (fp1, "%lf\n", &line1[j].y1);
fscanf (fp1, "%lf\n", &line1[j].z1);
}
strcpy(val, "");
fclose (fp1);
/* read next time step */
if (i+1 < 10) sprintf (val, "res/ptv_is.%1d", i+1);
else if (i+1 < 100) sprintf (val, "res/ptv_is.%2d", i+1);
else sprintf (val, "res/ptv_is.%3d", i+1);
fp1 = fopen (val, "r");
fscanf (fp1,"%d\n", &anz2);
line2 = (vector *) calloc (anz2, sizeof (vector));
for (j=0;j<anz2;j++) {
fscanf (fp1, "%d\n", &line2[j].p);
fscanf (fp1, "%d\n", &line2[j].n);
fscanf (fp1, "%lf\n", &line2[j].x1);
fscanf (fp1, "%lf\n", &line2[j].y1);
fscanf (fp1, "%lf\n", &line2[j].z1);
}
fclose (fp1);
for(j=0;j<anz1;j++) {
m = line1[j].n;
if (m >= 0) {
for (k=0; k<n_img; k++)
{
img_coord (line1[j].x1, line1[j].y1, line1[j].z1, Ex[k],I[k], G[k], ap[k], mmp, &p1[k].x, &p1[k].y);
metric_to_pixel (p1[k].x, p1[k].y, imx,imy, pix_x,pix_y, &p1[k].x, &p1[k].y, chfield);
img_coord (line2[m].x1, line2[m].y1, line2[m].z1, Ex[k],I[k], G[k], ap[k], mmp, &p2[k].x, &p2[k].y);
metric_to_pixel (p2[k].x, p2[k].y, imx,imy, pix_x,pix_y, &p2[k].x, &p2[k].y, chfield);
if ( fabs( p2[k].x-zoom_x[k]) < imx/(2*zoom_f[k])
&& ( fabs(p2[k].y-zoom_y[k]) < imy/(2*zoom_f[k])) )
{
intx1 = (int)(imx/2+zoom_f[k]*(p1[k].x-zoom_x[k]));
inty1 = (int)(imy/2+zoom_f[k]*(p1[k].y-zoom_y[k]));
intx2 = (int)(imx/2+zoom_f[k]*(p2[k].x-zoom_x[k]));
inty2 = (int)(imy/2+zoom_f[k]*(p2[k].y-zoom_y[k]));
drawcross ( interp, intx1, inty1, cr_sz+1, k, "blue");
drawcross ( interp, intx2, inty2, cr_sz+1, k, "red");
drawvector (interp, intx1, inty1, intx2, inty2, 2, k, "green");
}
}
}
}
Tcl_Eval(interp, "update idletasks");
strcpy(val, "");
free(line1); free(line2);
} /* end of sequence loop */
sprintf(val, "...done");
Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 3");
Tcl_Eval(interp, ".text insert 3 $tbuf");
return TCL_OK;
}
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;
}
}
