1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22 package org.hipparchus.optim.nonlinear.vector.leastsquares;
23
24 import java.util.Arrays;
25
26 import org.hipparchus.exception.MathIllegalStateException;
27 import org.hipparchus.linear.ArrayRealVector;
28 import org.hipparchus.linear.RealMatrix;
29 import org.hipparchus.optim.ConvergenceChecker;
30 import org.hipparchus.optim.LocalizedOptimFormats;
31 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation;
32 import org.hipparchus.util.FastMath;
33 import org.hipparchus.util.Incrementor;
34 import org.hipparchus.util.Precision;
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114 public class LevenbergMarquardtOptimizer implements LeastSquaresOptimizer {
115
116
117 private static final double TWO_EPS = 2 * Precision.EPSILON;
118
119
120
121 private final double initialStepBoundFactor;
122
123 private final double costRelativeTolerance;
124
125 private final double parRelativeTolerance;
126
127
128 private final double orthoTolerance;
129
130 private final double qrRankingThreshold;
131
132
133
134
135
136
137
138
139
140
141
142
143 public LevenbergMarquardtOptimizer() {
144 this(100, 1e-10, 1e-10, 1e-10, Precision.SAFE_MIN);
145 }
146
147
148
149
150
151
152
153
154
155
156
157
158 public LevenbergMarquardtOptimizer(
159 final double initialStepBoundFactor,
160 final double costRelativeTolerance,
161 final double parRelativeTolerance,
162 final double orthoTolerance,
163 final double qrRankingThreshold) {
164 this.initialStepBoundFactor = initialStepBoundFactor;
165 this.costRelativeTolerance = costRelativeTolerance;
166 this.parRelativeTolerance = parRelativeTolerance;
167 this.orthoTolerance = orthoTolerance;
168 this.qrRankingThreshold = qrRankingThreshold;
169 }
170
171
172
173
174
175
176
177
178
179
180
181 public LevenbergMarquardtOptimizer withInitialStepBoundFactor(double newInitialStepBoundFactor) {
182 return new LevenbergMarquardtOptimizer(
183 newInitialStepBoundFactor,
184 costRelativeTolerance,
185 parRelativeTolerance,
186 orthoTolerance,
187 qrRankingThreshold);
188 }
189
190
191
192
193
194 public LevenbergMarquardtOptimizer withCostRelativeTolerance(double newCostRelativeTolerance) {
195 return new LevenbergMarquardtOptimizer(
196 initialStepBoundFactor,
197 newCostRelativeTolerance,
198 parRelativeTolerance,
199 orthoTolerance,
200 qrRankingThreshold);
201 }
202
203
204
205
206
207
208 public LevenbergMarquardtOptimizer withParameterRelativeTolerance(double newParRelativeTolerance) {
209 return new LevenbergMarquardtOptimizer(
210 initialStepBoundFactor,
211 costRelativeTolerance,
212 newParRelativeTolerance,
213 orthoTolerance,
214 qrRankingThreshold);
215 }
216
217
218
219
220
221
222 public LevenbergMarquardtOptimizer withOrthoTolerance(double newOrthoTolerance) {
223 return new LevenbergMarquardtOptimizer(
224 initialStepBoundFactor,
225 costRelativeTolerance,
226 parRelativeTolerance,
227 newOrthoTolerance,
228 qrRankingThreshold);
229 }
230
231
232
233
234
235
236
237
238 public LevenbergMarquardtOptimizer withRankingThreshold(double newQRRankingThreshold) {
239 return new LevenbergMarquardtOptimizer(
240 initialStepBoundFactor,
241 costRelativeTolerance,
242 parRelativeTolerance,
243 orthoTolerance,
244 newQRRankingThreshold);
245 }
246
247
248
249
250
251
252
253 public double getInitialStepBoundFactor() {
254 return initialStepBoundFactor;
255 }
256
257
258
259
260
261
262
263 public double getCostRelativeTolerance() {
264 return costRelativeTolerance;
265 }
266
267
268
269
270
271
272
273 public double getParameterRelativeTolerance() {
274 return parRelativeTolerance;
275 }
276
277
278
279
280
281
282
283 public double getOrthoTolerance() {
284 return orthoTolerance;
285 }
286
287
288
289
290
291
292
293 public double getRankingThreshold() {
294 return qrRankingThreshold;
295 }
296
297
298 @Override
299 public Optimum optimize(final LeastSquaresProblem problem) {
300
301 final int nR = problem.getObservationSize();
302 final int nC = problem.getParameterSize();
303
304 final Incrementor iterationCounter = problem.getIterationCounter();
305 final Incrementor evaluationCounter = problem.getEvaluationCounter();
306
307 final ConvergenceChecker<Evaluation> checker = problem.getConvergenceChecker();
308
309
310 final int solvedCols = FastMath.min(nR, nC);
311
312 double[] lmDir = new double[nC];
313
314 double lmPar = 0;
315
316
317 double delta = 0;
318 double xNorm = 0;
319 double[] diag = new double[nC];
320 double[] oldX = new double[nC];
321 double[] oldRes = new double[nR];
322 double[] qtf = new double[nR];
323 double[] work1 = new double[nC];
324 double[] work2 = new double[nC];
325 double[] work3 = new double[nC];
326
327
328
329 evaluationCounter.increment();
330
331 Evaluation current = problem.evaluate(problem.getStart());
332 double[] currentResiduals = current.getResiduals().toArray();
333 double currentCost = current.getCost();
334 double[] currentPoint = current.getPoint().toArray();
335
336
337 boolean firstIteration = true;
338 while (true) {
339 iterationCounter.increment();
340
341 final Evaluation previous = current;
342
343
344 final InternalData internalData = qrDecomposition(current.getJacobian(), solvedCols);
345 final double[][] weightedJacobian = internalData.weightedJacobian;
346 final int[] permutation = internalData.permutation;
347 final double[] diagR = internalData.diagR;
348 final double[] jacNorm = internalData.jacNorm;
349
350
351 double[] weightedResidual = currentResiduals;
352 System.arraycopy(weightedResidual, 0, qtf, 0, nR);
353
354
355 qTy(qtf, internalData);
356
357
358
359 for (int k = 0; k < solvedCols; ++k) {
360 int pk = permutation[k];
361 weightedJacobian[k][pk] = diagR[pk];
362 }
363
364 if (firstIteration) {
365
366
367 xNorm = 0;
368 for (int k = 0; k < nC; ++k) {
369 double dk = jacNorm[k];
370 if (dk == 0) {
371 dk = 1.0;
372 }
373 double xk = dk * currentPoint[k];
374 xNorm += xk * xk;
375 diag[k] = dk;
376 }
377 xNorm = FastMath.sqrt(xNorm);
378
379
380 delta = (xNorm == 0) ? initialStepBoundFactor : (initialStepBoundFactor * xNorm);
381 }
382
383
384 double maxCosine = 0;
385 if (currentCost != 0) {
386 for (int j = 0; j < solvedCols; ++j) {
387 int pj = permutation[j];
388 double s = jacNorm[pj];
389 if (s != 0) {
390 double sum = 0;
391 for (int i = 0; i <= j; ++i) {
392 sum += weightedJacobian[i][pj] * qtf[i];
393 }
394 maxCosine = FastMath.max(maxCosine, FastMath.abs(sum) / (s * currentCost));
395 }
396 }
397 }
398 if (maxCosine <= orthoTolerance) {
399
400 return Optimum.of(
401 current,
402 evaluationCounter.getCount(),
403 iterationCounter.getCount());
404 }
405
406
407 for (int j = 0; j < nC; ++j) {
408 diag[j] = FastMath.max(diag[j], jacNorm[j]);
409 }
410
411
412 for (double ratio = 0; ratio < 1.0e-4;) {
413
414
415 for (int j = 0; j < solvedCols; ++j) {
416 int pj = permutation[j];
417 oldX[pj] = currentPoint[pj];
418 }
419 final double previousCost = currentCost;
420 double[] tmpVec = weightedResidual;
421 weightedResidual = oldRes;
422 oldRes = tmpVec;
423
424
425 lmPar = determineLMParameter(qtf, delta, diag,
426 internalData, solvedCols,
427 work1, work2, work3, lmDir, lmPar);
428
429
430 double lmNorm = 0;
431 for (int j = 0; j < solvedCols; ++j) {
432 int pj = permutation[j];
433 lmDir[pj] = -lmDir[pj];
434 currentPoint[pj] = oldX[pj] + lmDir[pj];
435 double s = diag[pj] * lmDir[pj];
436 lmNorm += s * s;
437 }
438 lmNorm = FastMath.sqrt(lmNorm);
439
440 if (firstIteration) {
441 delta = FastMath.min(delta, lmNorm);
442 }
443
444
445 evaluationCounter.increment();
446 current = problem.evaluate(new ArrayRealVector(currentPoint));
447 currentResiduals = current.getResiduals().toArray();
448 currentCost = current.getCost();
449 currentPoint = current.getPoint().toArray();
450
451
452 double actRed = -1.0;
453 if (0.1 * currentCost < previousCost) {
454 double r = currentCost / previousCost;
455 actRed = 1.0 - r * r;
456 }
457
458
459
460 for (int j = 0; j < solvedCols; ++j) {
461 int pj = permutation[j];
462 double dirJ = lmDir[pj];
463 work1[j] = 0;
464 for (int i = 0; i <= j; ++i) {
465 work1[i] += weightedJacobian[i][pj] * dirJ;
466 }
467 }
468 double coeff1 = 0;
469 for (int j = 0; j < solvedCols; ++j) {
470 coeff1 += work1[j] * work1[j];
471 }
472 double pc2 = previousCost * previousCost;
473 coeff1 /= pc2;
474 double coeff2 = lmPar * lmNorm * lmNorm / pc2;
475 double preRed = coeff1 + 2 * coeff2;
476 double dirDer = -(coeff1 + coeff2);
477
478
479 ratio = (preRed == 0) ? 0 : (actRed / preRed);
480
481
482 if (ratio <= 0.25) {
483 double tmp =
484 (actRed < 0) ? (0.5 * dirDer / (dirDer + 0.5 * actRed)) : 0.5;
485 if ((0.1 * currentCost >= previousCost) || (tmp < 0.1)) {
486 tmp = 0.1;
487 }
488 delta = tmp * FastMath.min(delta, 10.0 * lmNorm);
489 lmPar /= tmp;
490 } else if ((lmPar == 0) || (ratio >= 0.75)) {
491 delta = 2 * lmNorm;
492 lmPar *= 0.5;
493 }
494
495
496 if (ratio >= 1.0e-4) {
497
498 firstIteration = false;
499 xNorm = 0;
500 for (int k = 0; k < nC; ++k) {
501 double xK = diag[k] * currentPoint[k];
502 xNorm += xK * xK;
503 }
504 xNorm = FastMath.sqrt(xNorm);
505
506
507 if (checker != null && checker.converged(iterationCounter.getCount(), previous, current)) {
508 return Optimum.of(current, evaluationCounter.getCount(), iterationCounter.getCount());
509 }
510 } else {
511
512 currentCost = previousCost;
513 for (int j = 0; j < solvedCols; ++j) {
514 int pj = permutation[j];
515 currentPoint[pj] = oldX[pj];
516 }
517 tmpVec = weightedResidual;
518 weightedResidual = oldRes;
519 oldRes = tmpVec;
520
521 current = previous;
522 }
523
524
525 if ((FastMath.abs(actRed) <= costRelativeTolerance &&
526 preRed <= costRelativeTolerance &&
527 ratio <= 2.0) ||
528 delta <= parRelativeTolerance * xNorm) {
529 return Optimum.of(current, evaluationCounter.getCount(), iterationCounter.getCount());
530 }
531
532
533 if (FastMath.abs(actRed) <= TWO_EPS &&
534 preRed <= TWO_EPS &&
535 ratio <= 2.0) {
536 throw new MathIllegalStateException(LocalizedOptimFormats.TOO_SMALL_COST_RELATIVE_TOLERANCE,
537 costRelativeTolerance);
538 } else if (delta <= TWO_EPS * xNorm) {
539 throw new MathIllegalStateException(LocalizedOptimFormats.TOO_SMALL_PARAMETERS_RELATIVE_TOLERANCE,
540 parRelativeTolerance);
541 } else if (maxCosine <= TWO_EPS) {
542 throw new MathIllegalStateException(LocalizedOptimFormats.TOO_SMALL_ORTHOGONALITY_TOLERANCE,
543 orthoTolerance);
544 }
545 }
546 }
547 }
548
549
550
551
552
553
554
555 private static class InternalData {
556
557 private final double[][] weightedJacobian;
558
559 private final int[] permutation;
560
561 private final int rank;
562
563 private final double[] diagR;
564
565 private final double[] jacNorm;
566
567 private final double[] beta;
568
569
570
571
572
573
574
575
576
577
578
579
580 InternalData(double[][] weightedJacobian,
581 int[] permutation,
582 int rank,
583 double[] diagR,
584 double[] jacNorm,
585 double[] beta) {
586 this.weightedJacobian = weightedJacobian;
587 this.permutation = permutation;
588 this.rank = rank;
589 this.diagR = diagR;
590 this.jacNorm = jacNorm;
591 this.beta = beta;
592 }
593 }
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623 private double determineLMParameter(double[] qy, double delta, double[] diag,
624 InternalData internalData, int solvedCols,
625 double[] work1, double[] work2, double[] work3,
626 double[] lmDir, double lmPar) {
627 final double[][] weightedJacobian = internalData.weightedJacobian;
628 final int[] permutation = internalData.permutation;
629 final int rank = internalData.rank;
630 final double[] diagR = internalData.diagR;
631
632 final int nC = weightedJacobian[0].length;
633
634
635
636 for (int j = 0; j < rank; ++j) {
637 lmDir[permutation[j]] = qy[j];
638 }
639 for (int j = rank; j < nC; ++j) {
640 lmDir[permutation[j]] = 0;
641 }
642 for (int k = rank - 1; k >= 0; --k) {
643 int pk = permutation[k];
644 double ypk = lmDir[pk] / diagR[pk];
645 for (int i = 0; i < k; ++i) {
646 lmDir[permutation[i]] -= ypk * weightedJacobian[i][pk];
647 }
648 lmDir[pk] = ypk;
649 }
650
651
652
653 double dxNorm = 0;
654 for (int j = 0; j < solvedCols; ++j) {
655 int pj = permutation[j];
656 double s = diag[pj] * lmDir[pj];
657 work1[pj] = s;
658 dxNorm += s * s;
659 }
660 dxNorm = FastMath.sqrt(dxNorm);
661 double fp = dxNorm - delta;
662 if (fp <= 0.1 * delta) {
663 lmPar = 0;
664 return lmPar;
665 }
666
667
668
669
670 double sum2;
671 double parl = 0;
672 if (rank == solvedCols) {
673 for (int j = 0; j < solvedCols; ++j) {
674 int pj = permutation[j];
675 work1[pj] *= diag[pj] / dxNorm;
676 }
677 sum2 = 0;
678 for (int j = 0; j < solvedCols; ++j) {
679 int pj = permutation[j];
680 double sum = 0;
681 for (int i = 0; i < j; ++i) {
682 sum += weightedJacobian[i][pj] * work1[permutation[i]];
683 }
684 double s = (work1[pj] - sum) / diagR[pj];
685 work1[pj] = s;
686 sum2 += s * s;
687 }
688 parl = fp / (delta * sum2);
689 }
690
691
692 sum2 = 0;
693 for (int j = 0; j < solvedCols; ++j) {
694 int pj = permutation[j];
695 double sum = 0;
696 for (int i = 0; i <= j; ++i) {
697 sum += weightedJacobian[i][pj] * qy[i];
698 }
699 sum /= diag[pj];
700 sum2 += sum * sum;
701 }
702 double gNorm = FastMath.sqrt(sum2);
703 double paru = gNorm / delta;
704 if (paru == 0) {
705 paru = Precision.SAFE_MIN / FastMath.min(delta, 0.1);
706 }
707
708
709
710 lmPar = FastMath.min(paru, FastMath.max(lmPar, parl));
711 if (lmPar == 0) {
712 lmPar = gNorm / dxNorm;
713 }
714
715 for (int countdown = 10; countdown >= 0; --countdown) {
716
717
718 if (lmPar == 0) {
719 lmPar = FastMath.max(Precision.SAFE_MIN, 0.001 * paru);
720 }
721 double sPar = FastMath.sqrt(lmPar);
722 for (int j = 0; j < solvedCols; ++j) {
723 int pj = permutation[j];
724 work1[pj] = sPar * diag[pj];
725 }
726 determineLMDirection(qy, work1, work2, internalData, solvedCols, work3, lmDir);
727
728 dxNorm = 0;
729 for (int j = 0; j < solvedCols; ++j) {
730 int pj = permutation[j];
731 double s = diag[pj] * lmDir[pj];
732 work3[pj] = s;
733 dxNorm += s * s;
734 }
735 dxNorm = FastMath.sqrt(dxNorm);
736 double previousFP = fp;
737 fp = dxNorm - delta;
738
739
740
741 if (FastMath.abs(fp) <= 0.1 * delta ||
742 (parl == 0 &&
743 fp <= previousFP &&
744 previousFP < 0)) {
745 return lmPar;
746 }
747
748
749 for (int j = 0; j < solvedCols; ++j) {
750 int pj = permutation[j];
751 work1[pj] = work3[pj] * diag[pj] / dxNorm;
752 }
753 for (int j = 0; j < solvedCols; ++j) {
754 int pj = permutation[j];
755 work1[pj] /= work2[j];
756 double tmp = work1[pj];
757 for (int i = j + 1; i < solvedCols; ++i) {
758 work1[permutation[i]] -= weightedJacobian[i][pj] * tmp;
759 }
760 }
761 sum2 = 0;
762 for (int j = 0; j < solvedCols; ++j) {
763 double s = work1[permutation[j]];
764 sum2 += s * s;
765 }
766 double correction = fp / (delta * sum2);
767
768
769 if (fp > 0) {
770 parl = FastMath.max(parl, lmPar);
771 } else if (fp < 0) {
772 paru = FastMath.min(paru, lmPar);
773 }
774
775
776 lmPar = FastMath.max(parl, lmPar + correction);
777 }
778
779 return lmPar;
780 }
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805 private void determineLMDirection(double[] qy, double[] diag,
806 double[] lmDiag,
807 InternalData internalData,
808 int solvedCols,
809 double[] work,
810 double[] lmDir) {
811 final int[] permutation = internalData.permutation;
812 final double[][] weightedJacobian = internalData.weightedJacobian;
813 final double[] diagR = internalData.diagR;
814
815
816
817 for (int j = 0; j < solvedCols; ++j) {
818 int pj = permutation[j];
819 for (int i = j + 1; i < solvedCols; ++i) {
820 weightedJacobian[i][pj] = weightedJacobian[j][permutation[i]];
821 }
822 lmDir[j] = diagR[pj];
823 work[j] = qy[j];
824 }
825
826
827 for (int j = 0; j < solvedCols; ++j) {
828
829
830
831 int pj = permutation[j];
832 double dpj = diag[pj];
833 if (dpj != 0) {
834 Arrays.fill(lmDiag, j + 1, lmDiag.length, 0);
835 }
836 lmDiag[j] = dpj;
837
838
839
840
841 double qtbpj = 0;
842 for (int k = j; k < solvedCols; ++k) {
843 int pk = permutation[k];
844
845
846
847 if (lmDiag[k] != 0) {
848
849 final double sin;
850 final double cos;
851 double rkk = weightedJacobian[k][pk];
852 if (FastMath.abs(rkk) < FastMath.abs(lmDiag[k])) {
853 final double cotan = rkk / lmDiag[k];
854 sin = 1.0 / FastMath.sqrt(1.0 + cotan * cotan);
855 cos = sin * cotan;
856 } else {
857 final double tan = lmDiag[k] / rkk;
858 cos = 1.0 / FastMath.sqrt(1.0 + tan * tan);
859 sin = cos * tan;
860 }
861
862
863
864 weightedJacobian[k][pk] = cos * rkk + sin * lmDiag[k];
865 final double temp = cos * work[k] + sin * qtbpj;
866 qtbpj = -sin * work[k] + cos * qtbpj;
867 work[k] = temp;
868
869
870 for (int i = k + 1; i < solvedCols; ++i) {
871 double rik = weightedJacobian[i][pk];
872 final double temp2 = cos * rik + sin * lmDiag[i];
873 lmDiag[i] = -sin * rik + cos * lmDiag[i];
874 weightedJacobian[i][pk] = temp2;
875 }
876 }
877 }
878
879
880
881 lmDiag[j] = weightedJacobian[j][permutation[j]];
882 weightedJacobian[j][permutation[j]] = lmDir[j];
883 }
884
885
886
887 int nSing = solvedCols;
888 for (int j = 0; j < solvedCols; ++j) {
889 if ((lmDiag[j] == 0) && (nSing == solvedCols)) {
890 nSing = j;
891 }
892 if (nSing < solvedCols) {
893 work[j] = 0;
894 }
895 }
896 if (nSing > 0) {
897 for (int j = nSing - 1; j >= 0; --j) {
898 int pj = permutation[j];
899 double sum = 0;
900 for (int i = j + 1; i < nSing; ++i) {
901 sum += weightedJacobian[i][pj] * work[i];
902 }
903 work[j] = (work[j] - sum) / lmDiag[j];
904 }
905 }
906
907
908 for (int j = 0; j < lmDir.length; ++j) {
909 lmDir[permutation[j]] = work[j];
910 }
911 }
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939 private InternalData qrDecomposition(RealMatrix jacobian, int solvedCols)
940 throws MathIllegalStateException {
941
942
943 final double[][] weightedJacobian = jacobian.scalarMultiply(-1).getData();
944
945 final int nR = weightedJacobian.length;
946 final int nC = weightedJacobian[0].length;
947
948 final int[] permutation = new int[nC];
949 final double[] diagR = new double[nC];
950 final double[] jacNorm = new double[nC];
951 final double[] beta = new double[nC];
952
953
954 for (int k = 0; k < nC; ++k) {
955 permutation[k] = k;
956 double norm2 = 0;
957 for (double[] doubles : weightedJacobian) {
958 double akk = doubles[k];
959 norm2 += akk * akk;
960 }
961 jacNorm[k] = FastMath.sqrt(norm2);
962 }
963
964
965 for (int k = 0; k < nC; ++k) {
966
967
968 int nextColumn = -1;
969 double ak2 = Double.NEGATIVE_INFINITY;
970 for (int i = k; i < nC; ++i) {
971 double norm2 = 0;
972 for (int j = k; j < nR; ++j) {
973 double aki = weightedJacobian[j][permutation[i]];
974 norm2 += aki * aki;
975 }
976 if (Double.isInfinite(norm2) || Double.isNaN(norm2)) {
977 throw new MathIllegalStateException(LocalizedOptimFormats.UNABLE_TO_PERFORM_QR_DECOMPOSITION_ON_JACOBIAN,
978 nR, nC);
979 }
980 if (norm2 > ak2) {
981 nextColumn = i;
982 ak2 = norm2;
983 }
984 }
985 if (ak2 <= qrRankingThreshold) {
986 return new InternalData(weightedJacobian, permutation, k, diagR, jacNorm, beta);
987 }
988 int pk = permutation[nextColumn];
989 permutation[nextColumn] = permutation[k];
990 permutation[k] = pk;
991
992
993 double akk = weightedJacobian[k][pk];
994 double alpha = (akk > 0) ? -FastMath.sqrt(ak2) : FastMath.sqrt(ak2);
995 double betak = 1.0 / (ak2 - akk * alpha);
996 beta[pk] = betak;
997
998
999 diagR[pk] = alpha;
1000 weightedJacobian[k][pk] -= alpha;
1001
1002
1003 for (int dk = nC - 1 - k; dk > 0; --dk) {
1004 double gamma = 0;
1005 for (int j = k; j < nR; ++j) {
1006 gamma += weightedJacobian[j][pk] * weightedJacobian[j][permutation[k + dk]];
1007 }
1008 gamma *= betak;
1009 for (int j = k; j < nR; ++j) {
1010 weightedJacobian[j][permutation[k + dk]] -= gamma * weightedJacobian[j][pk];
1011 }
1012 }
1013 }
1014
1015 return new InternalData(weightedJacobian, permutation, solvedCols, diagR, jacNorm, beta);
1016 }
1017
1018
1019
1020
1021
1022
1023
1024 private void qTy(double[] y,
1025 InternalData internalData) {
1026 final double[][] weightedJacobian = internalData.weightedJacobian;
1027 final int[] permutation = internalData.permutation;
1028 final double[] beta = internalData.beta;
1029
1030 final int nR = weightedJacobian.length;
1031 final int nC = weightedJacobian[0].length;
1032
1033 for (int k = 0; k < nC; ++k) {
1034 int pk = permutation[k];
1035 double gamma = 0;
1036 for (int i = k; i < nR; ++i) {
1037 gamma += weightedJacobian[i][pk] * y[i];
1038 }
1039 gamma *= beta[pk];
1040 for (int i = k; i < nR; ++i) {
1041 y[i] -= gamma * weightedJacobian[i][pk];
1042 }
1043 }
1044 }
1045 }