ModelTracker::Transform ModelTracker::position(const ModelTracker::ImgPoint imagePoints[],const ModelTracker::Transform::Rotation& orientation) const
{
/* Build the least-squares linear system: */
Math::Matrix ata(3,3,0.0);
Math::Matrix atb(3,1,0.0);
const Projection::Matrix& pm=projection.getMatrix();
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
{
/* Transform the model point with the known orientation and projection matrix: */
Projection::HVector pmp=projection.transform(Projection::HVector(orientation.transform(modelPoints[mpi])));
/* Create the two equations for the model/image point pair: */
double eq[2][4];
for(int i=0;i<2;++i)
{
for(int j=0;j<3;++j)
eq[i][j]=pm(i,j)-imagePoints[mpi][i]*pm(3,j);
eq[i][3]=imagePoints[mpi][i]*pmp[3]-pmp[i];
}
/* Add the two equations to the least-squares system: */
for(int i=0;i<3;++i)
{
for(int j=0;j<3;++j)
ata(i,j)+=eq[0][i]*eq[0][j]+eq[1][i]*eq[1][j];
atb(i)+=eq[0][i]*eq[0][3]+eq[1][i]*eq[1][3];
}
}
/* Solve the least-squares system: */
Math::Matrix x=atb.divideFullPivot(ata);
/* Return the result transformation: */
return Transform(Transform::Vector(x(0),x(1),x(2)),orientation);
}
void SplineSeg3<D> :: GetCoeff (Vector & u) const
{
double t;
int i;
Point<D> p;
DenseMatrix a(6, 6);
DenseMatrix ata(6, 6);
Vector f(6);
u.SetSize(6);
// ata.SetSymmetric(1);
t = 0;
for (i = 1; i <= 5; i++, t += 0.25)
{
p = GetPoint (t);
a.Elem(i, 1) = p(0) * p(0);
a.Elem(i, 2) = p(1) * p(1);
a.Elem(i, 3) = p(0) * p(1);
a.Elem(i, 4) = p(0);
a.Elem(i, 5) = p(1);
a.Elem(i, 6) = 1;
}
a.Elem(6, 1) = 1;
CalcAtA (a, ata);
u = 0;
u.Elem(6) = 1;
a.MultTrans (u, f);
ata.Solve (f, u);
}
/* If no solution, check: 1. ret.size() < 3
* Or more precisely, 2. interPnt(ret[0], ret[1])
* in all the lines. (use (l.S - l.F).cross(p - l.F) > 0
*/
vector<Line> halfPlaneInter(vector<Line> lines) {
int sz = lines.size();
vector<double> ata(sz), ord(sz);
for (int i=0; i<sz; i++) {
ord[i] = i;
pdd d = lines[i].S - lines[i].F;
ata[i] = atan2(d.y, d.x);
}
sort(ALL(ord), [&](int i, int j) {
if (abs(ata[i] - ata[j]) < EPS) {
return cross(lines[i].S, lines[j].S, lines[i].F) < 0;
}
return ata[i] < ata[j];
});
vector<Line> fin;
for (int i=0; i<sz; i++) {
if (!i or fabs(ata[ord[i]] - ata[ord[i-1]]) > EPS) {
fin.PB(lines[ord[i]]);
}
}
deque<Line> dq;
for (int i=0; i<SZ(fin); i++) {
while(SZ(dq) >= 2 and
not isin(fin[i], dq[SZ(dq)-2], dq[SZ(dq)-1])) {
dq.pop_back();
}
while(SZ(dq) >= 2 and
not isin(fin[i], dq[0], dq[1])) {
dq.pop_front();
}
dq.push_back(fin[i]);
}
while (SZ(dq) >= 3 and
not isin(dq[0], dq[SZ(dq)-2], dq[SZ(dq)-1])) {
dq.pop_back();
}
while (SZ(dq) >= 3 and
not isin(dq[SZ(dq)-1], dq[0], dq[1])) {
dq.pop_front();
}
vector<Line> res(ALL(dq));
return res;
}
static void ktrcv(struct sk_buff *skb)
{
struct sk_buff *rskb;
struct aoedev *d;
struct aoe_hdr *aoe;
int major, minor;
aoe = (struct aoe_hdr *) skb_mac_header(skb);
major = be16_to_cpu(aoe->major);
minor = aoe->minor;
spin_lock(&lock);
for (d=devlist; d; d=d->next) {
if ((major != d->major && major != 0xffff) ||
(minor != d->minor && minor != 0xff) ||
(skb->dev != d->netdev))
continue;
rskb = make_response(skb, d->major, d->minor);
if (rskb == NULL)
continue;
switch (aoe->cmd) {
case AOECMD_ATA:
rskb = ata(d, rskb);
break;
case AOECMD_CFG:
rskb = cfg(d, rskb);
break;
default:
dev_kfree_skb(rskb);
continue;
}
if (rskb)
skb_queue_tail(&skb_outq, rskb);
}
spin_unlock(&lock);
dev_kfree_skb(skb);
}
END_TEST
START_TEST(test_ata)
{
double a[4][3] = {{1, 0, 1}, {2, 2, 4},{1, 2, 3}, {2, 4, 3}};
double b[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
double expected[3][3] = {{10, 14, 18}, {14, 24, 26},{18, 26, 35}};
int i, j;
ata((double*) a, (double *) b, 4, 3, 3);
for (i=0; i<3; i++){
for (j=0; j<3; j++){
ck_assert_msg(fabs(b[i][j] - expected[i][j]) < EPS, "wrong item \
[%d][%d] %f instead of %f", i,j,b[i][j],expected[i][j]);
}
}
}
void DepthCorrectionTool::buttonCallback(int buttonSlotIndex,Vrui::InputDevice::ButtonCallbackData* cbData)
{
if(cbData->newButtonState)
{
if(buttonSlotIndex==0)
{
/* Add a new averaged depth frame and calculate the best-fitting plane: */
DepthFrame df;
df.frame=Kinect::FrameBuffer(application->depthFrameSize[0],application->depthFrameSize[1],application->depthFrameSize[0]*application->depthFrameSize[1]*sizeof(float));
float foregroundCutoff=float(application->averageNumFrames)*0.5f;
float* afdPtr=application->averageFrameDepth;
float* affPtr=application->averageFrameForeground;
float* dfPtr=static_cast<float*>(df.frame.getBuffer());
typedef Geometry::PCACalculator<3>::Point PPoint;
typedef Geometry::PCACalculator<3>::Vector PVector;
Geometry::PCACalculator<3> pca;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++afdPtr,++affPtr,++dfPtr)
{
if(*affPtr>=foregroundCutoff)
{
/* Calculate the average depth value: */
*dfPtr=(*afdPtr)/(*affPtr);
/* Add the depth pixel to the PCA calculator: */
pca.accumulatePoint(PPoint(double(x)+0.5,double(y)+0.5,double(*dfPtr)));
}
else
*dfPtr=2047.0f;
}
/* Calculate the best-fitting plane: */
PPoint centroid=pca.calcCentroid();
pca.calcCovariance();
double evs[3];
pca.calcEigenvalues(evs);
PVector normal=pca.calcEigenvector(evs[2]);
df.plane=Plane(normal,centroid);
depthFrames.push_back(df);
}
else
{
/* Calculate the per-pixel affine depth correction coefficients: */
Kinect::FrameSource::PixelDepthCorrection* coefficients=new Kinect::FrameSource::PixelDepthCorrection[application->depthFrameSize[1]*application->depthFrameSize[0]];
Kinect::FrameSource::PixelDepthCorrection* cPtr=coefficients;
unsigned int pixelOffset=0;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
{
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++cPtr,++pixelOffset)
{
/* Build the least-squares linear regression system: */
Math::Matrix ata(2,2,0.0);
Math::Matrix atb(2,1,0.0);
unsigned int numFrames=0;
for(std::vector<DepthFrame>::iterator dfIt=depthFrames.begin();dfIt!=depthFrames.end();++dfIt)
{
double actual=double(static_cast<float*>(dfIt->frame.getBuffer())[pixelOffset]);
if(actual!=2047.0)
{
ata(0,0)+=actual*actual;
ata(0,1)+=actual;
ata(1,0)+=actual;
ata(1,1)+=1.0;
double expected=(dfIt->plane.getOffset()-(double(x)+0.5)*dfIt->plane.getNormal()[0]-(double(y)+0.5)*dfIt->plane.getNormal()[1])/dfIt->plane.getNormal()[2];
atb(0)+=actual*expected;
atb(1)+=expected;
++numFrames;
}
}
if(numFrames>=2)
{
/* Solve for the regression coefficients: */
Math::Matrix x=atb/ata;
cPtr->scale=float(x(0));
cPtr->offset=float(x(1));
}
else
{
/* Use identity correction if the pixel is underdetermined: */
cPtr->scale=1.0f;
cPtr->offset=0.0f;
}
}
}
/* Save the depth correction image: */
std::string depthCorrectionFileName=KINECT_CONFIG_DIR;
depthCorrectionFileName.push_back('/');
depthCorrectionFileName.append(KINECT_CAMERA_DEPTHCORRECTIONFILENAMEPREFIX);
depthCorrectionFileName.push_back('-');
depthCorrectionFileName.append(application->camera->getSerialNumber());
depthCorrectionFileName.append(".dat");
std::cout<<"Writing depth correction file "<<depthCorrectionFileName<<std::endl;
IO::FilePtr depthCorrectionFile(Vrui::openFile(depthCorrectionFileName.c_str(),IO::File::WriteOnly));
depthCorrectionFile->setEndianness(Misc::LittleEndian);
cPtr=coefficients;
for(unsigned int y=0;y<application->depthFrameSize[1];++y)
for(unsigned int x=0;x<application->depthFrameSize[0];++x,++cPtr)
{
depthCorrectionFile->write<float>(cPtr->scale);
depthCorrectionFile->write<float>(cPtr->offset);
}
/* Clean up: */
delete[] coefficients;
}
}
}
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;
}
}
