25 #include "TrackerBaseHomography.hh"
26 #include "Geometry/HMatrix.hh"
27 #include "MathAlgo/Lapack.hh"
28 #include "MathAlgo/SVD.hh"
34 template <
class StorageType>
43 for (
unsigned int i=0; i<2; i++) {
44 for (
unsigned int j=0; j<2; j++) {
45 H[i][j] = AffinePred[i][j];
47 H[i][2] = p2[i]-p1[i];
52 const int hh = _WinSize/2;
58 Jh[0][1] = Jh[1][0] = 0;
66 BilinearRegion1_(p1[0], p1[1], hh);
72 for (iter=0; iter<_MaxIterations; iter++) {
77 for (
int i = -hh ; i <= hh ; i++) {
79 for (
int j = -hh ; j <= hh ; j++) {
87 Jh[0][0] = Jh[1][1] = 1/x2[2];
88 Jh[0][2] = -x2[0]/(x2[2]*x2[2]);
89 Jh[1][2] = -x2[1]/(x2[2]*x2[2]);
91 double ex2_x = x2[0]/x2[2] + p1[0];
92 double ex2_y = x2[1]/x2[2] + p1[1];
93 grad[0][0] = _gradim2x->FastBilinearInterpolationGrey(ex2_x, ex2_y);
94 grad[0][1] = _gradim2y->FastBilinearInterpolationGrey(ex2_x, ex2_y);
100 for (
unsigned int t=0; t<8; t++)
103 BilinearRegion2_(ex2_x, ex2_y, 0);
105 double l = _bl1[j+hh][i+hh] - _bl2[0][0];
120 for (
unsigned i=0,k=0; i<3; i++) {
121 for (
unsigned j=0; (j<3) && (k<8); j++,k++) {
122 H[j][i] += alpha*update[k];
127 for (
int i = -hh ; i <= hh ; i++) {
129 for (
int j = -hh ; j <= hh ; j++) {
132 double ex2_x = x2[0]/x2[2] + p1[0];
133 double ex2_y = x2[1]/x2[2] + p1[1];
134 BilinearRegion2_(ex2_x, ex2_y, 0);
135 double l = _bl1[j+hh][i+hh] - _bl2[0][0];
139 if (Omega_new>Omega) {
140 for (
unsigned i=0,k=0; i<3; i++) {
141 for (
unsigned j=0; (j<3) && (k<8); j++,k++) {
142 H[j][i] -= alpha*update[k];
147 }
while ((alpha>1e-4) && (Omega_new>Omega));
151 double relativeUpdate = alpha*alpha*update.
ScalarProduct(N*update);
155 if (relativeUpdate<1e-4)
163 for (
unsigned int i=0; i<2; i++) {
164 result[i] = p1[i] + H[i][2];
165 for (
unsigned int j=0; j<2; j++) {
166 AffineResult[i][j] = H[i][j] - H[i][2]*H[2][j];
176 template <
class StorageType>
190 #ifdef BUILD_IMAGE_INT
193 #ifdef BUILD_IMAGE_CHAR
196 #ifdef BUILD_IMAGE_SHORT
199 #ifdef BUILD_IMAGE_USHORT
202 #ifdef BUILD_IMAGE_UINT
205 #ifdef BUILD_IMAGE_DOUBLE
T ScalarProduct(const Vector< T > &argvec) const
scalar product (inner product) of two vectors returning a scalr
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
a 3x3 Matrix describing projective transformations between planes
void Kronecker(Matrix< T > const B, Matrix< T > &dest) const
Kronecker-product with matrix, result in dest.
void SetZero()
equivalent to matrix call
BIAS::Vector< double > Lapack_LU_linear_solve(const BIAS::Matrix< double > &A, const BIAS::Vector< double > &b)
solve linear equations using LU factorization
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.
Matrix< T > OuterProduct(const Vector< T > &v) const
outer product, constructs a matrix.
void SetZero()
Sets all values to zero.
void SetIdentity()
Converts matrix to identity matrix.
virtual void EvaluateResult_(KLT_TYPE &mad, KLT_TYPE &msd, Matrix< KLT_TYPE > &cov)
fill in computed residui from Track_