/* release the objects the killed animal has stolen */
void
relobj(struct monst *mtmp, int show)
{
struct obj *otmp, *otmp2;
for(otmp = mtmp->minvent; otmp; otmp = otmp2){
otmp->ox = mtmp->mx;
otmp->oy = mtmp->my;
otmp2 = otmp->nobj;
otmp->nobj = fobj;
fobj = otmp;
stackobj(fobj);
if(show & cansee(mtmp->mx,mtmp->my))
atl(otmp->ox,otmp->oy,otmp->olet);
}
mtmp->minvent = (struct obj *) 0;
if(mtmp->mgold || mtmp->data->mlet == 'L') {
long tmp;
tmp = (mtmp->mgold > 10000) ? 10000 : mtmp->mgold;
mkgold((long)(tmp + d(dlevel,30)), mtmp->mx, mtmp->my);
if(show & cansee(mtmp->mx,mtmp->my))
atl(mtmp->mx,mtmp->my,'$');
}
}
END_TEST
START_TEST(test_atl)
{
double a[4][3] = {{1, 0, 1}, {2, 2, 4},{1, 2, 3}, {2, 4, 3}};
double l[4] = {1,2,3,4};
double u[3];
double expected[3] = {16, 26, 30};
int i, j;
for (i=0; i<3; i++){
u[i] = 0.0;
}
atl((double *)u, (double *)a, (double *)l, 4, 3, 3);
for (i=0; i<3; i++){
ck_assert_msg(fabs(u[i] - expected[i]) < EPS, "wrong item \
[%d] %f instead of %f", i,u[i],expected[i]);
}
}
void
worm_move(struct monst *mtmp)
{
struct wseg *wtmp, *whd;
int tmp = mtmp->wormno;
wtmp = newseg();
wtmp->wx = mtmp->mx;
wtmp->wy = mtmp->my;
wtmp->nseg = 0;
(whd = wheads[tmp])->nseg = wtmp;
wheads[tmp] = wtmp;
if (cansee(whd->wx, whd->wy)) {
unpmon(mtmp);
atl(whd->wx, whd->wy, '~');
whd->wdispl = 1;
} else
whd->wdispl = 0;
if (wgrowtime[tmp] <= moves) {
if (!wgrowtime[tmp])
wgrowtime[tmp] = moves + rnd(5);
else
wgrowtime[tmp] += 2 + rnd(15);
mtmp->mhpmax += 3;
mtmp->mhp += 3;
return;
}
whd = wsegs[tmp];
wsegs[tmp] = whd->nseg;
remseg(whd);
}
void
pwseg(struct wseg *wtmp)
{
if (!wtmp->wdispl) {
atl(wtmp->wx, wtmp->wy, '~');
wtmp->wdispl = 1;
}
}
int findit(void)
{ // returns number of things found
int num;
xchar zx, zy;
struct trap *ttmp;
struct monst *mtmp;
xchar lx, hx, ly, hy;
if (u.uswallow)
return 0;
for (lx = u.ux; (num = levl[lx - 1][u.uy].typ) && num != CORR; lx--);
for (hx = u.ux; (num = levl[hx + 1][u.uy].typ) && num != CORR; hx++);
for (ly = u.uy; (num = levl[u.ux][ly - 1].typ) && num != CORR; ly--);
for (hy = u.uy; (num = levl[u.ux][hy + 1].typ) && num != CORR; hy++);
num = 0;
for (zy = ly; zy <= hy; zy++)
for (zx = lx; zx <= hx; zx++) {
if (levl[zx][zy].typ == SDOOR) {
levl[zx][zy].typ = DOOR;
atl(zx, zy, '+');
num++;
} else if (levl[zx][zy].typ == SCORR) {
levl[zx][zy].typ = CORR;
atl(zx, zy, CORR_SYM);
num++;
} else if ((ttmp = t_at(zx, zy)) != NULL) {
if (ttmp->ttyp == PIERC) {
(void) makemon(PM_PIERCER, zx, zy);
num++;
deltrap(ttmp);
} else if (!ttmp->tseen) {
ttmp->tseen = 1;
if (!vism_at(zx, zy))
atl(zx, zy, '^');
num++;
}
} else if ((mtmp = m_at(zx, zy)) != NULL)
if (mtmp->mimic) {
seemimic(mtmp);
num++;
}
}
return num;
}
int dosearch(void)
{
xchar x, y;
struct trap *trap;
struct monst *mtmp;
if (u.uswallow)
pline("What are you looking for? The exit?");
else
for (x = u.ux - 1; x < u.ux + 2; x++)
for (y = u.uy - 1; y < u.uy + 2; y++)
if (x != u.ux || y != u.uy) {
if (levl[x][y].typ == SDOOR) {
if (rn2(7))
continue;
levl[x][y].typ = DOOR;
levl[x][y].seen = 0; // force prl
prl(x, y);
nomul(0);
} else if (levl[x][y].typ == SCORR) {
if (rn2(7))
continue;
levl[x][y].typ = CORR;
levl[x][y].seen = 0; // force prl
prl(x, y);
nomul(0);
} else {
// Be careful not to find anything in an SCORR or SDOOR
if ((mtmp = m_at(x, y)) != NULL) {
if (mtmp->mimic) {
seemimic(mtmp);
pline("You find a mimic.");
return 1;
}
}
for (trap = ftrap; trap; trap = trap->ntrap) {
if (trap->tx == x && trap->ty == y && !trap->tseen && !rn2(8)) {
nomul(0);
pline("You find a%s.", traps[trap->ttyp]);
if (trap->ttyp == PIERC) {
deltrap(trap);
(void) makemon(PM_PIERCER, x, y);
return 1;
}
trap->tseen = 1;
if (!vism_at(x, y))
atl(x, y, '^');
}
}
}
}
return 1;
}
/* drop a rock and remove monster */
static void
monstone(struct monst *mdef)
{
if (strchr(mlarge, mdef->data->mlet))
mksobj_at(ENORMOUS_ROCK, mdef->mx, mdef->my);
else
mksobj_at(ROCK, mdef->mx, mdef->my);
if (cansee(mdef->mx, mdef->my)) {
unpmon(mdef);
atl(mdef->mx, mdef->my, fobj->olet);
}
mondead(mdef);
}
/* drop (perhaps) a cadaver and remove monster */
void
mondied(struct monst *mdef)
{
const struct permonst *pd = mdef->data;
if (letter(pd->mlet) && rn2(3)) {
(void) mkobj_at(pd->mlet, mdef->mx, mdef->my);
if (cansee(mdef->mx, mdef->my)) {
unpmon(mdef);
atl(mdef->mx, mdef->my, fobj->olet);
}
stackobj(fobj);
}
mondead(mdef);
}
int main(int ac, char *av[])
{
if(ac > 2)
{
std::auto_ptr<QCoreApplication> cmd;
{
for (int i = 1; i < ac; ++i)
if (std::string(av[i]) == std::string("--action=cnt") ||
std::string(av[i]) == std::string("--action=vis"))
{
cmd.reset(new QApplication(ac, av));
std::cout << "Starting GUI-based command line interface." << std::endl;
break;
}
if(!cmd.get())
cmd.reset(new QCoreApplication(ac, av));
}
cmd->setOrganizationName("LabSolver");
cmd->setApplicationName("DSI Studio");
try
{
std::cout << "DSI Studio " << __DATE__ << ", Fang-Cheng Yeh" << std::endl;
// options for general options
po::options_description desc("reconstruction options");
desc.add_options()
("help", "help message")
("action", po::value<std::string>(), "rec:diffusion reconstruction trk:fiber tracking")
("source", po::value<std::string>(), "assign the .src or .fib file name")
;
po::variables_map vm;
po::store(po::command_line_parser(ac, av).options(desc).allow_unregistered().run(),vm);
if (vm.count("help"))
{
std::cout << "example: perform reconstruction" << std::endl;
std::cout << " --action=rec --source=test.src.gz --method=4 " << std::endl;
std::cout << "example: perform fiber tracking" << std::endl;
std::cout << " --action=trk --source=test.src.gz.fib.gz --method=0 --fiber_count=5000" << std::endl;
return 1;
}
if (!vm.count("action") || !vm.count("source"))
{
std::cout << "invalid command, use --help for more detail" << std::endl;
return 1;
}
if(vm["action"].as<std::string>() == std::string("rec"))
return rec(ac,av);
if(vm["action"].as<std::string>() == std::string("trk"))
return trk(ac,av);
if(vm["action"].as<std::string>() == std::string("src"))
return src(ac,av);
if(vm["action"].as<std::string>() == std::string("ana"))
return ana(ac,av);
if(vm["action"].as<std::string>() == std::string("exp"))
return exp(ac,av);
if(vm["action"].as<std::string>() == std::string("atl"))
return atl(ac,av);
if(vm["action"].as<std::string>() == std::string("cnt"))
return cnt(ac,av);
if(vm["action"].as<std::string>() == std::string("vis"))
return vis(ac,av);
std::cout << "invalid command, use --help for more detail" << std::endl;
return 1;
}
catch(const std::exception& e ) {
std::cout << e.what() << std::endl;
}
catch(...)
{
std::cout << "unknown error occured" << std::endl;
}
return 1;
}
QApplication a(ac,av);
a.setOrganizationName("LabSolver");
a.setApplicationName("DSI Studio");
QFont font;
font.setFamily(QString::fromUtf8("Arial"));
a.setFont(font);
// load template
if(!fa_template_imp.load_from_file())
{
QMessageBox::information(0,"Error","Cannot find HCP488_QA.nii.gz in file directory",0);
return false;
}
// load atlas
load_atlas();
MainWindow w;
w.setFont(font);
w.show();
w.setWindowTitle(QString("DSI Studio ") + __DATE__ + " build");
return a.exec();
}
void killed(struct monst *mtmp)
{
#define NEW_SCORING
int tmp;
int tmp2;
int nk;
int x;
int y;
struct permonst *mdat = mtmp->data;
if(mtmp->cham != 0) {
mdat = PM_CHAM;
}
if(Blind != 0) {
pline("You destroy it!");
}
else {
if(mtmp->mtame != 0) {
pline("You destroy %s!", amonnam(mtmp, "poor"));
}
else {
pline("You destroy %s!", monnam(mtmp));
}
}
if(u.umconf != 0) {
if(Blind == 0) {
pline("Your hands stop clowing blue.");
u.umconf = 0;
}
}
/* Count killed monsters */
#define MAXMONNO 100
/* In case we cannot find it in mons */
nk = 1;
/* Index in mons array (if not 'd', '@', ...) */
tmp = mdat - mons;
if((tmp >= 0) && (tmp < (CMNUM + 2))) {
extern char fut_geno[];
++u.nr_killed[tmp];
nk = u.nr_killed[tmp];
if((nk > MAXMONNO) && (index(fut_geno, mdat->mlet) == 0)) {
charcat(fut_geno, mdat->mlet);
}
}
/* Punish bad behaviour */
if(mdat->mlet == '@') {
Telepat = 0;
u.uluck -= 2;
}
if((mtmp->mpeaceful != 0) || (mtmp->mtame != 0)) {
--u.uluck;
}
if(mdat->mlet == 'u') {
u.uluck -= 5;
}
/* Give experience points */
tmp = 1 + (mdat->mlevel * mdat->mlevel);
if(mdat->ac < 3) {
tmp += (2 * (7 - mdat->ac));
}
if(index("AcsSDXaeRTVWU&In:P", mdat->mlet) != 0) {
tmp += (2 * mdat->mlevel);
}
if(index("DeV&P", mdat->mlet) != 0) {
tmp += (7 * mdat->mlevel);
}
if(mdat->mlevel > 6) {
tmp += 50;
}
#ifdef NEW_SCORING
/*
* ------- Recent addition: make number of points decrease
* when this is not the first of this kind
*/
int ul = u.ulevel;
int ml = mdat->mlevel;
/* Points are given based on present and future level */
if(ul < 14) {
for(tmp2 = 0; (tmp2 == 0) || ((ul + tmp2) <= ml); ++tmp2) {
if(tmp <= 0) {
if(((u.uexp + 1) + ((tmp + (0)) / nk)) >= (10 * pow(2, (unsigned)(ul - 1)))) {
++ul;
if(ul == 14) {
break;
}
}
}
else {
if(((u.uexp + 1) + ((tmp + (4 << (tmp2 - 1))) / nk)) >= (10 * pow(2, (unsigned)(ul - 1)))) {
++ul;
if(ul == 14) {
break;
}
}
}
}
}
tmp2 = (ml - ul) - 1;
if(tmp2 < 0) {
tmp = (tmp + (0)) / nk;
}
else {
tmp = (tmp + (4 << tmp2)) / nk;
}
if(tmp == 0) {
tmp = 1;
}
/*
* Note: ul is not necessarily the future value of u.ulevel
* ------- End of recent valuation change -------
*/
#endif
u.uexp += tmp;
u.urexp += (4 * tmp);
flags.botl = 1;
while((u.ulevel < 14) && (u.uexp >= (10 * pow(2, u.ulevel - 1)))) {
++u.ulevel;
pline("Welcome to level %d.", u.ulevel);
tmp = rnd(30);
if(tmp < 3) {
tmp = rnd(10);
}
u.uhpmax += tmp;
u.uhp += tmp;
flags.botl = 1;
}
/* Dispose of monster and make cadaver */
x = mtmp->mx;
y = mtmp->my;
mondead(mtmp);
tmp = mdat->mlet;
if(tmp == 'm') {
/* He killed a minotaur, give him a wand of digging */
/* Note: The dead minotaur will be on top of it! */
mksobj_at(WAND_SYM, WAN_DIGGING, x, y);
/*
* if(cansee(x, y) != 0) {
* atl(x, y, fobj->olet);
* }
*/
stackobj(fobj);
}
#ifndef NOWORM
else if(tmp == 'w') {
mksobj_at(WEAPON_SYM, WORM_TOOTH, x, y);
stackobj(fobj);
}
#endif
else {
if((letter(tmp) == 0) || (rn2(3) == 0)) {
tmp = 0;
}
if(levl[x][y].typ >= DOOR) {
/* Might be a mimic in wall */
if((x != u.ux) || (y != u.uy)) {
/* Might be here after swallowed */
if((index("NTVm&", mdat->mlet) != NULL) || (rn2(5) != 0)) {
mkobj_at(tmp, x, y);
if(cansee(x, y) != 0) {
atl(x, y, fobj->olet);
}
stackobj(fobj);
}
}
}
}
}
//
// Add items to the history
//
void pr(node *cell) {
atl(&history, cell);
}
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;
}
}
