26 #include <Matcher2D/ImageAlignment.hh>
27 #include <MathAlgo/SVD.hh>
28 #include <Base/Image/ImageIO.hh>
29 #include <Base/Common/FileHandling.hh>
30 #include <Base/Common/BIASpragma.hh>
37 #define UNDEF_VALUE 0.0
43 MaxIterations_(20), MaxUpdate_(0.01), Dampening_(0.0),
44 AffineBrightnessInvariance_(false),
45 LineSearch_(0.3), intensitySigma_(1.0),
46 T_(NULL), mean1_(0.0),
47 mean2_(0.0), dev1_(1.0), dev2_(1.0),iterationDebug_(0) {
60 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
61 "compute template and gradients from warped I1 with prediction "
63 T_->SetParameters(params);
66 if (T_->ParameterInversionJacobian(PIJ)!=0)
return -3;
70 const bool TextureJacobianIsConstant = T_->TextureJacobianIsConstant();
71 const bool ParameterJacobianIsConstant = T_->ParameterJacobianIsConstant();
74 for (
unsigned int i=0; i<active_.size(); i++) active_[i] =
true;
78 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
79 "auto pyramid, determine required scale for each pixel");
86 if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
87 int numberOfPixelsPerRow =
88 int(ceil(sqrt(
double(TemplatePositions_.size()))));
91 numberOfPixelsPerRow,3);
94 numberOfPixelsPerRow,1);
100 for (
unsigned int p=0; p<TemplatePositions_.size(); p++) {
104 T_->MapForward(x, ImagePositions_[p]);
106 if (p==0 || !ParameterJacobianIsConstant)
109 if (T_->ParameterJacobianBackward(JacParams, ImagePositions_[p])!=0)
113 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
114 "propagate parameter uncertainty "<<Cov);
117 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
" to pixel cov "<<PixelCov);
119 sqrt(PixelCov.Trace());
120 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
" to position uncertainty "<<scale);
122 double pyrLevel = 0.0;
123 if (scale<=0.5) scale = 0.5;
124 else if (scale>I1.
size()-1.0) {
125 scale = I1.
size()-1.0;
126 pyrLevel = log2(scale) + 1.0;
128 pyrLevel = log2(scale) + 1.0;
130 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Using pyramid level "<<pyrLevel);
131 TemplateScales_[p] = scale;
137 TemplateGreyValues_[p]);
141 if (I1[0]->IsInROI(x[0], x[1]))
144 BIASERR(x<<
" is not in ROI of source!!!");
145 TemplateGreyValues_[p] = UNDEF_VALUE;
151 double gv1,gv2, gv3,gv4;
167 if (I1[0]->IsInROI(x[0], x[1]))
170 BIASERR(x<<
" is not in ROI of source!!!");
171 TemplateGreyValues_[p] = UNDEF_VALUE;
174 double gv1,gv2, gv3,gv4;
181 if (p==0 || !TextureJacobianIsConstant)
182 if (T_->TextureJacobianForward(x, JacTex)!=0)
return -5;
185 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Could not obtain grey value for "
186 <<x<<
" at pyramid level "
191 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Using "<<x<<
" at pyramid level "
192 <<pyrLevel<<
" with grey value "<<TemplateGreyValues_[p]);
195 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Rotate and scale gradient "
196 <<grad[0]<<
";"<<grad[1]);
199 TemplateGradients_[p] = JacTex * (grad * 1.0 / scale);
200 active_[p] = TemplateGradients_[p].
NormL2()*scale>intensitySigma_;
201 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"GRAD="<<TemplateGradients_[p]
202 <<(active_[p]?
" active":
" skipped"));
206 if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
208 *pwdata ++ = PixelCov[0][0];
209 *pwdata ++ = PixelCov[0][1];
210 *pwdata ++ = PixelCov[1][1];
212 *pwdata ++ = pyrLevel;
216 INFNANCHECK(TemplateGreyValues_[p]);
220 if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
232 const double scale = pow(2.0,PyramidLevel);
233 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Using pyramid level "
234 <<PyramidLevel<<
" for grey values and gradients (scale="<<scale
237 for (
unsigned int p=0; p<TemplatePositions_.size(); p++) {
239 T_->MapForward(x, ImagePositions_[p]);
242 TemplateGreyValues_[p]);
246 if (I1[0]->IsInROI(x[0], x[1]))
249 BIASERR(x<<
" is not in ROI of source!!!");
250 TemplateGreyValues_[p] = UNDEF_VALUE;
254 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Pixel grey value at "<<x<<
" is "
255 <<TemplateGreyValues_[p]);
256 INFNANCHECK(TemplateGreyValues_[p]);
261 double gv1,gv2, gv3,gv4;
263 double(PyramidLevel), gv1);
265 double(PyramidLevel), gv2);
267 double(PyramidLevel), gv3);
269 double(PyramidLevel), gv4);
274 if (p==0 || !TextureJacobianIsConstant)
275 if (T_->TextureJacobianForward(x, JacTex)!=0)
return -6;
278 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Could not obtain grey value for "
279 <<x<<
" at pyramid level "<<PyramidLevel);
286 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
287 "rotate gradient "<<grad[0]<<
";"<<grad[1]
288 <<
" with local texture warp ["<<JacTex[0][0]<<
" "<<JacTex[0][1]
289 <<
"; "<<JacTex[1][0]<<
" "<<JacTex[1][1]<<
"]");
290 TemplateGradients_[p] = JacTex * (grad * 1.0 / scale);
291 double gradnorm = TemplateGradients_[p].
NormL2();
292 active_[p] = (gradnorm*scale>intensitySigma_);
293 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"GRAD="<<TemplateGradients_[p]
294 <<(active_[p]?
" active":
" skipped"));
296 if ((BIAS_ISINF(TemplateGreyValues_[p])||BIAS_ISNAN(TemplateGreyValues_[p]))
298 BIASERR(
"bad value for active pixel..");
305 if (AffineBrightnessInvariance_) {
313 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
314 mean += TemplateGreyValues_[i];
316 mean1_ = mean / TemplateGreyValues_.size();
319 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
320 scatter += (TemplateGreyValues_[i]-mean1_) *
321 (TemplateGreyValues_[i]-mean1_);
323 scatter = scatter/(TemplateGreyValues_.size()-1);
324 if (scatter>0.01*intensitySigma_) dev1_ = sqrt(scatter);
325 else dev1_ = 0.1*intensitySigma_;
328 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
329 TemplateGreyValues_[i] = (TemplateGreyValues_[i]-mean1_)/dev1_;
330 TemplateGradients_[i] /= dev1_;
332 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
333 "Affine normalization of gradients and grey values yielded mean="
334 <<mean1_<<
" and dev="<<dev1_);
340 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"compute jacobians + hessian");
345 if (GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) {
346 int numberOfPixelsPerRow =
347 int(ceil(sqrt(
double(TemplateGreyValues_.size()))));
348 Image<float> thetemplate(numberOfPixelsPerRow,numberOfPixelsPerRow,1);
349 Image<float> thegradients(numberOfPixelsPerRow,numberOfPixelsPerRow,3);
353 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
354 *pF++ = TemplateGreyValues_[i];
357 pG++;*pG++ = TemplateGradients_[i].NormL2();pG++;
360 *pG++ = TemplateGradients_[i].NormL2();pG++;pG++;
381 T_->GetParameters(parameterUpdate);
382 for (
unsigned int i=0; i<parameterUpdate.
Size(); i++)
383 parameterUpdate[i] = 0;
390 unsigned int numMeasurements = 0;
391 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
393 if (i==0 || !ParameterJacobianIsConstant)
398 if (!active_[i])
continue;
400 Jac.
MultLeft(TemplateGradients_[i], SteepestDescentImages_[i]);
401 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Steepest descent image is "<<
402 SteepestDescentImages_[i]);
405 SteepestDescentImages_[i].OuterProduct(SteepestDescentImages_[i]);
410 if (Dampening_>0.0) {
415 bool CovInvgood =
true;
416 for (
int i=0; i<CovInv.num_rows(); i++) {
417 for (
int j=0; j<CovInv.num_cols(); j++) {
418 if (BIAS_ISNAN(CovInv[i][j]) || BIAS_ISINF(CovInv[i][j])) {
424 BIASERR(
"Bad CovInv "<<CovInv<<
" cov was "<<Cov<<
" lastPIJ="<<PIJ);
429 SVD covinvsvd(
false);
430 if (covinvsvd.
Compute(CovInv)==0) {
431 invCovInv = covinvsvd.
Invert();
432 Hessian += invCovInv * intensitySigma_ * intensitySigma_ *
433 Dampening_ / (dev1_*dev1_);
436 BIASERR(
"Bad covariance, cannot decompose: "<<CovInv);
440 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Finished Hessian, "
441 <<Hessian<<
", inverting ...");
447 bool hessiangood =
true;
448 for (
int i=0; i<Hessian.num_rows(); i++) {
449 for (
int j=0; j<Hessian.num_cols(); j++) {
450 if (BIAS_ISNAN(Hessian[i][j]) || BIAS_ISINF(Hessian[i][j])) {
456 BIASERR(
"Bad Hessian !!!");
457 cout<<
"Last Jacobian was "<< Jac<<endl;
458 for (
unsigned int i=0; i<SteepestDescentImages_.size(); i++) {
459 cout<<
"SDI "<<i<<
" ";
461 cout<<SteepestDescentImages_[i];
463 cout<<
"is disabled."<<endl;
472 if (Dampening_<0.0) {
473 for (
int i=0; i<Hessian.num_cols(); i++) {
474 Hessian[i][i] -= S[0] * Dampening_;
477 }
else if ((S[0]<1e-9) || (S[S.
Size()-1]/S[0]<1e-16)) {
479 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Not enough structure for "
480 <<
" uniquely solving for all params in H: "<<Hessian
481 <<
", singular values are "<<S);
487 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Starting iterations");
490 bool converged =
false;
492 vector<double> greyValuesI2(TemplateGreyValues_.size());
494 vector<Matrix2x2<double> > pyramidLevelsI2(TemplateGreyValues_.size());
496 vector<double> pyramidLevelsI2(TemplateGreyValues_.size());
501 double lastresidual = DBL_MAX;
509 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Iteration "<<iter);
512 steepestDescentImageSum.SetZero();
514 if (GetDebugLevel() & D_IMAGEALIGNMENT_PARAMETER) {
516 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
517 "Current parameter update onto prediction is "
522 if (PyramidLevel<0) {
523 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Cov is "<<Cov);
527 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
530 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Looking up Pixel at "<< curPos);
533 if ((i==0 || !ParameterJacobianIsConstant)) {
536 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
537 " PJBW="<<JacParams);
539 covI2 = JacParams*Cov*JacParams.
Transpose();
540 double scale = covI2.
Trace();
544 (JacParams*Cov*JacParams.
Transpose()).Trace());
546 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
547 " position uncertainty "<<scale);
550 if (scale<=0.5) scale = 0.5;
551 else if (scale>I1.
size()-1.0) {
552 scale = I1.
size()-1.0;
553 pyrLevel = log2(scale) + 1.0;
555 pyrLevel = log2(scale) + 1.0;
559 pyramidLevelsI2[i] = covI2;
561 pyramidLevelsI2[i] = pyrLevel;
565 if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) &&
566 (i+1==TemplateGreyValues_.size())) {
567 int numberOfPixelsPerRow =
568 int(ceil(sqrt(
double(pyramidLevelsI2.size()))));
571 theweights(numberOfPixelsPerRow,
572 numberOfPixelsPerRow,3);
575 theweights(numberOfPixelsPerRow,
576 numberOfPixelsPerRow,1);
580 for (
unsigned int j=0; j<pyramidLevelsI2.size(); j++) {
582 *pF++ = pyramidLevelsI2[j][0][0];
583 *pF++ = pyramidLevelsI2[j][0][1];
584 *pF++ = pyramidLevelsI2[j][1][1];
586 *pF++ = pyramidLevelsI2[j];
601 greyValuesI2[i])!=0) {
603 greyValuesI2[i] = TemplateGreyValues_[i];
609 greyValuesI2[i])!=0) {
611 greyValuesI2[i] = TemplateGreyValues_[i];
616 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Using "<<curPos
617 <<
" at pyramid level "<<pyramidLevelsI2[i]
618 <<
" with grey value(2) "<<greyValuesI2[i]);
619 INFNANCHECK(greyValuesI2[i]);
623 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
624 pUpdateWarp->
MapBackward(ImagePositions_[i], curPos);
625 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Looking up Pixel at "<< curPos);
626 if (I2.
IsInROI(curPos[0], curPos[1], PyramidLevel))
630 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Not in ROI, disabling! ");
631 greyValuesI2[i] = TemplateGreyValues_[i];
634 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Grey value is "<< greyValuesI2[i]);
635 INFNANCHECK(greyValuesI2[i]);
639 if (AffineBrightnessInvariance_) {
645 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
646 mean += greyValuesI2[i];
648 mean2_ = mean / TemplateGreyValues_.size();
650 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
651 scatter += (greyValuesI2[i] - mean2_)*(greyValuesI2[i] - mean2_);
653 scatter /= TemplateGreyValues_.size()-1;
654 if (scatter>0.01*intensitySigma_) dev2_ = sqrt(scatter);
655 else dev2_ = 0.1*intensitySigma_;
657 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
658 "Affine normalization of grey values(2) yielded mean="
659 <<mean2_<<
" and dev="<<dev2_);
662 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
663 greyValuesI2[i] = (greyValuesI2[i]-mean2_)/dev2_;
667 if (GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) {
668 int numberOfPixelsPerRow = int(ceil(sqrt(
double(greyValuesI2.size()))));
669 Image<float> theimage(numberOfPixelsPerRow,numberOfPixelsPerRow,1);
671 for (
unsigned int g=0; g<greyValuesI2.size(); g++) {
672 *pF++ = greyValuesI2[g];
692 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
704 errorImage[i] = greyValuesI2[i] - TemplateGreyValues_[i];
705 residual += errorImage[i]*errorImage[i];
706 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
"Grey value difference is "<<
709 steepestDescentImageSum += errorImage[i] * SteepestDescentImages_[i];
716 residual = sqrt(residual/numMeasurements);
717 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"RMS residual is "<<residual);
718 if (residual>lastresidual) {
719 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Error increase from "
720 <<lastresidual<<
". Stopping.");
721 if (LineSearch_>0.0) {
722 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Step did not improve, trying a "
723 <<LineSearch_<<
" smaller step.");
726 parameterUpdate *= LineSearch_;
728 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"No further improvement.Stopping.");
736 lastresidual = residual;
738 if (Dampening_<=0.0) {
739 Hessian.Mult(steepestDescentImageSum, parameterUpdate);
746 double weightfactor = (intensitySigma_*intensitySigma_) / (dev2_*dev2_);
747 parameterUpdate = Hessian * (steepestDescentImageSum -
751 for (
unsigned int i=0; i<parameterUpdate.
Size(); i++) {
752 if (BIAS_ISINF(parameterUpdate[i]) || BIAS_ISNAN(parameterUpdate[i])) {
753 BIASERR(
"bad result:"<<parameterUpdate<<
" invCovinv="<<invCovInv
755 <<
" hessian="<<Hessian<<
" sdis="<<steepestDescentImageSum);
759 cout<<
"Grey value data ("<<TemplateGreyValues_.size()<<
")"<<endl;
760 for (
unsigned int r=0; r<TemplateGreyValues_.size(); r++) {
762 if (!active_[r])
continue;
763 cout<< errorImage[r] <<
"="
764 <<greyValuesI2[r]<<
"-"<< TemplateGreyValues_[r];
766 cout<<
"SDI="<< SteepestDescentImages_[i]<<endl;
774 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
775 "inv.compositional parameter update is "
777 if (parameterUpdate.
NormL2()>1000) {
779 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Diverged! ");
784 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Update of "
785 <<parameterUpdate.
NormL2()<<
" thresh="<<MaxUpdate_);
786 if (parameterUpdate.
NormL2()<MaxUpdate_) {
788 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Convergence ! Update = "
789 <<parameterUpdate.
NormL2());
790 }
else if (++iter>MaxIterations_) {
791 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Too many iterations: "<<iter);
797 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
798 "Finished. Total parameter update onto prediction is "
801 T_->ComposeWithInverseDeltaP(parameterUpdate);
803 T_->GetParameters(params);
805 for (
unsigned int i=0; i<params.
Size(); i++) {
806 INFNANCHECK(params[i]);
809 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Final warp parameters are "<<params);
812 if (numMeasurements>params.
Size()) {
813 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
"residual is now "<<residual);
814 double additionalresidual = 0;
815 for (
unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
817 if (!active_[i])
continue;
820 if (TemplateScales_[i]>0.5)
821 additionalresidual +=
822 (TemplateGradients_[i]*(TemplateScales_[i]-0.5)).NormL2();
824 residual += additionalresidual/double(numMeasurements);
825 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
"residual changed to "<<residual);
829 double refvar = 0.0001 +
830 (residual*residual*dev1_*dev1_) *
double(numMeasurements) /
831 (double(numMeasurements-params.
size())*intensitySigma_*intensitySigma_);
840 bool hessiangood =
true;
841 for (
int i=0; i<upJac.
num_rows(); i++) {
842 for (
int j=0; j<upJac.
num_cols(); j++) {
843 if (BIAS_ISNAN(upJac[i][j]) || BIAS_ISINF(upJac[i][j])) {
851 BIASERR(
"Bad upjac !!!"<< upJac<<
" or PIJ "<<PIJ<<
" for params "
857 SVD covtransformSVD(upJac*PIJ);
858 PIJ = covtransformSVD.
Invert();
865 BIASWARN(
"Could not estimate covariance, setting large value");
868 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Covariance changed to "<<Cov);
875 int numberOfPixelsPerRow)
878 if (numberOfPixelsPerRow<1) {
880 numberOfPixelsPerRow = int(rint(sqrt(4.0*LAF.
GetA().
det())))+1;
881 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"NumOfPixelsPerRow automatically "
882 <<
" computed as "<<numberOfPixelsPerRow<<
" with A="<<LAF.
GetA());
885 double pixeldistance = 2.0/(numberOfPixelsPerRow-1);
886 if (numberOfPixelsPerRow<2) pixeldistance = 1.0;
888 int numberOfPixels = numberOfPixelsPerRow * numberOfPixelsPerRow;
889 TemplateGreyValues_.resize(numberOfPixels);
890 TemplateGradients_.resize(numberOfPixels);
891 TemplatePositions_.resize(numberOfPixels);
892 TemplateScales_.resize(numberOfPixels);
893 ImagePositions_.resize(numberOfPixels);
894 SteepestDescentImages_.resize(numberOfPixels);
895 active_.resize(numberOfPixels);
896 ControlPositions_.resize(4);
897 const double offset = -1.0;
898 for (
int y=0; y<numberOfPixelsPerRow; y++) {
899 for (
int x=0; x<numberOfPixelsPerRow; x++) {
900 int index = y*numberOfPixelsPerRow+x;
901 TemplatePositions_[index] =
903 y*pixeldistance+offset));
904 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
905 "Using Position "<<TemplatePositions_[y*numberOfPixelsPerRow+x]);
908 ControlPositions_[0] = TemplatePositions_[0];
909 ControlPositions_[1] = TemplatePositions_[numberOfPixelsPerRow-1];
910 ControlPositions_[2] =
911 TemplatePositions_[numberOfPixels-numberOfPixelsPerRow-1];
912 ControlPositions_[3] =
913 TemplatePositions_[numberOfPixels-1];
915 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Have "<<numberOfPixels
916 <<
" pixels with distance "<<pixeldistance<<
" in local affine frame");
928 double oldLineSearch = LineSearch_;
930 double trace = Cov.
Trace();
931 double lasttrace = trace + 1e100;
933 double desiredAccuracy = GetMaxUpdate();
934 const unsigned int numparams = params.
size();
935 BIASASSERT(Cov.
num_rows()==int(numparams));
936 BIASASSERT(Cov.
num_cols()==int(numparams));
940 while (trace<lasttrace) {
941 lasttrace = trace*0.95;
944 for (
unsigned int i=0; i<numparams; i++) {
945 for (
unsigned int j=i; j<numparams; j++) {
946 if (BIAS_ISINF(Cov[j][i]) || BIAS_ISNAN(Cov[j][i])) {
947 BIASERR(
"corrupted cov, giving up "<<Cov);
950 Cov[j][i] = Cov[i][j] =
951 (Cov[i][j] + Cov[j][i] + 0.5*lastCov[j][i])*0.25;
954 if (Cov[i][i]>lastCov[i][i]) {
955 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
"Cov increased:"<<Cov
956 <<
" from "<<lastCov);
957 if (lastCov[i][i]<1e-10) lastCov[i][i] = 1e-10;
958 double factor = sqrt(lastCov[i][i]/Cov[i][i]);
959 for (
unsigned int j=0; j<numparams; j++) {
960 Cov[i][j] = Cov[j][i] = Cov[j][i]*factor;
963 BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
"Cov clipped to "<<Cov);
967 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Aligning with Cov="<<Cov);
970 double newthresh = 0.03*sqrt(trace);
971 if (newthresh<desiredAccuracy) newthresh = desiredAccuracy;
972 SetUpdateThreshold(newthresh);
981 returnvalue = Align(I1, I2, params, Cov, -1);
990 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Align returned "<<returnvalue
991 <<
". Stopping iteration.");
994 if (iterationDebug_++ > MaxIterations_) {
995 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Too many iterations: "
1000 trace = Cov.
Trace();
1013 if (CheckConvergence_(params, Cov, 0.1)==0) {
1014 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
1015 "Covariance reached desired accuracy: "<<Cov);
1021 SetUpdateThreshold(desiredAccuracy);
1022 iterationDebug_ = 0;
1023 LineSearch_ = oldLineSearch;
1027 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Covariance trace="<<trace
1028 <<
" while last time it was "<<lasttrace
1029 <<
", now using full resolution");
1031 SetUpdateThreshold(desiredAccuracy);
1032 LineSearch_ = oldLineSearch;
1033 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Aligning with full resolution.");
1034 returnvalue = Align(I1, I2, params, Cov, 0);
1035 iterationDebug_ = 0;
1047 BIASASSERT(I1.
size()>0);
1050 int returnvalue = 0;
1051 double oldLineSearch = LineSearch_;
1053 double lasttrace = 1e200;
1054 double trace = 1e100;
1058 double desiredAccuracy = GetMaxUpdate();
1060 const unsigned int numparams = params.
size();
1062 BIASASSERT(Cov.
num_rows()==int(numparams));
1063 BIASASSERT(Cov.
num_cols()==int(numparams));
1064 iterationDebug_ = 0;
1067 for (
int pyrLevel =
int(I1.
size()-1); pyrLevel>0; pyrLevel--) {
1069 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Aligning on level "<<pyrLevel);
1071 for (
unsigned int i=0; i<numparams; i++) {
1072 INFNANCHECK(params[i]);
1078 returnvalue = Align(I1, I2, params, Cov, pyrLevel);
1079 if (returnvalue<0) {
1080 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Align returned "<<returnvalue
1081 <<
". Trying smaller pyramid level.");
1086 if (iterationDebug_++ > MaxIterations_) {
1087 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Too many iterations: "
1093 trace = Cov.
Trace();
1096 if (CheckConvergence_(params, Cov, 0.1)==0) {
1097 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
1098 "Covariance reached desired accuracy");
1104 SetUpdateThreshold(desiredAccuracy);
1105 iterationDebug_ = 0;
1106 LineSearch_ = oldLineSearch;
1109 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Covariance did not improve, trace="<<trace
1110 <<
" while last time it was "<<lasttrace);
1112 SetUpdateThreshold(desiredAccuracy);
1113 LineSearch_ = oldLineSearch;
1114 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
"Aligning with full resolution.");
1115 returnvalue = Align(I1, I2, params, Cov, 0);
1116 iterationDebug_ = 0;
1124 const double& pixelAccuracy)
1127 BIASASSERT(T_!=NULL);
1129 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
1130 "compute template and gradients from warped I1 with prediction "
1132 T_->SetParameters(params);
1135 const bool ParameterJacobianIsConstant = T_->ParameterJacobianIsConstant();
1137 BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
1138 "auto pyramid, determine required scale for each pixel");
1140 for (
unsigned int p=0; p<ControlPositions_.size(); p++) {
1144 if (p==0 || !ParameterJacobianIsConstant)
1145 T_->ParameterJacobianForward(JacParams, x);
1147 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
1148 "propagate parameter uncertainty "<<Cov);
1152 BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
" to position uncertainty "<<scale);
1154 if (scale>pixelAccuracy)
return 1;
virtual Vector< double > GetInverseParameters() const
returns parameter vector which undoes the current warp
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
T det() const
calculate the determinante
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Subscript num_cols() const
unsigned size() const
deprecated interface
double GetImageValue(const double &x, const double &y, unsigned int scale, int channel=0) const
bilinear value from scale space: (x,y) is position in pyramid[0], 0<=scale<=size()-1 is pyramid level...
Matrix< T > Transpose() const
transpose function, storing data destination matrix
double NormL2() const
Return the L2 norm: sqrt(a^1 + a^2 + ...)
affine transformation of 2D image plane which relates image coordinate system and local affine featur...
int GetTrilinearImageValue(const double &x, const double &y, const double &scale, double &T, int channel=0) const
trilinear value from scale space: (x,y) is position in pyramid[0], 0<=scale<=size()-1 is pyramid leve...
virtual int ParameterJacobianBackward(Matrix< double > &Jac, const HomgPoint2D &sink)
transformed position change when parameters change
int StrictPyramidAlign(const PyramidImage< float > &I1, const PyramidImage< float > &I2, Vector< double > ¶ms, Matrix< double > &Cov)
aligns image1 with image2 with respect to parameters params, traditional pyramid based alignment is u...
virtual int MapBackward(const HomgPoint2D &sink, HomgPoint2D &source) const =0
map a point in image "source" to a point in image "sink"
int AutoAlign(const PyramidImage< float > &I1, const PyramidImage< float > &I2, Vector< double > ¶ms, Matrix< double > &Cov)
aligns image1 with image2 with respect to parameters params, scale space tracking is used recursively...
class for representing parameterized image warps, such as homography, displacement, ...
unsigned int Size() const
length of the vector
int Compute(const Matrix< double > &M, double ZeroThreshold=DEFAULT_DOUBLE_ZERO_THRESHOLD)
set a new matrix and compute its decomposition.
int CheckConvergence_(const Vector< double > ¶ms, const Matrix< double > &Cov, const double &pixelAccuracy=0.3)
check for given cov and params, whether control points move more than pixelAccuracy ...
double NormL2() const
Return the L2 norm: a^2 + b^2 + c^2 + ...
BIAS::HomgPoint2D LocalToGlobal(const BIAS::HomgPoint2D &x) const
tranform a point in the feature coordinate system into the iamge coordinate system ...
void SetAlignmentPixelsRectangular(const LocalAffineFrame &LAF, int numberOfPixels=-1)
choose any rectangular tracking region and resolution
int Align(const PyramidImage< float > &I1, const PyramidImage< float > &I2, Vector< double > ¶ms, Matrix< double > &Cov, int PyramidLevel=-1)
aligns image1 with image2 with respect to parameters params
const BIAS::Matrix2x2< double > & GetA() const
return affine matrix
virtual int ParameterInversionJacobian(Matrix< double > &Jac) const
compute parameters for inverse operation and obtain the jacobian of the inverse parameters with respe...
void GetParameters(Vector< double > &p) const
get the current parameter vector
virtual TextureTransform * Clone() const =0
virtual covariant copy constructor, caller must eventually destroy the created object ...
const Vector< double > & GetS() const
return S which is a vector of the singular values of A in descending order.
static int Save(const std::string &filename, const ImageBase &img, const enum TFileFormat FileFormat=FF_auto, const bool sync=BIAS_DEFAULT_SYNC, const int c_jpeg_quality=BIAS_DEFAULT_IMAGE_QUALITY, const bool forceNewID=BIAS_DEFAULT_FORCENEWID, const bool &writeMetaData=true)
Export image as file using extrnal libs.
void SetIdentity()
Converts matrix to identity matrix.
static std::string toString(const T thenumber, int numzeros=DEFAULT_LEADING_ZEROS)
Converts number to string with leading zeros.
const StorageType * GetImageData() const
overloaded GetImageData() from ImageBase
bool IsInROI(double x, double y, int layer) const
int GetAnisotropicImageValue(const double &xsource, const double &ysource, const Matrix2x2< double > &Cov, double &T, unsigned int channel=0) const
computes (Gaussian) expectation value across a region, used e.g.
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
void MultLeft(const Matrix< T > &arg)
in Place matrix multiplication this is equal to M = arg*M, but faster
virtual void SetParameters(const Vector< double > &p)=0
override the current state with the new parameters, the meaning of the parameters is defined in the d...
Subscript num_rows() const
virtual void ComposeWithInverseDeltaP(const Vector< double > &deltaP)=0
concatenate *this and an inverse transform with param deltaP and save new parameter vector to *this...
void SetZero()
zeroes the image
class BIASGeometryBase_EXPORT HomgPoint2D