25 #include "TrackerBaseAffine.hh"
26 #include "MathAlgo/SVD.hh"
27 #include "Base/Image/ImageIO.hh"
28 #include "Base/Common/FileHandling.hh"
29 #include "Geometry/HMatrix.hh"
34 const double DEFAULT_MAX_AFFINECHANGE = 0.3;
99 template <
class StorageType>
103 _MaxAffineChange = DEFAULT_MAX_AFFINECHANGE;
104 _SimilarityTransformOnly = only4params;
105 UseHomography_ = useHomography;
108 template <
class StorageType>
119 BIASERR(
"affine guess seems unitialized, set at least identity");
126 result.
GetData(), AffineResult,
127 error, iter, Cov_, residuumMAD_, residuumMSD_,
128 _SimilarityTransformOnly,
true);
137 template <
class StorageType>
143 KLT_TYPE& mad, KLT_TYPE& msd,
148 if (_HalfWinSize != _LastHalfWinSize){
149 this->Resize_(_HalfWinSize);
152 static int featurecounter = 0;
158 BIASDOUT(D_TRACKERB_AFFINE,
"Called Affine for "
159 <<p1[0]<<
";"<<p1[1]<<
", guess is "<<p2[0]<<
";"<<p2[1]
160 <<
" affine guess is "<< p2A);
162 const int width = _WinSize;
163 const int height = _WinSize;
164 const int hw = width/2;
165 const int hh = height/2;
167 const KLT_TYPE fhw = hw;
168 const KLT_TYPE fhh = hh;
170 if (_minx1 >= p1[0] || _miny1 >= p1[1] || _maxx1 < p1[0] || _maxy1 < p1[1]){
171 BIASDOUT(D_TRACKERB_KLT,
"original point is too close to border: "
172 <<p1[0]<<
";"<<p1[1]);
177 const int max_iterations = _MaxIterations;
178 const KLT_TYPE maxerr = _MaxError;
179 const KLT_TYPE th_aff = _MaxAffineChange;
186 UseHomography_?8:SimilarityTransformOnly?4:6);
189 Cov.
newsize(SimilarityTransformOnly?4:6,SimilarityTransformOnly?4:6);
193 KLT_TYPE *Axx = &(resultA[0][0]);
194 KLT_TYPE *Axy = &(resultA[0][1]);
195 KLT_TYPE *Ayx = &(resultA[1][0]);
196 KLT_TYPE *Ayy = &(resultA[1][1]);
201 KLT_TYPE h31=0,h32=0;
203 bool convergence =
false;
207 KLT_TYPE *imgdiff =
new KLT_TYPE[width * height];
208 KLT_TYPE *gradx =
new KLT_TYPE[width * height];
209 KLT_TYPE *grady =
new KLT_TYPE[width * height];
212 BilinearRegion1_(p1[0], p1[1], hh);
215 KLT_TYPE ul_x = (result[0] + *Axy * fhh - *Axx * fhw)/(-h31*fhw+h32*fhh+1);
216 KLT_TYPE ul_y = (result[1] + *Ayy * fhh - *Ayx * fhw)/(-h31*fhw+h32*fhh+1);
217 KLT_TYPE ll_x = (result[0] - *Axx * fhw - *Axy * fhh)/(-h31*fhw-h32*fhh+1);
218 KLT_TYPE ll_y = (result[1] - *Ayx * fhw - *Ayy * fhh)/(-h31*fhw-h32*fhh+1);
219 KLT_TYPE ur_x = (result[0] + *Axx * fhw + *Axy * fhh)/(h31*fhw+h32*fhh+1);
220 KLT_TYPE ur_y = (result[1] + *Ayx * fhw + *Ayy * fhh)/(h31*fhw+h32*fhh+1);
221 KLT_TYPE lr_x = (result[0] + *Axx * fhw - *Axy * fhh)/(h31*fhw-h32*fhh+1);
222 KLT_TYPE lr_y = (result[1] + *Ayx * fhw - *Ayy * fhh)/(h31*fhw-h32*fhh+1);
224 if ( ul_x < _minx2 || ul_x > _maxx2 ||
225 ll_x < _minx2 || ll_x > _maxx2 ||
226 ur_x < _minx2 || ur_x > _maxx2 ||
227 lr_x < _minx2 || lr_x > _maxx2 ||
228 ul_y < _miny2 || ul_y > _maxy2 ||
229 ll_y < _miny2 || ll_y > _maxy2 ||
230 ur_y < _miny2 || ur_y > _maxy2 ||
231 lr_y < _miny2 || lr_y > _maxy2) {
233 BIASDOUT( D_TRACKERB_KLT,
"out of image");
238 ComputeIntensityDifferenceAffine_(result[0], result[1],
239 *Axx, *Ayx , *Axy, *Ayy, h31, h32,
243 GetGradientWinAffine_(result[0], result[1],
244 *Axx, *Ayx , *Axy, *Ayy, h31, h32,
245 hw, hh, gradx, grady);
252 if (this->GetDebugLevel() & D_TRACKERB_AFFINE) {
254 ss<<
"Aff_"<<setw(3)<<setfill(
'0')<<int(featurecounter)<<
"-"
255 <<setw(4)<<setfill(
'0')<<int(iteration)<<
"-";
256 string imname = ss.str();
260 for (
int y=0; y<2*hh+1; y++) {
261 for (
int x=0; x<2*hw+1; x++) {
262 im1.GetImageDataArray()[y][x] = float(_bl1[y][x]);
274 if (UseHomography_) {
276 *Axx, *Axy, *Ayx, *Ayy,
282 *Axx, *Axy, *Ayx, *Ayy,
291 status = testsvd.
Solve(T,a,par);
301 BIASERR(
"structure degeneracy for homography estimation");
309 result[0] += dx = a[4];
310 result[1] += dy = a[5];
341 if (SimilarityTransformOnly){
352 result[0] += dx = a[2];
353 result[1] += dy = a[3];
365 result[0] += dx = a[4];
366 result[1] += dy = a[5];
400 ul_x -= (result[0] + *Axy * fhh - *Axx * fhw)/(-h31*fhw+h32*fhh+1);
401 ul_y -= (result[1] + *Ayy * fhh - *Ayx * fhw)/(-h31*fhw+h32*fhh+1);
402 ll_x -= (result[0] - *Axx * fhw - *Axy * fhh)/(-h31*fhw-h32*fhh+1);
403 ll_y -= (result[1] - *Ayx * fhw - *Ayy * fhh)/(-h31*fhw-h32*fhh+1);
404 ur_x -= (result[0] + *Axx * fhw + *Axy * fhh)/(h31*fhw+h32*fhh+1);
405 ur_y -= (result[1] + *Ayx * fhw + *Ayy * fhh)/(h31*fhw+h32*fhh+1);
406 lr_x -= (result[0] + *Axx * fhw - *Axy * fhh)/(h31*fhw-h32*fhh+1);
407 lr_y -= (result[1] + *Ayx * fhw - *Ayy * fhh)/(h31*fhw-h32*fhh+1);
410 convergence = (fabs(dx) < maxerr && fabs(dy) < maxerr &&
411 fabs(ul_x) < th_aff && fabs(ul_y) < th_aff &&
412 fabs(ll_x) < th_aff && fabs(ll_y) < th_aff &&
413 fabs(ur_x) < th_aff && fabs(ur_y) < th_aff &&
414 fabs(lr_x) < th_aff && fabs(lr_y) < th_aff);
417 BIASDOUT( D_TRACKERB_KLT,
"no structure for tracking");
422 }
while ( !convergence && iteration < max_iterations);
423 if (iteration >= max_iterations) {
425 BIASDOUT( D_TRACKERB_KLT,
"too many iter: "<<iteration
426 <<
" maxiter="<<max_iterations);
438 if (UseHomography_) {
458 const unsigned int wh = width*height;
459 register KLT_TYPE* fw = imgdiff;
460 for (
unsigned int h=0; h<wh ; h++) {
461 const KLT_TYPE cur = (KLT_TYPE)fabsf((
float) (*fw) );
467 mad /= double(wh)*dev1_;
469 double sigma = msd / (double)(wh - SimilarityTransformOnly?4:6);
472 if (ComputeCovariance) {
474 T.
newsize(SimilarityTransformOnly?4:6,SimilarityTransformOnly?4:6);
475 if (SimilarityTransformOnly)
480 Cov = covsvd.
Invert() * sigma;
484 for (
int i=0; i<Cov.
num_cols()-2; i++) {
485 Cov[i][i] = 0.01 * (dx * dx + dy * dy);
487 if (SimilarityTransformOnly) {
497 error = (fabs(dx)>fabs(dy))?fabs(dx):fabs(dy);
500 if (result[0]<_minx2 || result[0] > _maxx2 ||
501 result[1]<_miny2 || result[1] > _maxy2)
510 BIASDOUT( D_TRACKERB_KLT,
"status at end is "<<status);
516 template <
class StorageType>
520 if (_SimilarityTransformOnly) {
552 KLT_TYPE big,dum,pivinv,tmp;
556 for (j=0;j<n;j++) ipiv[j]=0;
563 if (fabs(a[j][k]) >= big) {
574 a[row][l] = a[col][l];
584 pivinv=1.0/a[col][col];
586 for (l=0;l<n;l++) a[col][l] *= pivinv;
592 for (l=0;l<n;l++) a[ll][l] -= a[col][l]*dum;
596 for (l=n-1;l>=0;l--) {
597 if (indxr[l] != indxc[l])
599 tmp = a[k][indxr[l]];
600 a[k][indxr[l]] = a[k][indxc[l]];
601 a[k][indxc[l]] = tmp;
608 template <
class StorageType>
611 const KLT_TYPE& Axx,
const KLT_TYPE& Ayx,
612 const KLT_TYPE& Axy,
const KLT_TYPE& Ayy,
613 const KLT_TYPE& h31,
const KLT_TYPE& h32,
614 const int hw,
const int hh,
616 KLT_TYPE* out_grady) {
617 if (_ComputeFilteredImages) {
621 BIASWARNONCE(
"Inefficient code: affine sparse tracking, needs rewrite."
622 <<
" either use precomputed gradients or implment this ");
632 bool AffineBrightnessInvariance = _AffineBrightnessInvariance;
633 _AffineBrightnessInvariance =
false;
639 for (j = -hh ; j <= hh ; j++) {
640 for (i = -hw ; i <= hw ; i++) {
648 mi = (x + Axx * i + Axy * j) / (1.0 + h31*i + h32*j);
649 mj = (y + Ayx * i + Ayy * j) / (1.0 + h31*i + h32*j);
651 BilinearRegion2_(mi, mj, 0);
652 gx[j+hh][i+hw] = _gx2[0][0];
653 gy[j+hh][i+hw] = _gy2[0][0];
654 I[j+hh][i+hw] = _bl2[0][0];
657 _AffineBrightnessInvariance = AffineBrightnessInvariance;
658 if (_AffineBrightnessInvariance) {
659 this->NormalizeRegion_(I, gx, gy);
662 KLT_TYPE *pgx = gx.GetData(), *pgy = gy.
GetData();
663 for (
int c = (2*hh+1)*(2*hw+1); c>0; c--) {
664 *out_gradx++ = *pgx++;
665 *out_grady++ = *pgy++;
670 if (_AffineBrightnessInvariance) {
671 for (j = -hh ; j <= hh ; j++) {
672 for (i = -hw ; i <= hw ; i++) {
673 mi = (x + Axx * i + Axy * j) / (1.0 + h31*i + h32*j);
674 mj = (y + Ayx * i + Ayy * j) / (1.0 + h31*i + h32*j);
677 *out_gradx++ = _gradim2x->FastBilinearInterpolationGrey(mi,
679 *out_grady++ = _gradim2y->FastBilinearInterpolationGrey(mi,
684 for (j = -hh ; j <= hh ; j++) {
685 for (i = -hw ; i <= hw ; i++) {
686 mi = (x + Axx * i + Axy * j) / (1.0 + h31*i + h32*j);
687 mj = (y + Ayx * i + Ayy * j) / (1.0 + h31*i + h32*j);
688 *out_gradx++ = _gradim2x->FastBilinearInterpolationGrey(mi, mj);
689 *out_grady++ = _gradim2y->FastBilinearInterpolationGrey(mi, mj);
696 template <
class StorageType>
699 const KLT_TYPE& Axx,
const KLT_TYPE& Ayx,
700 const KLT_TYPE& Axy,
const KLT_TYPE& Ayy,
701 const KLT_TYPE& h31,
const KLT_TYPE& h32,
702 const int hw,
const int hh,
706 KLT_TYPE* imgdifforig = imgdiff;
707 if (_ComputeFilteredImages) {
708 BIASERR(
"FIXME: reeeeally inefficient to interpolate always with size 1 !!!");
710 for (j = -hh ; j <= hh ; j++) {
711 for (i = -hw ; i <= hw ; i++) {
714 KLT_TYPE mi = (x2 + Axx * i + Axy * j) / (1.0 + h31*i + h32*j);
715 KLT_TYPE mj = (y2 + Ayx * i + Ayy * j) / (1.0 + h31*i + h32*j);
716 BilinearRegion2_(mi, mj, 0);
717 *imgdiff++ = _bl1[j+hh][i+hw] - _bl2[0][0];
722 BIASASSERT(_bl1.num_rows()==_bl1.num_cols());
723 BIASASSERT(_bl1.num_rows()==2*hw+1);
725 KLT_TYPE *pbl1 = _bl1.GetData();
726 KLT_TYPE *pbl2 = _bl2.GetData();
727 for (j = -hh ; j <= hh ; j++) {
728 for (i = -hw ; i <= hw ; i++) {
729 mi = (x2 + Axx * i + Axy * j) / (1.0 + h31*i + h32*j);
730 mj = (y2 + Ayx * i + Ayy * j) / (1.0 + h31*i + h32*j);
731 mean2_ += *pbl2 = _im2->FastBilinearInterpolationGrey(mi, mj);
732 *imgdiff++ = *pbl1++ - *pbl2++;
735 if (_AffineBrightnessInvariance) {
736 pbl2 = _bl2.GetData();
737 const KLT_TYPE thearea = (2*hh+1)*(2*hw+1);
741 for (j = -hh ; j <= hh ; j++)
742 for (i = -hw ; i <= hw ; i++) {
743 diff = (*pbl2++ -= mean2_);
744 dev2_ += diff * diff;
746 if (fabs(dev2_)>1e-10) dev2_ = sqrt(thearea / dev2_);
748 imgdiff = imgdifforig;
749 pbl1 = _bl1.GetData();
750 pbl2 = _bl2.GetData();
751 for (j = -hh ; j <= hh ; j++) {
752 for (i = -hw ; i <= hw ; i++) {
753 *imgdiff++ =(*pbl1++) - (*pbl2++ *= dev2_);
767 KLT_TYPE gx, gy, gxx, gxy, gyy, x, y, xx, xy, yy;
771 for (j = 0 ; j < 6 ; j++) {
772 for (i = j ; i < 6 ; i++) {
777 for (j = -hh ; j <= hh ; j++) {
778 for (i = -hw ; i <= hw ; i++) {
821 for (j = 0 ; j < 5 ; j++) {
822 for (i = j+1 ; i < 6 ; i++) {
846 register KLT_TYPE diff, diffgradx, diffgrady;
849 for(i = 0; i < 6; i++) e[i] = 0.0;
852 for (j = -hh ; j <= hh ; j++) {
853 for (i = -hw ; i <= hw ; i++) {
855 diffgradx = diff * (*gradx++);
856 diffgrady = diff * (*grady++);
857 e[0] += diffgradx * i;
858 e[1] += diffgrady * i;
859 e[2] += diffgradx * j;
860 e[3] += diffgrady * j;
867 for(i = 0; i < 6; i++) e[i] *= 0.5;
879 KLT_TYPE gx, gy, x, y;
883 for (j = 0 ; j < 4 ; j++) {
884 for (i = 0 ; i < 4 ; i++) {
889 for (j = -hh ; j <= hh ; j++) {
890 for (i = -hw ; i <= hw ; i++) {
895 T[0][0] += (x*gx+y*gy) * (x*gx+y*gy);
896 T[0][1] += (x*gx+y*gy)*(x*gy-y*gx);
897 T[0][2] += (x*gx+y*gy)*gx;
898 T[0][3] += (x*gx+y*gy)*gy;
900 T[1][1] += (x*gy-y*gx) * (x*gy-y*gx);
901 T[1][2] += (x*gy-y*gx)*gx;
902 T[1][3] += (x*gy-y*gx)*gy;
911 for (j = 0 ; j < 3 ; j++) {
912 for (i = j+1 ; i < 4 ; i++) {
926 register KLT_TYPE diff, diffgradx, diffgrady;
929 for (i = 0; i < 4; i++) e[i] = 0.0;
932 for (j = -hh ; j <= hh ; j++) {
933 for (i = -hw ; i <= hw ; i++) {
935 diffgradx = diff * (*gradx++);
936 diffgrady = diff * (*grady++);
937 e[0] += diffgradx * i + diffgrady * j;
938 e[1] += diffgrady * i - diffgradx * j;
944 for (i = 0; i < 4; i++) e[i] *= 0.5;
965 for (j = -hh ; j <= hh ; j++) {
966 for (i = -hw ; i <= hw ; i++) {
967 const double hom = h31*i+h32*j+1.0;
968 const KLT_TYPE gx = *gradx++ / hom;
969 const KLT_TYPE gy = *grady++ / hom;
977 dIdH[6] = -i/hom * (gx*(a11*i+a12*j+tx) + gy*(a21*i+a22*j+ty));
978 dIdH[7] = -j/hom * (gx*(a11*i+a12*j+tx) + gy*(a21*i+a22*j+ty));
980 T += dIdH.OuterProduct(dIdH);
1019 register KLT_TYPE diffgradx, diffgrady;
1022 for(i = 0; i < 8; i++) e[i] = 0.0;
1025 for (j = -hh ; j <= hh ; j++) {
1026 for (i = -hw ; i <= hw ; i++) {
1028 const double hom = h31*i+h32*j+1.0;
1029 diffgradx = *imgdiff * (*gradx++) / hom;
1030 diffgrady = *imgdiff++ * (*grady++) / hom;
1032 e[0] += diffgradx*i;
1033 e[1] += diffgrady*i;
1034 e[2] += diffgradx*j;
1035 e[3] += diffgrady*j;
1038 e[6] += -diffgradx*(a11*i+a12*j+tx)/hom*i
1039 -diffgrady*(a21*i+a22*j+ty)/hom*i;
1040 e[7] += -diffgradx*(a11*i+a12*j+tx)/hom*j
1041 -diffgrady*(a21*i+a22*j+ty)/hom*j;
1047 for(i = 0; i < 8; i++) e[i] *= 0.5;
1065 #ifdef BUILD_IMAGE_INT
1068 #ifdef BUILD_IMAGE_CHAR
1071 #ifdef BUILD_IMAGE_SHORT
1074 #ifdef BUILD_IMAGE_USHORT
1077 #ifdef BUILD_IMAGE_UINT
1080 #ifdef BUILD_IMAGE_DOUBLE
void Compute4by4GradientMatrix(KLT_TYPE *gradx, KLT_TYPE *grady, const int hw, const int hh, BIAS::Matrix< KLT_TYPE > &T)
void Compute6by6GradientMatrix(KLT_TYPE *gradx, KLT_TYPE *grady, const int hw, const int hh, BIAS::Matrix< KLT_TYPE > &T)
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Subscript num_cols() const
Vector< double > Solve(const Vector< double > &y) const
a 3x3 Matrix describing projective transformations between planes
virtual void EvaluateResult_(KLT_TYPE &mad, KLT_TYPE &msd, Matrix< KLT_TYPE > &cov)
fill in computed residui from Track_
void ComputeIntensityDifferenceAffine_(const KLT_TYPE &x2, const KLT_TYPE &y2, const KLT_TYPE &Axx, const KLT_TYPE &Ayx, const KLT_TYPE &Axy, const KLT_TYPE &Ayy, const KLT_TYPE &h31, const KLT_TYPE &h32, const int halfwidth, const int halfheight, KLT_TYPE *imgdiff)
TrackerBaseAffine(bool only4params=false, bool useHomography=false)
Matrix< T > & newsize(Subscript M, Subscript N)
int TrackAffine_(KLT_TYPE p1[2], KLT_TYPE p2[2], const Matrix2x2< KLT_TYPE > &p2A, KLT_TYPE result[2], Matrix2x2< KLT_TYPE > &resultA, KLT_TYPE &error, int &iter, Matrix< KLT_TYPE > &Cov, KLT_TYPE &mad, KLT_TYPE &msd, bool SimilarityTransformOnly=false, bool ComputeCovariance=true)
track using affine warp
void GetJacobian(const HomgPoint2D &x, Matrix< double > &Jac) const
returns jacobian of H: R^2 –> R^2
const T * GetData() const
get the data pointer the member function itself is const (before {..}) because it doesn't change the ...
long int ComputeCovariance(long int NumErrors, long int NumParams, double *Jac, int *Permutation, double &SumOfSquaredErrors, Matrix< double > &Cov)
Compute covariance matrix from Levenberg-Marquardt resulting Jacobian matrix J(x) and permutation mat...
success (error < maxerror)
void SetZero()
Sets all values to zero.
unsigned int GetRows() const
no spatial structure is present
void Compute8by8GradientMatrix(KLT_TYPE *gradx, KLT_TYPE *grady, KLT_TYPE a11, KLT_TYPE a12, KLT_TYPE a21, KLT_TYPE a22, KLT_TYPE tx, KLT_TYPE ty, KLT_TYPE h31, KLT_TYPE h32, const int hw, const int hh, BIAS::Matrix< KLT_TYPE > &T)
int GaussJordanElimination(BIAS::Matrix< KLT_TYPE > &a, BIAS::Vector< KLT_TYPE > &b)
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
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.
static std::string toString(const T thenumber, int numzeros=DEFAULT_LEADING_ZEROS)
Converts number to string with leading zeros.
void Compute4by1ErrorVector(KLT_TYPE *imgdiff, KLT_TYPE *gradx, KLT_TYPE *grady, const int hw, const int hh, BIAS::Vector< KLT_TYPE > &e)
void Compute6by1ErrorVector(KLT_TYPE *imgdiff, KLT_TYPE *gradx, KLT_TYPE *grady, const int hw, const int hh, BIAS::Vector< KLT_TYPE > &e)
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
void Compute8by1ErrorVector(KLT_TYPE *imgdiff, KLT_TYPE *gradx, KLT_TYPE *grady, KLT_TYPE a11, KLT_TYPE a12, KLT_TYPE a21, KLT_TYPE a22, KLT_TYPE tx, KLT_TYPE ty, KLT_TYPE h31, KLT_TYPE h32, const int hw, const int hh, BIAS::Vector< KLT_TYPE > &e)
virtual int Track_(Vector2< KLT_TYPE > &p1, Vector2< KLT_TYPE > &p2, Vector2< KLT_TYPE > &result, KLT_TYPE &error, int &iter, const Matrix2x2< KLT_TYPE > &AffinePred, Matrix2x2< KLT_TYPE > &AffineResult)
Interface of all tracking algorithms implemented in derived classes.
double NormFrobenius() const
Return Frobenius norm = sqrt(trace(A^t * A)).
void GetGradientWinAffine_(const KLT_TYPE &x, const KLT_TYPE &y, const KLT_TYPE &Axx, const KLT_TYPE &Ayx, const KLT_TYPE &Axy, const KLT_TYPE &Ayy, const KLT_TYPE &h31, const KLT_TYPE &h32, const int halfwidth, const int halfheight, KLT_TYPE *out_gradx, KLT_TYPE *out_grady)
void SetZero()
zeroes the image
class BIASGeometryBase_EXPORT HomgPoint2D
maxiter is reached and error is above maxerror
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase