29 #include "tnt/tnt_array1d_utils.h"
109 m_filePattern = pattern;
174 double &resid, GMatrix &atai,
AffineRadio &affrad) {
183 double rshift = radio.Shift();
184 double rgain = radio.Gain();
186 int maxPnts((pattern.
Samples()-2) * (pattern.
Lines()-2));
187 GMatrix a(maxPnts, 8);
188 GVector lvec(maxPnts);
193 for(
int line = 2 ; line < pattern.
Lines() ; line++) {
194 for(
int samp = 2; samp < pattern.
Samples() ; samp++) {
196 if(!pattern.
IsValid(samp, line))
continue;
197 if(!subsearch.
IsValid(samp, line))
continue;
198 if(!subsearch.
IsValid(samp + 1, line))
continue;
199 if(!subsearch.
IsValid(samp - 1, line))
continue;
200 if(!subsearch.
IsValid(samp, line - 1))
continue;
201 if(!subsearch.
IsValid(samp, line + 1))
continue;
204 double x0 = (double)(samp - tackSamp);
205 double y0 = (double)(line - tackLine);
208 double gxtemp = subsearch.
GetValue(samp + 1, line) -
210 double gytemp = subsearch.
GetValue(samp, line + 1) -
214 a[npts][1] = gxtemp * x0;
215 a[npts][2] = gxtemp * y0;
217 a[npts][4] = gytemp * x0;
218 a[npts][5] = gytemp * y0;
220 a[npts][7] = subsearch.
GetValue(samp, line);
222 double ell = pattern.
GetValue(samp, line) -
223 (((1.0 + rgain) * subsearch.
GetValue(samp, line)) +
227 resid += (ell * ell);
235 std::ostringstream mess;
237 <<
") criteria not met (" << npts <<
")";
238 return (
logError(NotEnoughPoints, mess.str().c_str()));
243 for(
int i = 0 ; i < 8 ; i++) {
244 for(
int j = 0 ; j < 8 ; j++) {
246 for (
int k = 0 ; k < npts ; k++) {
247 temp += (a[k][i] * a[k][j]);
257 GMatrix b = Identity(8);
259 atai =
Cholsl(atai, p, b, x);
262 QString mess =
"Cholesky Failed:: " + ie.
toString();
263 return (
logError(CholeskyFailed, mess));
268 for (
int i = 0 ; i < 8 ; i++) {
270 for (
int k = 0 ; k < npts ; k++) {
271 temp += a[k][i] * lvec[k];
276 GVector alpha(8, 0.0);
277 for (
int i = 0 ; i < 8 ; i++) {
278 for (
int k = 0 ; k < 8 ; k++) {
279 alpha[i] += atai[i][k] * atl[k];
287 QString mess =
"Affine failed: " + ie.
toString();
288 return (
logError(AffineNotInvertable, mess));
306 const GMatrix &atai) {
309 results.m_npts = npts;
314 for(
int r = 0 ; r < 8 ; r++) {
315 for(
int c = 0 ; c < 8 ; c++) {
316 kmat[r][c] = variance * atai[r][c];
319 results.m_variance = variance;
324 skmat[0][0] = kmat[0][0];
325 skmat[0][1] = kmat[0][3];
326 skmat[1][0] = kmat[3][0];
327 skmat[1][1] = kmat[3][3];
330 Jacobi(skmat, eigen, skmat);
332 for (
int i = 0 ; i < 2 ; i++) {
333 results.m_sevals[i] = eigen[i];
334 results.m_kmat[i] = kmat[i*3][i*3];
338 QString errmsg =
"Eigen Solution Failed:: " + ie.
toString();
339 results.m_status =
logError(EigenSolutionFailed, errmsg);
343 results.m_status = 0;
361 GMatrix aa = a.copy();
364 for(
int i = 0 ; i < nrows ; i++) {
365 for(
int j = i ; j < ncols ; j++) {
366 double sum = aa[i][j];
367 for(
int k = i-1 ; k >= 0 ; k--) {
368 sum -= (aa[i][k] * aa[j][k]);
374 "Choldc failed - matrix not postive definite",
380 aa[j][i] = sum / p[i];
400 const GMatrix &x)
const {
401 assert(b.dim1() == x.dim1());
402 assert(b.dim2() == x.dim2());
403 assert(p.dim1() == b.dim2());
408 GMatrix xout = x.copy();
409 for(
int j = 0 ; j < nrows ; j++) {
411 for(
int i = 0 ; i < ncols ; i++) {
412 double sum = b[j][i];
413 for(
int k = i-1 ; k >= 0 ; k--) {
414 sum -= (a[i][k] * xout[j][k]);
416 xout[j][i] = sum / p[i];
419 for (
int i = ncols-1 ; i >= 0 ; i--) {
420 double sum = xout[j][i];
421 for(
int k = i+1 ; k < ncols ; k++) {
422 sum -= (a[k][i] * xout[j][k]);
424 xout[j][i] = sum / p[i];
444 GMatrix &evecs,
const int &MaxIters)
const {
448 GMatrix v = Identity(nrows);
449 GVector d(nrows),b(nrows), z(nrows);
451 for(
int ip = 0 ; ip < nrows ; ip++) {
457 double n2(
double(nrows) *
double(nrows));
458 GMatrix aa = a.copy();
460 for ( ; nrot < MaxIters ; nrot++) {
462 for(
int ip = 0 ; ip < nrows-1 ; ip++) {
463 for(
int iq = ip+1 ; iq < nrows ; iq++) {
464 sm += fabs(aa[ip][iq]);
475 double thresh = (nrot < 3) ? (0.2 * sm/n2 ): 0.0;
476 for (
int ip = 0 ; ip < nrows-1 ; ip++) {
477 for (
int iq = ip+1 ; iq < nrows ; iq++) {
478 double g = 100.0 * fabs(aa[ip][iq]);
480 ( (fabs(d[ip]+g) == fabs(d[ip])) ) &&
481 ( (fabs(d[iq]+g) == fabs(d[iq])) ) ) {
484 else if ( fabs(aa[ip][iq]) > thresh ) {
485 double h = d[iq] - d[ip];
487 if ( (fabs(h)+g) == fabs(h) ) {
491 double theta = 0.5 * h / aa[ip][iq];
492 t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
493 if (theta < 0.0) t = -1.0 * t;
495 double c = 1./sqrt(1.0 + t * t);
497 double tau = s / (1.0 + c);
507 for (
int j = 0 ; j < ip-1 ; j++) {
510 aa[j][ip] = g - s * (h + g * tau);
511 aa[j][iq] = h + s * (g - h * tau);
514 for (
int j = ip+1 ; j < iq-1 ; j++) {
517 aa[ip][j] = g - s * (h + g * tau);
518 aa[j][iq] = h + s * (g - h * tau);
521 for (
int j = iq+1 ; j < ncols ; j++) {
524 aa[ip][j] = g - s * (h + g * tau);
525 aa[j][iq] = h + s * (g - h * tau);
528 for (
int j = 0 ; j < ncols ; j++) {
531 v[j][ip] = g - s * (h + g * tau);
532 v[j][iq] = h + s * (g - h * tau);
539 for (
int ip = 0 ; ip < nrows ; ip++) {
540 b[ip] = b[ip] + z[ip];
550 "Too many iterations in Jacobi",
556 GMatrix Gruen::Identity(
const int &ndiag)
const {
557 GMatrix ident(ndiag, ndiag, 0.0);
558 for (
int i = 0 ; i < ndiag ; i++) {
573 assert(evals.dim1() == evecs.dim1());
575 for (
int i = 0 ; i < n-1 ; i++ ) {
578 for (
int j = i+1 ; j < n ; j++) {
587 for (
int j = 0 ; j < n ; j++) {
589 evecs[j][i] = evecs[j][k];
632 npts, resid, atai, affrad);
636 if (analysis.isValid()) {
653 return (fit1 <= fit2);
698 Chip &fChip,
int startSamp,
699 int startLine,
int endSamp,
700 int endLine,
int bestSamp,
706 QString chipOut = m_filePattern;
744 Affine extractor(affine.m_affine);
745 sChip.
Extract(fChip, extractor);
748 if (!chipOut.isEmpty()) {
749 std::ostringstream ss;
750 ss <<
"C" << std::setw(6) << std::setfill(
'0') << m_callCount <<
"I"
751 << std::setw(3) << std::setfill(
'0') << m_nIters;
752 QString sfname = chipOut + ss.str().c_str() +
".cub";
761 int status =
algorithm(pChip, fChip, affine.m_radio, npts, resid,
765 return (
Status(matchpt.setStatus(status)));
769 matchpt.m_nIters = ++m_nIters;
770 if (m_nIters > m_maxIters) {
771 QString errmsg =
"Maximum Iterations exceeded";
772 matchpt.setStatus(
logError(MaxIterationsExceeded, errmsg));
780 matchpt.m_affine = affine;
782 matchpt.setStatus(matchpt.m_analysis.m_status);
783 if (matchpt.isValid()) {
804 QString mess =
"Affine invalid/not invertable";
805 matchpt.setStatus(
logError(AffineNotInvertable, mess));
822 regdef =
Pvl(
"$base/templates/autoreg/coreg.adaptgruen.p1515s3030.def");
847 for (
int e = 0 ; e < m_errors.
size() ; e++) {
848 algo += m_errors.
getNth(e).LogIt();
851 if (m_unclassified > 0) {
911 parms +=
ValidateKey(
"AffineScaleTolerance", m_scaleTol);
912 parms +=
ValidateKey(
"AffineShearTolerance", m_shearTol);
913 parms +=
ValidateKey(
"AffineTranslationTolerance", m_transTol);
918 parms +=
ParameterKey(
"RadioShiftTolerance", m_shiftTol);
920 parms +=
ParameterKey(
"RadioGainMinTolerance", m_rgainMinTol);
921 parms +=
ParameterKey(
"RadioGainMaxTolerance", m_rgainMaxTol);
923 parms +=
ValidateKey(
"DefaultRadioGain", m_defGain);
924 parms +=
ValidateKey(
"DefaultRadioShift", m_defShift);
966 if (!m_errors.
exists(gerrno)) {
970 m_errors.
get(gerrno).BumpIt();
993 if (m_prof.
Name().isEmpty()) m_prof.
setName(
"Gruen");
1017 m_totalIterations = 0;
1036 m_eigenStat.
Reset();
1038 m_shiftStat.
Reset();
1057 return ((BigInt) pts);
1110 if (point.isValid()) {
1111 if (point.m_nIters > m_maxIters) {
1112 QString errmsg =
"Maximum Iterations exceeded";
1113 return (
logError(MaxIterationsExceeded, errmsg));
1115 m_iterStat.
AddData(point.m_nIters);
1118 QString errmsg =
"Maximum Eigenvalue exceeded";
1119 return (
logError(MaxEigenExceeded, errmsg));
1121 m_eigenStat.
AddData(point.getEigen());
1123 double shift = point.m_affine.m_radio.Shift();
1124 if ( shift > m_shiftTol) {
1125 QString errmsg =
"Radiometric shift exceeds tolerance";
1126 return (
logError(RadShiftExceeded, errmsg));
1130 double gain = point.m_affine.m_radio.Gain();
1131 if (((1.0 + gain) > m_rgainMaxTol) ||
1132 ((1.0 + gain) < m_rgainMinTol)) {
1133 QString errmsg =
"Radiometric gain exceeds tolerances";
1134 return (
logError(RadGainExceeded, errmsg));
1141 QString errmsg =
"Affine distance exceeded";
1142 return (
logError(AffineDistExceeded, errmsg));
1145 return (point.getStatus());