21 #ifndef __TrackerBaseInterface__hh__
22 #define __TrackerBaseInterface__hh__
24 #include <Base/Common/BIASpragmaStart.hh>
26 #include <Base/Debug/Debug.hh>
27 #include <Base/Geometry/HomgPoint2D.hh>
28 #include <Base/Math/Matrix2x2.hh>
29 #include <Base/Image/Image.hh>
30 #include <MathAlgo/Median1D.hh>
31 #include <Filter/FilterMask.hh>
35 #define KLT_TYPE double
37 #define D_TRACKERB_INIT 0x00000001
38 #define D_TRACKERB_KLT 0x00000002
39 #define D_TRACKERB_BILINEAR 0x00000004
40 #define D_TRACKERB_INPUT 0x00000008
41 #define D_TRACKERB_REJECT 0x00000010
42 #define D_TRACKERB_AFFINE 0x00000020
44 #define TRACKERBASE_INVALID_PIXEL -1e10
74 template <
class StorageType>
143 KLT_TYPE& error,
int& iter, KLT_TYPE& residuumMAD,
151 void Track(std::vector<HomgPoint2D>& p1,
152 std::vector<HomgPoint2D>& p2,
153 std::vector<HomgPoint2D>& p2tracked,
154 std::vector<KLT_TYPE>& error,
155 std::vector<int>& numiter,
156 std::vector<KLT_TYPE>& residuiMAD,
157 std::vector<KLT_TYPE>& residuiMSD,
158 std::vector<int>& res,
165 { _HalfWinSize=hws; _WinSize=(hws<<1)+1; }
167 { _MaxIterations=maxiter; }
169 { _MaxError=maxerr; }
177 { _MaxResiduumMAD=maxres; }
179 { _RejectionType = rejection_type; }
184 _AffineBrightnessInvariance = bi;
230 LOWPASS_FILTERMASK_SEPARABLE
238 GRAD_FILTERMASK_SEPARABLE
282 virtual void EvaluateResult_(KLT_TYPE& mad,KLT_TYPE& msd,
290 int RejectMAD_(std::vector<KLT_TYPE>& sad, std::vector<int>& res);
293 int RejectX84_(std::vector<KLT_TYPE>& ssd, std::vector<KLT_TYPE>& sad,
294 std::vector<int>& res);
302 inline void Resize_(
const int halfwinsize);
308 KLT_TYPE ComputeMaskSum(
const FilterMask& mask);
315 inline void FilterLowpass_ByMask(
int minX,
int minY,
323 inline void FilterLowpass_BySeparableMask(
int minX,
int minY,
331 inline void FilterLowpass_Binomial3x3(
int minX,
int minY,
366 inline void BilinearRegion1_(KLT_TYPE x, KLT_TYPE y,
int hws);
371 inline void BilinearRegion2_(KLT_TYPE x, KLT_TYPE y,
int hws);
375 BilinearRegionFromImages_(KLT_TYPE x, KLT_TYPE y,
int hws,
385 BilinearRegionFromMatrices_(KLT_TYPE x, KLT_TYPE y,
int hws,
398 const KLT_TYPE& min_value = -1e9);
406 template <
class StorageType>
410 _LastHalfWinSize=halfwinsize;
411 _WinSize=(halfwinsize<<1)+1;
412 _gx1.newsize(_WinSize, _WinSize);
413 _gy1.newsize(_WinSize, _WinSize);
414 _gx2.newsize(_WinSize, _WinSize);
415 _gy2.newsize(_WinSize, _WinSize);
416 _gsx.newsize(_WinSize, _WinSize);
417 _gsy.newsize(_WinSize, _WinSize);
418 _bl1.newsize(_WinSize, _WinSize);
419 _bl2.newsize(_WinSize, _WinSize);
422 template <
class StorageType>
434 int borderRows = (kernel.GetRows()-1) / 2;
435 int borderCols = (kernel.GetCols()-1) / 2;
441 for (
int i = 0; i < rows; i++)
443 for (
int j = 0; j < cols; j++)
446 for (
int k = 0; k < (int)kernel.GetRows(); k++)
448 for (
int l = 0; l < (int)kernel.GetCols(); l++)
451 kernel[k][l] * ida[i+k-borderRows+minY][j+l-borderCols+minX];
454 out[i][j] /= maskSum;
459 template <
class StorageType>
472 int borderRows = (vert.Size()-1) / 2;
473 int borderCols = (horz.
Size()-1) / 2;
479 for (
int i = 0; i < rows; i++)
481 for (
int j = 0; j < cols; j++)
484 for (
int k = 0; k < (int)vert.Size(); k++)
486 for (
int l = 0; l < (int)horz.
Size(); l++)
490 * ida[i+k-borderRows+minY][j+l-borderCols+minX];
493 out[i][j] /= maskSum;
498 template <
class StorageType>
508 for (
int i = 0; i < rows; i++)
510 for (
int j = 0; j < cols; j++)
519 ida[y-1][x-1] + 2*ida[y-1][x] + ida[y-1][x+1]
520 +2*ida[y ][x-1] + 4*ida[y ][x] + 2*ida[y ][x+1]
521 + ida[y+1][x-1] + 2*ida[y+1][x] + ida[y+1][x+1];
527 template <
class StorageType>
536 for (
int i = 1; i < rows-1; i++)
538 for (
int j = 1; j < cols-1; j++)
544 - in[i-1][j-1] + in[i-1][j+1]
545 -2*in[i ][j-1] + 2*in[i ][j+1]
546 - in[i+1][j-1] + in[i+1][j+1];
552 template <
class StorageType>
561 for (
int i = 1; i < rows-1; i++)
563 for (
int j = 1; j < cols-1; j++)
569 -in[i-1][j-1] - 2*in[i-1][j] - in[i-1][j+1]
570 +in[i+1][j-1] + 2*in[i+1][j] + in[i+1][j+1];
571 out[i][j] /= (StorageType)8.0;
576 template <
class StorageType>
587 int borderRows = (kernel.GetRows()-1) / 2;
588 int borderCols = (kernel.GetCols()-1) / 2;
595 for (
int i = borderRows; i < rows-borderRows; i++)
597 for (
int j = borderCols; j < cols-borderCols; j++)
600 for (
int k = 0; k < (int)kernel.GetRows(); k++)
602 for (
int l = 0; l < (int)kernel.GetCols(); l++)
604 out[i][j] += kernel[k][l] * in[i+k-borderRows][j+l-borderCols];
607 out[i][j] /= maskSum;
612 template <
class StorageType>
624 int borderRows = (vert.Size()-1) / 2;
625 int borderCols = (horz.
Size()-1) / 2;
632 for (
int i = borderRows; i < rows-borderRows; i++)
634 for (
int j = borderCols; j < cols-borderCols; j++)
637 for (
int k = 0; k < (int)vert.Size(); k++)
639 for (
int l = 0; l < (int)horz.
Size(); l++)
643 * in[i+k-borderRows][j+l-borderCols];
646 out[i][j] /= maskSum;
651 template <
class StorageType>
655 if (_ComputeFilteredImages)
658 int lowpassHWS = hws + _LowpassWindowExtraSize;
659 int lowpassWS = 2*lowpassHWS+2;
660 int lowpassMinX = (int)floor(x) - lowpassHWS;
661 int lowpassMinY = (int)floor(y) - lowpassHWS;
663 switch (_LowpassFilter)
665 case LOWPASS_BINOMIAL3X3:
666 FilterLowpass_Binomial3x3(lowpassMinX, lowpassMinY,
667 lowpassWS, lowpassWS,
670 case LOWPASS_FILTERMASK:
671 FilterLowpass_ByMask(lowpassMinX, lowpassMinY,
672 lowpassWS, lowpassWS,
674 _LowpassMask, _LowpassMaskSum);
676 case LOWPASS_FILTERMASK_SEPARABLE:
677 FilterLowpass_BySeparableMask(lowpassMinX, lowpassMinY,
678 lowpassWS, lowpassWS,
680 _LowpassMask, _LowpassMaskSum);
685 switch (_GradXFilter)
688 Filter_GradXSobel3x3(lowpass, gradx);
690 case GRAD_FILTERMASK:
691 Filter_ByMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
693 case GRAD_FILTERMASK_SEPARABLE:
694 Filter_BySeparableMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
699 switch (_GradYFilter)
702 Filter_GradYSobel3x3(lowpass, grady);
704 case GRAD_FILTERMASK:
705 Filter_ByMask(lowpass, grady, _GradYMask, _GradYMaskSum);
707 case GRAD_FILTERMASK_SEPARABLE:
708 Filter_BySeparableMask(lowpass, grady, _GradYMask, _GradYMaskSum);
712 dev1_ = this->BilinearRegionFromMatrices_(x - lowpassMinX,
715 lowpass, gradx, grady,
720 dev1_ = this->BilinearRegionFromImages_(x, y, hws,
721 *_im1, *_gradim1x, *_gradim1y,
726 template <
class StorageType>
731 if (_ComputeFilteredImages)
734 int lowpassHWS = hws + _LowpassWindowExtraSize;
735 int lowpassWS = 2*lowpassHWS+2;
736 int lowpassMinX = (int)floor(x) - lowpassHWS;
737 int lowpassMinY = (int)floor(y) - lowpassHWS;
739 switch (_LowpassFilter)
741 case LOWPASS_BINOMIAL3X3:
742 FilterLowpass_Binomial3x3(lowpassMinX, lowpassMinY,
743 lowpassWS, lowpassWS,
746 case LOWPASS_FILTERMASK:
747 FilterLowpass_ByMask(lowpassMinX, lowpassMinY,
748 lowpassWS, lowpassWS,
750 _LowpassMask, _LowpassMaskSum);
752 case LOWPASS_FILTERMASK_SEPARABLE:
753 FilterLowpass_BySeparableMask(lowpassMinX, lowpassMinY,
754 lowpassWS, lowpassWS,
756 _LowpassMask, _LowpassMaskSum);
761 switch (_GradXFilter)
764 Filter_GradXSobel3x3(lowpass, gradx);
766 case GRAD_FILTERMASK:
767 Filter_ByMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
769 case GRAD_FILTERMASK_SEPARABLE:
770 Filter_BySeparableMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
775 switch (_GradYFilter)
778 Filter_GradYSobel3x3(lowpass, grady);
780 case GRAD_FILTERMASK:
781 Filter_ByMask(lowpass, grady, _GradYMask, _GradYMaskSum);
783 case GRAD_FILTERMASK_SEPARABLE:
784 Filter_BySeparableMask(lowpass, grady, _GradYMask, _GradYMaskSum);
788 dev2_ = this->BilinearRegionFromMatrices_(x - lowpassMinX,
791 lowpass, gradx, grady,
794 dev2_ = this->BilinearRegionFromImages_(x, y, hws,
795 *_im2, *_gradim2x, *_gradim2y,
800 template <
class StorageType>
813 int x_floor = (int)floor(x);
814 int y_floor = (int)floor(y);
815 int x_floor_end = x_floor+hws+1;
816 int y_floor_end = y_floor+hws+1;
817 KLT_TYPE brdy = y - (KLT_TYPE)(y_floor);
818 KLT_TYPE brdx = x - (KLT_TYPE)(x_floor);
819 KLT_TYPE brdxy = brdx*brdy;
821 int rf, rc, cf, cc, r, c;
822 for (rf=y_floor-hws, rc=rf+1, r=0; rf<y_floor_end; rf++, rc++, r++) {
823 for (cf=x_floor-hws, cc=cf+1, c=0; cf<x_floor_end; cf++, cc++, c++) {
824 KLT_TYPE ul, ur, ll, lr;
826 ul = (KLT_TYPE)image_ida[rf][cf];
827 ur = (KLT_TYPE)image_ida[rf][cc];
828 ll = (KLT_TYPE)image_ida[rc][cf];
829 lr = (KLT_TYPE)image_ida[rc][cc];
830 bl[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
831 brdxy*(ul + lr - ll - ur));
832 ul = (KLT_TYPE)(gradx_ida[rf][cf]);
833 ur = (KLT_TYPE)(gradx_ida[rf][cc]);
834 ll = (KLT_TYPE)(gradx_ida[rc][cf]);
835 lr = (KLT_TYPE)(gradx_ida[rc][cc]);
836 gx[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
837 brdxy*(ul + lr - ll - ur));
838 ul = (KLT_TYPE)(grady_ida[rf][cf]);
839 ur = (KLT_TYPE)(grady_ida[rf][cc]);
840 ll = (KLT_TYPE)(grady_ida[rc][cf]);
841 lr = (KLT_TYPE)(grady_ida[rc][cc]);
842 gy[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
843 brdxy*(ul + lr - ll - ur));
846 if (_AffineBrightnessInvariance)
return NormalizeRegion_(bl, gx, gy);
850 template <
class StorageType>
859 int x_floor = (int)floor(x);
860 int y_floor = (int)floor(y);
861 int x_floor_end = x_floor+hws+1;
862 int y_floor_end = y_floor+hws+1;
863 KLT_TYPE brdy = y - (KLT_TYPE)(y_floor);
864 KLT_TYPE brdx = x - (KLT_TYPE)(x_floor);
865 KLT_TYPE brdxy = brdx*brdy;
867 int rf, rc, cf, cc, r, c;
868 for (rf=y_floor-hws, rc=rf+1, r=0; rf<y_floor_end; rf++, rc++, r++) {
869 for (cf=x_floor-hws, cc=cf+1, c=0; cf<x_floor_end; cf++, cc++, c++) {
870 KLT_TYPE ul, ur, ll, lr;
872 ul = (KLT_TYPE)image[rf][cf];
873 ur = (KLT_TYPE)image[rf][cc];
874 ll = (KLT_TYPE)image[rc][cf];
875 lr = (KLT_TYPE)image[rc][cc];
876 bl[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
877 brdxy*(ul + lr - ll - ur));
878 ul = (KLT_TYPE)(gradx[rf][cf]);
879 ur = (KLT_TYPE)(gradx[rf][cc]);
880 ll = (KLT_TYPE)(gradx[rc][cf]);
881 lr = (KLT_TYPE)(gradx[rc][cc]);
882 gx[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
883 brdxy*(ul + lr - ll - ur));
884 ul = (KLT_TYPE)(grady[rf][cf]);
885 ur = (KLT_TYPE)(grady[rf][cc]);
886 ll = (KLT_TYPE)(grady[rc][cf]);
887 lr = (KLT_TYPE)(grady[rc][cc]);
888 gy[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) +
889 brdxy*(ul + lr - ll - ur));
892 if (_AffineBrightnessInvariance)
return NormalizeRegion_(bl, gx, gy);
896 template <
class StorageType>
901 const KLT_TYPE& min_value) {
911 KLT_TYPE *pbl = bl.
GetData()-1, *pblend = bl.
GetData() + matrixsize;
912 while (++pbl<pblend) {
913 if (*pbl > min_value) {
916 }
else *pbl = min_value;
919 if (validpixels>0) mean *= 1.0 / KLT_TYPE(validpixels);
923 KLT_TYPE stddev = 0.0;
925 while (++pbl<pblend) {
926 if (*pbl > min_value) {
927 stddev += (*pbl - mean)*(*pbl - mean);
930 if (fabs(stddev)>1e-10) {
931 stddev = sqrt(KLT_TYPE(validpixels)/stddev);
940 while (++pbl<pblend) {
941 if (*pbl > min_value) {
942 *pbl = (*pbl-mean) * stddev;
947 *pbl = TRACKERBASE_INVALID_PIXEL;
957 #include <Base/Common/BIASpragmaEnd.hh>
959 #endif // __TrackerBaseInterface__hh__
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Matrix2x2< KLT_TYPE > _Ginv
Subscript num_cols() const
bool IsSeparable() const
checks if the kernel is separable
KLT_TYPE BilinearRegionFromMatrices_(KLT_TYPE x, KLT_TYPE y, int hws, const Matrix< KLT_TYPE > &image, const Matrix< KLT_TYPE > &gradx, const Matrix< KLT_TYPE > &grady, Matrix< KLT_TYPE > &bl, Matrix< KLT_TYPE > &gx, Matrix< KLT_TYPE > &gy)
Bilinear filtering using matrices as source.
Base class for the different tracking algorithms, defining the interfaces for the tracking functions...
int _RejectionType
The rejection type: RT_MAD,RT_X84 or RT_X84M.
const Matrix< FM_FLOAT > * GetKernelf() const
returns pointer to float filter mask
void BilinearRegion2_(KLT_TYPE x, KLT_TYPE y, int hws)
Fills the matrices _gx2, _gy2 and _bl2 by interpolating gradients and lowpass filtered image at posit...
LowpassFilter _LowpassFilter
Matrix< T > & newsize(Subscript M, Subscript N)
void SetMaxResiduumMAD(const KLT_TYPE maxres)
!!! Also used for X84M as maximal residuum.
bool _AffineBrightnessInvariance
compute brightness "invariant"
void Filter_ByMask(const Matrix< KLT_TYPE > &in, Matrix< KLT_TYPE > &out, const FilterMask &mask, KLT_TYPE maskSum)
Generic gradient filter.
void Filter_BySeparableMask(const Matrix< KLT_TYPE > &in, Matrix< KLT_TYPE > &out, const FilterMask &mask, KLT_TYPE maskSum)
Generic gradient filter using separable filter mask.
unsigned int Size() const
length of the vector
void SetMaxError(const KLT_TYPE maxerr)
void SetHalfWinSize(const int hws)
void FilterLowpass_ByMask(int minX, int minY, int rows, int cols, const Image< StorageType > &image, Matrix< KLT_TYPE > &out, const FilterMask &mask, KLT_TYPE maskSum)
The lowpass filter functions apply a filter to a window of the source image and store the result in a...
GradientFilter _GradYFilter
void SetMaxIterations(const int maxiter)
Image< StorageType > * _im1
const Vector< FM_FLOAT > * GetSepfh() const
returns pointer to horiz float vector for sep.
success (error < maxerror)
unsigned int GetRows() const
no spatial structure is present
KLT_TYPE _MaxError
iteration stops if position refinement is less than *_MaxError
affine warp seems degenerated
input point was at infinity
void FilterLowpass_Binomial3x3(int minX, int minY, int rows, int cols, const Image< StorageType > &image, Matrix< KLT_TYPE > &out)
Optimized lowpass filtering using the binomial 3x3 filter.
const Vector< FM_FLOAT > * GetSepfv() const
returns pointer to vertical float vector for sep.
KLT_TYPE BilinearRegionFromImages_(KLT_TYPE x, KLT_TYPE y, int hws, Image< StorageType > &image, Image< StorageType > &gradx, Image< StorageType > &grady, Matrix< KLT_TYPE > &bl, Matrix< KLT_TYPE > &gx, Matrix< KLT_TYPE > &gy)
Bilinear filtering using images as source (for prefiltered images)
The image template class for specific storage types.
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
point is rejected by X84/X84M
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
void BilinearRegion1_(KLT_TYPE x, KLT_TYPE y, int hws)
Fills the matrices _gx1, _gy1 and _bl1 by interpolating gradients and lowpass filtered image at posit...
int _HalfWinSize
use support window of size 2*hws+1
bool _ComputeFilteredImages
unsigned int GetCols() const
int _LowpassWindowExtraSize
virtual void SetAffineBrightnessInvariance(bool bi)
enable brightness variance and offset invariant computation
void SetRejectionType(const int rejection_type)
int _MaxIterations
iteration stops after _MaxIterations
void Filter_GradYSobel3x3(const Matrix< KLT_TYPE > &in, Matrix< KLT_TYPE > &out)
Optimized gradient Y sobel3x3 filter.
KLT_TYPE _MaxResiduumMAD
outlier rejection via MAD and
Subscript num_rows() const
a point lies outside of valid region in images
point was rejected by MAD criterion
void FilterLowpass_BySeparableMask(int minX, int minY, int rows, int cols, const Image< StorageType > &image, Matrix< KLT_TYPE > &out, const FilterMask &mask, KLT_TYPE maskSum)
Lowpass filtering using a separable filter mask.
A filter mask (or a kernel) used for convolution.
Image< StorageType > * _im2
void Filter_GradXSobel3x3(const Matrix< KLT_TYPE > &in, Matrix< KLT_TYPE > &out)
The gradient filter functions apply a filter to an image window stored in a matrix and store the resu...
Matrix< KLT_TYPE > _weight
error increased from last iteration
void Resize_(const int halfwinsize)
resizes the internal matrices _gx1, _gy1, _bl1, _gx2, _gy2 and _bl2
KLT_TYPE NormalizeRegion_(Matrix< KLT_TYPE > &bl, Matrix< KLT_TYPE > &gx, Matrix< KLT_TYPE > &gy, const KLT_TYPE &min_value=-1e9)
bring region to mean zero and stddev 1 for brightness invariance
maxiter is reached and error is above maxerror
virtual void SetWeightMatrix(const BIAS::Matrix< KLT_TYPE > &weight)
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase