| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | void slaFitxy ( int itype, int np,
|
|---|
| 4 | double xye[][2], double xym[][2], double coeffs[6],
|
|---|
| 5 | int *j )
|
|---|
| 6 | /*
|
|---|
| 7 | ** - - - - - - - - -
|
|---|
| 8 | ** s l a F i t x y
|
|---|
| 9 | ** - - - - - - - - -
|
|---|
| 10 | **
|
|---|
| 11 | ** Fit a linear model to relate two sets of [x,y] coordinates.
|
|---|
| 12 | **
|
|---|
| 13 | ** Given:
|
|---|
| 14 | ** itype int type of model: 4 or 6 (note 1)
|
|---|
| 15 | ** np int number of samples (note 2)
|
|---|
| 16 | ** xye double[np][2] expected [x,y] for each sample
|
|---|
| 17 | ** xym double[np][2] measured [x,y] for each sample
|
|---|
| 18 | **
|
|---|
| 19 | ** Returned:
|
|---|
| 20 | ** coeffs double[6] coefficients of model (note 3)
|
|---|
| 21 | ** *j int status: 0 = OK
|
|---|
| 22 | ** -1 = illegal itype
|
|---|
| 23 | ** -2 = insufficient data
|
|---|
| 24 | ** -3 = singular solution
|
|---|
| 25 | **
|
|---|
| 26 | ** Notes:
|
|---|
| 27 | **
|
|---|
| 28 | ** 1) itype, which must be either 4 or 6, selects the type of model
|
|---|
| 29 | ** fitted. Both allowed itype values produce a model coeffs which
|
|---|
| 30 | ** consists of six coefficients, namely the zero points and, for
|
|---|
| 31 | ** each of xe and ye, the coefficient of xm and ym. For itype=6,
|
|---|
| 32 | ** all six coefficients are independent, modelling squash and shear
|
|---|
| 33 | ** as well as origin, scale, and orientation. However, itype=4
|
|---|
| 34 | ** selects the "solid body rotation" option; the model coeffs
|
|---|
| 35 | ** still consists of the same six coefficients, but now two of
|
|---|
| 36 | ** them are used twice (appropriately signed). Origin, scale
|
|---|
| 37 | ** and orientation are still modelled, but not squash or shear -
|
|---|
| 38 | ** the units of x and y have to be the same.
|
|---|
| 39 | **
|
|---|
| 40 | ** 2) For nc=4, np must be at least 2. For nc=6, np must be at
|
|---|
| 41 | ** least 3.
|
|---|
| 42 | **
|
|---|
| 43 | ** 3) The model is returned in the array coeffs. Naming the
|
|---|
| 44 | ** elements of coeffs as follows:
|
|---|
| 45 | **
|
|---|
| 46 | ** coeffs[0] = a
|
|---|
| 47 | ** coeffs[1] = b
|
|---|
| 48 | ** coeffs[2] = c
|
|---|
| 49 | ** coeffs[3] = d
|
|---|
| 50 | ** coeffs[4] = e
|
|---|
| 51 | ** coeffs[5] = f
|
|---|
| 52 | **
|
|---|
| 53 | ** The model is:
|
|---|
| 54 | **
|
|---|
| 55 | ** xe = a + b*xm + c*ym
|
|---|
| 56 | ** ye = d + e*xm + f*ym
|
|---|
| 57 | **
|
|---|
| 58 | ** For the "solid body rotation" option (itype=4), the
|
|---|
| 59 | ** magnitudes of b and f, and of c and e, are equal. The
|
|---|
| 60 | ** signs of these coefficients depend on whether there is a
|
|---|
| 61 | ** sign reversal between xe,ye and xm,ym. Fits are performed
|
|---|
| 62 | ** with and without a sign reversal and the best one chosen.
|
|---|
| 63 | **
|
|---|
| 64 | ** 4) Error status values j=-1 and -2 leave coeffs unchanged;
|
|---|
| 65 | ** If j=-3 coeffs may have been changed.
|
|---|
| 66 | **
|
|---|
| 67 | ** See also slaPxy, slaInvf, slaXy2xy, slaDcmpf
|
|---|
| 68 | **
|
|---|
| 69 | ** Called: slaDmat, slaDmxv
|
|---|
| 70 | **
|
|---|
| 71 | ** Last revision: 31 October 1993
|
|---|
| 72 | **
|
|---|
| 73 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 74 | */
|
|---|
| 75 | {
|
|---|
| 76 | int i, jstat;
|
|---|
| 77 | int iw[4];
|
|---|
| 78 | int nsol;
|
|---|
| 79 | double p, sxe, sxexm, sxeym, sye, syeym, syexm, sxm,
|
|---|
| 80 | sym, sxmxm, sxmym, symym, xe, ye,
|
|---|
| 81 | xm, ym, v[4], dm3[3][3], dm4[4][4], det,
|
|---|
| 82 | sgn, sxxyy, sxyyx, sx2y2, a, b, c, d,
|
|---|
| 83 | sdr2, xr, yr, aold, bold, cold, dold, sold;
|
|---|
| 84 |
|
|---|
| 85 | /* Preset the status */
|
|---|
| 86 | *j = 0;
|
|---|
| 87 |
|
|---|
| 88 | /* Float the number of samples */
|
|---|
| 89 | p = (double) np;
|
|---|
| 90 |
|
|---|
| 91 | /* Check itype */
|
|---|
| 92 | if ( itype == 6 ) {
|
|---|
| 93 |
|
|---|
| 94 | /*
|
|---|
| 95 | ** Six-coefficient linear model
|
|---|
| 96 | ** ----------------------------
|
|---|
| 97 | */
|
|---|
| 98 |
|
|---|
| 99 | /* Check enough samples */
|
|---|
| 100 | if ( np >= 3 ) {
|
|---|
| 101 |
|
|---|
| 102 | /* Form summations */
|
|---|
| 103 | sxe = 0.0;
|
|---|
| 104 | sxexm = 0.0;
|
|---|
| 105 | sxeym = 0.0;
|
|---|
| 106 | sye = 0.0;
|
|---|
| 107 | syeym = 0.0;
|
|---|
| 108 | syexm = 0.0;
|
|---|
| 109 | sxm = 0.0;
|
|---|
| 110 | sym = 0.0;
|
|---|
| 111 | sxmxm = 0.0;
|
|---|
| 112 | sxmym = 0.0;
|
|---|
| 113 | symym = 0.0;
|
|---|
| 114 |
|
|---|
| 115 | for ( i = 0; i < np; i++ ) {
|
|---|
| 116 | xe = xye[i][0];
|
|---|
| 117 | ye = xye[i][1];
|
|---|
| 118 | xm = xym[i][0];
|
|---|
| 119 | ym = xym[i][1];
|
|---|
| 120 | sxe = sxe + xe;
|
|---|
| 121 | sxexm = sxexm + xe * xm;
|
|---|
| 122 | sxeym = sxeym + xe * ym;
|
|---|
| 123 | sye = sye + ye;
|
|---|
| 124 | syeym = syeym + ye * ym;
|
|---|
| 125 | syexm = syexm + ye * xm;
|
|---|
| 126 | sxm = sxm + xm;
|
|---|
| 127 | sym = sym + ym;
|
|---|
| 128 | sxmxm = sxmxm + xm * xm;
|
|---|
| 129 | sxmym = sxmym + xm * ym;
|
|---|
| 130 | symym = symym + ym * ym;
|
|---|
| 131 | }
|
|---|
| 132 |
|
|---|
| 133 | /* Solve for a,b,c in xe = a + b*xm + c*ym */
|
|---|
| 134 | v[0] = sxe;
|
|---|
| 135 | v[1] = sxexm;
|
|---|
| 136 | v[2] = sxeym;
|
|---|
| 137 | dm3[0][0] = p;
|
|---|
| 138 | dm3[0][1] = sxm;
|
|---|
| 139 | dm3[0][2] = sym;
|
|---|
| 140 | dm3[1][0] = sxm;
|
|---|
| 141 | dm3[1][1] = sxmxm;
|
|---|
| 142 | dm3[1][2] = sxmym;
|
|---|
| 143 | dm3[2][0] = sym;
|
|---|
| 144 | dm3[2][1] = sxmym;
|
|---|
| 145 | dm3[2][2] = symym;
|
|---|
| 146 | slaDmat ( 3, dm3[0], v, &det, &jstat, iw);
|
|---|
| 147 | if (jstat == 0) {
|
|---|
| 148 | for ( i = 0; i < 3; i++ ) {
|
|---|
| 149 | coeffs[i] = v[i];
|
|---|
| 150 | }
|
|---|
| 151 |
|
|---|
| 152 | /* Solve for d,e,f in ye = d + e*xm + f*ym */
|
|---|
| 153 | v[0] = sye;
|
|---|
| 154 | v[1] = syexm;
|
|---|
| 155 | v[2] = syeym;
|
|---|
| 156 | slaDmxv ( dm3, v, &coeffs[3] );
|
|---|
| 157 | } else {
|
|---|
| 158 |
|
|---|
| 159 | /* No 6-coefficient solution possible */
|
|---|
| 160 | *j = -3;
|
|---|
| 161 | }
|
|---|
| 162 | } else {
|
|---|
| 163 |
|
|---|
| 164 | /* Insufficient data for 6-coefficient fit */
|
|---|
| 165 | *j = -2;
|
|---|
| 166 | }
|
|---|
| 167 | } else if ( itype == 4 ) {
|
|---|
| 168 |
|
|---|
| 169 | /*
|
|---|
| 170 | ** Four-coefficient solid body rotation model
|
|---|
| 171 | ** ------------------------------------------
|
|---|
| 172 | */
|
|---|
| 173 |
|
|---|
| 174 | /* Check enough samples */
|
|---|
| 175 | if ( np >= 2 ) {
|
|---|
| 176 |
|
|---|
| 177 | /* Try two solutions, first without then with flip in x */
|
|---|
| 178 | for ( nsol = 1; nsol <= 2; nsol++ ) {
|
|---|
| 179 | sgn = ( nsol == 1 ) ? 1.0 : -1.0;
|
|---|
| 180 |
|
|---|
| 181 | /* Form summations*/
|
|---|
| 182 | sxe = 0.0;
|
|---|
| 183 | sxxyy = 0.0;
|
|---|
| 184 | sxyyx = 0.0;
|
|---|
| 185 | sye = 0.0;
|
|---|
| 186 | sxm = 0.0;
|
|---|
| 187 | sym = 0.0;
|
|---|
| 188 | sx2y2 = 0.0;
|
|---|
| 189 | for ( i = 0; i < np; i++ ) {
|
|---|
| 190 | xe = xye[i][0] * sgn;
|
|---|
| 191 | ye = xye[i][1];
|
|---|
| 192 | xm = xym[i][0];
|
|---|
| 193 | ym = xym[i][1];
|
|---|
| 194 | sxe = sxe + xe;
|
|---|
| 195 | sxxyy = sxxyy + xe * xm + ye * ym;
|
|---|
| 196 | sxyyx = sxyyx + xe * ym - ye * xm;
|
|---|
| 197 | sye = sye + ye;
|
|---|
| 198 | sxm = sxm + xm;
|
|---|
| 199 | sym = sym + ym;
|
|---|
| 200 | sx2y2 = sx2y2 + xm * xm + ym * ym;
|
|---|
| 201 | }
|
|---|
| 202 |
|
|---|
| 203 | /*
|
|---|
| 204 | ** Solve for a,b,c,d in: +/- xe = a + b*xm - c*ym
|
|---|
| 205 | ** + ye = d + c*xm + b*ym
|
|---|
| 206 | */
|
|---|
| 207 | v[0] = sxe;
|
|---|
| 208 | v[1] = sxxyy;
|
|---|
| 209 | v[2] = sxyyx;
|
|---|
| 210 | v[3] = sye;
|
|---|
| 211 | dm4[0][0] = p;
|
|---|
| 212 | dm4[0][1] = sxm;
|
|---|
| 213 | dm4[0][2] = -sym;
|
|---|
| 214 | dm4[0][3] = 0.0;
|
|---|
| 215 | dm4[1][0] = sxm;
|
|---|
| 216 | dm4[1][1] = sx2y2;
|
|---|
| 217 | dm4[1][2] = 0.0;
|
|---|
| 218 | dm4[1][3] = sym;
|
|---|
| 219 | dm4[2][0] = sym;
|
|---|
| 220 | dm4[2][1] = 0.0;
|
|---|
| 221 | dm4[2][2] = -sx2y2;
|
|---|
| 222 | dm4[2][3] = -sxm;
|
|---|
| 223 | dm4[3][0] = 0.0;
|
|---|
| 224 | dm4[3][1] = sym;
|
|---|
| 225 | dm4[3][2] = sxm;
|
|---|
| 226 | dm4[3][3] = p;
|
|---|
| 227 | slaDmat ( 4, dm4[0], v, &det, &jstat, iw );
|
|---|
| 228 | if ( jstat == 0 ) {
|
|---|
| 229 | a = v[0];
|
|---|
| 230 | b = v[1];
|
|---|
| 231 | c = v[2];
|
|---|
| 232 | d = v[3];
|
|---|
| 233 |
|
|---|
| 234 | /* Determine sum of radial errors squared */
|
|---|
| 235 | sdr2 = 0.0;
|
|---|
| 236 | for ( i = 0; i < np; i++ ) {
|
|---|
| 237 | xm = xym[i][0];
|
|---|
| 238 | ym = xym[i][1];
|
|---|
| 239 | xr = a + b * xm - c * ym - xye[i][0] * sgn;
|
|---|
| 240 | yr = d + c * xm + b * ym- xye[i][1];
|
|---|
| 241 | sdr2 = sdr2 + xr * xr + yr * yr;
|
|---|
| 242 | }
|
|---|
| 243 | } else {
|
|---|
| 244 |
|
|---|
| 245 | /* Singular: set flag */
|
|---|
| 246 | sdr2 = -1.0;
|
|---|
| 247 | }
|
|---|
| 248 |
|
|---|
| 249 | /* If first pass and non-singular, save variables */
|
|---|
| 250 | if ( nsol == 1 && jstat == 0 ) {
|
|---|
| 251 | aold = a;
|
|---|
| 252 | bold = b;
|
|---|
| 253 | cold = c;
|
|---|
| 254 | dold = d;
|
|---|
| 255 | sold = sdr2;
|
|---|
| 256 | }
|
|---|
| 257 | }
|
|---|
| 258 |
|
|---|
| 259 | /* Pick the best of the two solutions */
|
|---|
| 260 | if ( sold >= 0.0 && sold <= sdr2 ) {
|
|---|
| 261 | coeffs[0] = aold;
|
|---|
| 262 | coeffs[1] = bold;
|
|---|
| 263 | coeffs[2] = -cold;
|
|---|
| 264 | coeffs[3] = dold;
|
|---|
| 265 | coeffs[4] = cold;
|
|---|
| 266 | coeffs[5] = bold;
|
|---|
| 267 | } else if ( jstat == 0 ) {
|
|---|
| 268 | coeffs[0] = -a;
|
|---|
| 269 | coeffs[1] = -b;
|
|---|
| 270 | coeffs[2] = c;
|
|---|
| 271 | coeffs[3] = d;
|
|---|
| 272 | coeffs[4] = c;
|
|---|
| 273 | coeffs[5] = b;
|
|---|
| 274 | } else {
|
|---|
| 275 |
|
|---|
| 276 | /* No 4-coefficient fit possible */
|
|---|
| 277 | *j = -3;
|
|---|
| 278 | }
|
|---|
| 279 | } else {
|
|---|
| 280 |
|
|---|
| 281 | /* Insufficient data for 4-coefficient fit */
|
|---|
| 282 | *j = -2;
|
|---|
| 283 | }
|
|---|
| 284 | } else {
|
|---|
| 285 |
|
|---|
| 286 | /* Illegal itype - not 4 or 6 */
|
|---|
| 287 | *j = -1;
|
|---|
| 288 | }
|
|---|
| 289 | }
|
|---|
