Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
LocalAffineFrame.cpp
1 #include "Geometry/LocalAffineFrame.hh"
2 #include "Base/ImageUtils/ImageDraw.hh"
3 #include "Base/Image/ColourRGB.hh"
4 #include "Geometry/HMatrix.hh"
5 
6 using namespace BIAS;
7 using namespace std;
8 
9 ostream& BIAS::operator<<(ostream& os, const LocalAffineFrame& fp)
10 {
11  os << " "<< fp.t_<< " " << fp.A_ << endl;
12  return os;
13 }
14 
15 istream& BIAS::operator>>(istream& is, LocalAffineFrame& fp)
16 {
17  is >> fp.t_;
18  BIASASSERT(is.good());
19  is >> fp.A_;
20  return is;
21 }
22 
23 #ifdef BIAS_HAVE_XML2
24 int LocalAffineFrame::XMLGetClassName(std::string& TopLevelTag,
25  double& Version) const {
26  TopLevelTag = "LocalAffineFrame";
27  Version = 0.1;
28  return 0;
29 }
30 
31 int LocalAffineFrame::XMLOut(const xmlNodePtr Node,
32  XMLIO& XMLObject) const {
33 
34  BIASERR("not yet implemented");
35  BIASABORT;
36  return 0;
37 }
38 
39 int LocalAffineFrame::XMLIn(const xmlNodePtr Node, XMLIO& XMLObject) {
40  BIASERR("not yet implemented");
41  BIASABORT;
42  return 0;
43 }
44 #endif
45 
46 
48  const double& positionCovScale,
49  const unsigned int linethickness) const {
50 
51  // draw elliptic shape and orientation
52 
53  double center[2]={t_[0], t_[1]}, a[2], b[2];
54  // length of ellipse main axes
55  double eva,evb;
57  Matrix2x2<double> theta;
58 
60  theta, eva, evb);
61 
62 
63  // ellipse
64  a[0] = phi[0][0] * eva;
65  a[1] = phi[1][0] * eva;
66  b[0] = phi[0][1] * evb;
67  b[1] = phi[1][1] * evb;
68  unsigned char value[]={127, 255, 0};
69  BIAS::ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, value);
70 
71  // orientation
72  Vector2<double> oriLine(0,1);
73  oriLine = A_ * oriLine;
74  if (targetImage.IsPositionInImage((int)rint(center[0]),
75  (int)rint(center[1])) &&
76  targetImage.IsPositionInImage((int)rint(center[0])+
77  (int)rint(oriLine[0]),
78  (int)rint(center[1]) +
79  (int)rint(oriLine[1]))) {
81  Line(targetImage,
82  (int)rint(center[0]), (int)rint(center[1]),
83  (int)rint(center[0])+ (int)rint(oriLine[0]),
84  (int)rint(center[1]) + (int)rint(oriLine[1]));
85  }
86 
87 
88  // draw covariance ellipse if desired
89  if (positionCovScale>0.0) {
90  double a[2], b[2], eva, evb;
91  Vector2<double> na, nb;
92  double center[2]={t_[0], t_[1]};
93  unsigned char col[]={255, 0, 0};
94  Matrix2x2<double> curcov;
95  for (unsigned int i=0; i<2; i++) {
96  for (unsigned int j=0; j<2; j++) {
97  curcov[i][j] = Cov_[4+i][4+j];
98  }
99  }
100  curcov.EigenvalueDecomposition(eva, na, evb, nb);
101  // eigenvalues are the \sigma^2
102  eva = sqrt(eva);
103  evb = sqrt(evb);
104  //out << eva<<", "<<evb<<"\n";
105  a[0] = na[0] * eva * positionCovScale;
106  a[1] = na[1] * eva * positionCovScale;
107  b[0] = nb[0] * evb * positionCovScale;
108  b[1] = nb[1] * evb * positionCovScale;
109  //cout<<"drawing ellipse at "<<center<<" with a="<<a[0]<<" "
110  // <<a[1]<<" b="<<b[0]<<" "<<b[1]<<endl;
111  ImageDraw<unsigned char>::Ellipse(targetImage, center, a, b, col);
112  }
113 }
114 
116 GetRelativeTransform(const LocalAffineFrame& F2, bool defaultCov) const {
117  LocalAffineFrame relativeTransform;
118  // first set transform
119  //cout<<GetAsMatrix()<<" "<< F2.GetAsInverseMatrix()<<endl;
120  relativeTransform.SetFromMatrix(GetAsMatrix() * F2.GetAsInverseMatrix());
121 
122  if (defaultCov) {
123  // use only standard cov without actually computing it
124  relativeTransform.SetDefaultCov();
125  } else {
126  // linear error propagation!
127 
128  // make big cov with uncertainties of both transforms:
129  Matrix<double> covbc(12,12,MatrixZero);
130  for (unsigned int i=0; i<6; i++) {
131  for (unsigned int j=i; j<6; j++) {
132  covbc[i][j] = covbc[j][i] = 0.5 * (Cov_[j][i]+Cov_[i][j]);
133  covbc[i+6][j+6] = covbc[j+6][i+6] = 0.5 * (F2.Cov_[j][i]+F2.Cov_[i][j]);
134  }
135  }
136  // short names for vars:
137  const double b11=A_[0][0], b12=A_[0][1], b21=A_[1][0], b22=A_[1][1];
138  const double c11=F2.A_[0][0], c12=F2.A_[0][1], c21=F2.A_[1][0],
139  c22=F2.A_[1][1], c13=F2.t_[0], c23=F2.t_[1];
140 
141  const double detc = c11*c22-c21*c12;
142 
143  // compute jacobian of parameter change (this(6), f2(6) --> relativ(6))
144  Matrix<double> dadbc(6,12,MatrixZero);
145  // da/db:
146  dadbc[0][0] = c22/detc;
147  dadbc[0][1] = -c21/detc;
148  dadbc[1][0] = -c12/detc;
149  dadbc[1][1] = c11/detc;
150  dadbc[4][0] = (c12*c23-c22*c13)/detc;
151  dadbc[4][1] = (c21*c13-c11*c23)/detc;
152  dadbc[4][4] = 1;
153 
154  dadbc[2][2] = c22/detc;
155  dadbc[2][3] = -c21/detc;
156  dadbc[3][2] = -c12/detc;
157  dadbc[3][3] = c11/detc;
158  dadbc[5][2] = (c12*c23-c22*c13)/detc;
159  dadbc[5][3] = (c21*c13-c11*c23)/detc;
160  dadbc[5][5] = 1;
161 
162  const double detc2 = detc*detc;
163  // da/dc11
164  dadbc[0][6] = -c22*(b11*c22-b12*c21)/detc2;
165  dadbc[1][6] = (b12*detc-c22*(b12*c11-b11*c12))/detc2;
166  dadbc[4][6] = (-c23*b12*detc-c22*(b11*(c12*c23-c22*c13) +
167  b12*(c21*c13-c11*c23)))/detc2;
168  dadbc[2][6] = -c22*(b21*c22-b22*c21)/detc2;
169  dadbc[3][6] = (b22*detc-c22*(b22*c11-b21*c12))/detc2;
170  dadbc[5][6] = (-c23*b22*detc-c22*(b21*(c12*c23-c22*c13) +
171  b22*(c21*c13-c11*c23)))/detc2;
172 
173  // da/dc12
174  dadbc[0][7] = c21*(b11*c22-b12*c21)/detc2;
175  dadbc[1][7] = (-b11*detc+c21*(b12*c11-b11*c12))/detc2;
176  dadbc[4][7] = (c23*b11*detc+c21*(b11*(c12*c23-c22*c13)+
177  b12*(c21*c13-c11*c23)))/detc2;
178  dadbc[2][7] = c21*(b21*c22-b22*c21)/detc2;
179  dadbc[3][7] = (-b21*detc+c21*(b22*c11-b21*c12))/detc2;
180  dadbc[5][7] = (c23*b21*detc+c21*(b21*(c12*c23-c22*c13)+
181  b22*(c21*c13-c11*c23)))/detc2;
182 
183  // da/dc13
184  dadbc[4][10] = (b12*c21-c22*b11)/detc;
185  dadbc[5][10] = (b22*c21-c22*b21)/detc;
186 
187  // da/dc21
188  dadbc[0][8] = (c12*(b11*c22-b12*c21)-b21*detc)/detc2;
189  dadbc[1][8] = c12*(b12*c11-b11*c12)/detc2;
190  dadbc[4][8] = (b12*c13*detc+c12*(b11*(c12*c23-c22*c13)+
191  b12*(c21*c13-c11*c23)))/detc2;
192  dadbc[2][8] = (c12*(b21*c22-b22*c21)-b22*detc)/detc2;
193  dadbc[3][8] = c12*(b22*c11-b21*c12)/detc2;
194  dadbc[5][8] = (b22*c13*detc+c12*(b21*(c12*c23-c22*c13)+
195  b22*(c21*c13-c11*c23)))/detc2;
196 
197  // da/dc22
198  dadbc[0][9]= (-c11*(b11*c22-b12*c21)+b11*detc)/detc2;
199  dadbc[1][9]= c11*(b12*c11-b11*c12)/detc2;
200  dadbc[4][9]= (-b11*c13*detc-c11*(b11*(c12*c23-c22*c13)+
201  b12*(c21*c13-c11*c23)))/detc2;
202  dadbc[2][9]= (-c11*(b21*c22-b22*c21)+b21*detc)/detc2;
203  dadbc[3][9]= c11*(b22*c11-b21*c12)/detc2;
204  dadbc[5][9]= (-b21*c13*detc-c11*(b21*(c12*c23-c22*c13)+
205  b22*(c21*c13-c11*c23)))/detc2;
206 
207  // da/dc23
208  dadbc[4][11]= (b11*c12-b12*c11)/detc;
209  dadbc[5][11]= (b21*c12-b22*c11)/detc;
210 
211  // linear error propagation
212  relativeTransform.Cov_ = dadbc * covbc * dadbc.Transpose();
213  }
214 
215  return relativeTransform;
216 }
217 
218 
221  LocalAffineFrame result;
222  Matrix2x2<double> jac;
223  HomgPoint2D x(t_);
224  H.GetJacobian(x,jac);
225  x = H*x;
226  x.Homogenize();
227  Matrix<double> affineTransformer(6,6,MatrixZero);
228  affineTransformer[1][1] = affineTransformer[0][0] = jac[0][0];
229  affineTransformer[3][3] = affineTransformer[2][2] = jac[1][1];
230  affineTransformer[2][0] = affineTransformer[3][1] = jac[1][0];
231  affineTransformer[0][2] = affineTransformer[1][3] = jac[0][1];
232  affineTransformer[4][4] = jac[0][0];
233  affineTransformer[4][5] = jac[0][1];
234  affineTransformer[5][4] = jac[1][0];
235  affineTransformer[5][5] = jac[1][1];
236 
237  Vector<double> affineparams(GetAsVector());
238  //cout<<"affine params are "<< affineparams << endl;
239 
240  affineparams = affineTransformer * affineparams;
241  // put in correct position
242  affineparams[4] = x[0];
243  affineparams[5] = x[1];
244  //cout<<"affine params after H are "<< affineparams << endl;
245  //cout<<"H was "<<H<<endl;
246  Matrix<double> cov(6,6,MatrixZero);
247  cov = affineTransformer * Cov_ * affineTransformer.Transpose();
248  result.SetFromVector(affineparams, cov);
249  return result;
250 }
251 
253  LocalAffineFrame Sim;
254  // a = vec(A_)
255  Vector<double> a(4);
256  a[0] = A_[0][0];
257  a[1] = A_[1][0];
258  a[2] = A_[0][1];
259  a[3] = A_[1][1];
260  // construct mat so that mat * (x,y)^T = a where x=l*cos, y=l*sin
261  Matrix<double> mat(4,2,MatrixZero);
262  mat[0][0] = 1;
263  mat[1][1] = -1;
264  mat[2][1] = 1;
265  mat[3][0] = 1;
266  Matrix2x2<double> tammat;
267  mat.GetSystemMatrix(tammat);
268  Vector<double> xy = tammat.Invert() * mat.Transpose()*a;
269  tammat[0][0] = xy[0];
270  tammat[0][1] = xy[1];
271  tammat[1][0] = -xy[1];
272  tammat[1][1] = xy[0];
273  Sim.SetT(GetT());
274  Sim.SetA(tammat);
275  return Sim;
276 }
int Invert(Matrix2x2< T > &result) const
analyticaly inverts matrix
Definition: Matrix2x2.cpp:115
LocalAffineFrame GetRelativeTransform(const LocalAffineFrame &F2, bool setDefaultCov=false) const
get global transform to transfer an image point of 1 into the image system of 2, e.g.
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
BIAS::Vector2< double > t_
original image position
a 3x3 Matrix describing projective transformations between planes
Definition: HMatrix.hh:39
Matrix< T > Transpose() const
transpose function, storing data destination matrix
Definition: Matrix.hh:823
BIAS::LocalAffineFrame GetLocalSimilarityFrame() const
find closest scaled rotation matrix to linear A (under Frobenius norm) and return ...
affine transformation of 2D image plane which relates image coordinate system and local affine featur...
bool IsPositionInImage(const int &x, const int &y) const
check if image contains that pixel position
Definition: ImageBase.hh:110
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
Definition: HomgPoint2D.hh:215
static int FactorizeAffineMatrixRRight(const Matrix2x2< double > &A, Matrix2x2< double > &phi, Matrix2x2< double > &theta, double &d1, double &d2)
decompose affine matrix into rotations and nonisotropic scalings
Definition: HMatrix.cpp:262
void GetJacobian(const HomgPoint2D &x, Matrix< double > &Jac) const
returns jacobian of H: R^2 –> R^2
Definition: HMatrix.cpp:190
void SetA(const BIAS::Matrix2x2< double > &A, const BIAS::Matrix< double > &cov=BIAS::Matrix< double >(4, 4, BIAS::MatrixZero))
return affine matrix
void SetDefaultCov()
sets default nonzero cov
virtual int XMLOut(const xmlNodePtr Node, BIAS::XMLIO &XMLObject) const
specialization of XML write function
BIAS::Matrix2x2< double > A_
"orientation + area(detA) of feature" affine matrix which transforms all pixels of the unit feature (...
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
Definition: XMLIO.hh:72
void SetFromVector(const BIAS::Vector< double > &affineparams)
set from a 6-vector (a11,a12,a21,a22,tx,ty), default cov
void SetFromMatrix(const BIAS::Matrix< double > &affineTransform)
construct from homography-style affine matrix (last row 0,0,1)
static int Line(Image< StorageType > &im, const unsigned int start[2], const unsigned int end[2], const StorageType value[])
lines
Definition: ImageDraw.cpp:404
static int Ellipse(Image< StorageType > &im, double center[2], double a[2], double b[2], const StorageType Value[])
draws an ellipse at center with half axes a and b
Definition: ImageDraw.cpp:1255
void SetT(const BIAS::Vector2< double > &T, const BIAS::Matrix< double > &cov=BIAS::Matrix< double >(2, 2, BIAS::MatrixZero))
return offset
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
specialization of XML block name function
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
Definition: Array2D.hh:260
virtual int XMLIn(const xmlNodePtr Node, BIAS::XMLIO &XMLObject)
specialization of XML read function
int EigenvalueDecomposition(T &value1, Vector2< T > &vector1, T &value2, Vector2< T > &vector2) const
Eigenvalue decomposition.
Definition: Matrix2x2.cpp:131
BIAS::Matrix< double > Cov_
6x6 covariance matrix of the 6 region parameters(a11,a12,a21,a22,tx,ty)
void Draw(BIAS::Image< unsigned char > &targetImage, const double &positionCovScale=1.0, const unsigned int linethickness=1) const
draw a square at the feature's ori position
BIAS::Matrix< double > GetAsInverseMatrix() const
returns matrix which transforms a point in image coordinates into a point in feature coordinates (Glo...
LocalAffineFrame GetHomographyTransformed(const BIAS::HMatrix &H) const
use linear propagation to transform the local affine frame e.g.
void GetSystemMatrix(Matrix< T > &dest) const
compute square system matrix dest = A^T * A
Definition: Matrix.hh:1075
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.
Definition: TimeStamp.cpp:157