TooN Algorithm Library - tag  0.2
ransac_estimators.h
Go to the documentation of this file.
1 #ifndef TAG_RANSAC_ESTIMATORS_H
2 #define TAG_RANSAC_ESTIMATORS_H
3 
4 #include <vector>
5 #include <algorithm>
6 #include <cassert>
7 
8 #include <TooN/TooN.h>
9 #include <TooN/helpers.h>
10 #include <TooN/Cholesky.h>
11 #include <TooN/wls.h>
12 #include <TooN/LU.h>
13 #include <TooN/SVD.h>
14 #include <TooN/SymEigen.h>
15 #include <TooN/se3.h>
16 #include <TooN/wls.h>
17 
18 #include <tag/helpers.h>
19 #include <tag/absorient.h>
20 
21 namespace tag {
22 
23 #if 0
24 namespace essential_matrix {
25 
26  template <class M> inline int getValidPair(const TooN::Matrix<3>& R1, const TooN::Matrix<3>& R2, const TooN::Vector<2>& e, double z1, const M& m)
27  {
28  TooN::Vector<2> dm = second_point(m)-e;
29  TooN::Vector<3> ha = TooN::unproject(first_point(m));
30  TooN::Vector<3> inf1 = R1*ha;
31  TooN::Vector<3> inf2 = R2*ha;
32  double zp1 = inf1[2];
33  double zp2 = inf2[2];
34  TooN::Vector<2> pinf1 = project(inf1);
35  TooN::Vector<2> pinf2 = project(inf2);
36  if (zp1*dm*(pinf1 - e) >= 0) {
37  // R1
38  if (zp1 < 0)
39  return z1 <= 0 ? 0 : 1;
40  // check for sign match
41  return ((pinf1-second_point(m))*dm*z1 >= 0) ? 0 : 1;
42  } else {
43  //R2
44  if (zp2 < 0)
45  return z1 <= 0 ? 2 : 3;
46  return ((pinf2-second_point(m))*dm*z1 >= 0) ? 2 : 3;
47  }
48  }
49 
50  template <int N, class Accessor> double determinant(const TooN::FixedMatrix<N,N,Accessor>& M)
51  {
52  TooN::LU<N> lu(M);
53  double det = 1;
54  for (int i=0; i<N; i++)
55  det *= lu.get_lu()[i][i];
56  return det;
57  }
58 
68 
69  struct EssentialMatrix {
71  static const int hypothesis_size = 8;
72 
73  TooN::Matrix<3> E;
74  template <class It> bool estimate(It begin, It end) {
75  TooN::Matrix<9> M = TooN::zeros<9,9>();
76  for (It it=begin; it!= end; ++it) {
77  const TooN::Vector<2>& a = first_point(*it);
78  const TooN::Vector<2>& b = second_point(*it);
79  const double factor = 1.0/noise(*it);
80  const double m[9] = {b[0]*a[0], b[0]*a[1], b[1]*a[0], b[1]*a[1], b[0], b[1], a[0], a[1], 1};
81  for (int j=0; j<9; j++) {
82  for (int k=j; k<9; k++) {
83  M[j][k] += m[j]*m[k] * factor;
84  }
85  }
86  }
87  for (int j=0; j<9;j++)
88  for (int k=j; k<9; k++)
89  M[k][j] = M[j][k];
90  TooN::Matrix<4> M11 = M.template slice<0,0,4,4>();
91  TooN::Matrix<4,5> M12 = M.template slice<0,4,4,5>();
92  TooN::Matrix<5> M22 = M.template slice<4,4,5,5>();
93  TooN::Cholesky<5> chol(M22);
94  if (chol.get_rank() < 5) {
95  if (chol.get_rank() < 3)
96  return false;
97  TooN::Matrix<3> R;
98  // Translation is zero (choose t = [0,0,1])
99  tag::getProjectiveHomography(begin, end, R);
100  TooN::SO3::coerce(R);
101  E[0] = -R[1];
102  E[1] = R[0];
103  E[2][0] = E[2][1] = E[2][2] = 0;
104  return true;
105  }
106  TooN::Matrix<5,4> K = chol.inverse_times(M12.T());
107  TooN::Matrix<4> Q = M11 - M12*K;
108  TooN::SymEigen<4> eigen(Q);
109  TooN::Vector<4> e1 = eigen.get_evectors()[0];
110  TooN::Vector<5> e2 = -(K * e1);
111  E[0][0] = e1[0];
112  E[0][1] = e1[1];
113  E[1][0] = e1[2];
114  E[1][1] = e1[3];
115  E[0][2] = e2[0];
116  E[1][2] = e2[1];
117  E[2] = e2.template slice<2,3>();
118 
119  TooN::SVD<3> svdE(E);
120  const TooN::Vector<3> temp = (TooN::make_Vector, 1, 1, 0);
121  E = svdE.get_U()*TooN::diagmult(temp,svdE.get_VT());
122  return true;
123  }
124 
125  template <class Match> inline bool isInlier(const Match& m, double r) const {
126  TooN::Vector<3> line = E.template slice<0,0,3,2>()*first_point(m) + E.T()[2];
127  double dot = line.template slice<0,2>() * second_point(m) + line[2];
128  return (dot*dot <= (line[0]*line[0] + line[1]*line[1]) * r*r * noise(m));
129  }
130 
131  template <class Match> inline double score(const Match& m) const {
132  TooN::Vector<3> line = E.template slice<0,0,3,2>()*first_point(m) + E.T()[2];
133  double dot = line.template slice<0,2>() * second_point(m) + line[2];
134  return dot*dot / (line[0]*line[0] + line[1]*line[1]);
135  }
136 
137 
143  template <class It> std::vector<std::pair<size_t, TooN::SE3> > decompose(It begin, It end, std::vector<int>& group)
144  {
145  static const double vals[9]={0,-1,0,1,0,0,0,0,1};
146  static const TooN::Matrix<3> Rz(vals);
147 
148  const size_t N = std::distance(begin,end);
149  assert(group.size() >= N);
150 
151  TooN::SVD<3> svdE(E);
152  TooN::Matrix<3> R1 = svdE.get_U()*Rz.T()*svdE.get_VT();
153  TooN::Matrix<3> R2 = svdE.get_U()*Rz*svdE.get_VT();
154  if (essential_matrix::determinant(R1) < 0) {
155  R1 = -1*R1;
156  R2 = -1*R2;
157  }
158  TooN::Vector<3> t1 = svdE.get_U().T()[2];
159  TooN::Vector<3> t2 = -t1;
160  TooN::Vector<2> epipole = project(t1); // which is the same as project(t2)
161  std::vector<std::pair<size_t, TooN::SE3> > result(4,std::make_pair(0,TooN::SE3()));
162  int i=0;
163  for (It it = begin; it!=end; ++it, ++i) {
164  int index = getValidPair(R1, R2, epipole, t1[2], *it);
165  result[index].first++;
166  group[i] = index;
167  }
168  result[0].second.get_rotation() = result[1].second.get_rotation() = R1;
169  result[2].second.get_rotation() = result[3].second.get_rotation() = R2;
170  result[0].second.get_translation() = result[2].second.get_translation() = t1;
171  result[1].second.get_translation() = result[3].second.get_translation() = t2;
172  return result;
173  }
174 
175  };
176 } // close namespace essential_matrix
177 
178 #endif
179 
180 // this is deprecated, use 5 point instead
181 // using essential_matrix::EssentialMatrix;
182 
192 struct Homography {
194  TooN::Matrix<3> H;
196  static const int hypothesis_size = 4;
197 
198  Homography() { H = TooN::Identity; }
199 
200  template <class It> void estimate(It begin, It end) {
201  tag::getProjectiveHomography(begin, end, H);
202  }
203 
204  template <class M> inline double score(const M& m) const {
205  TooN::Vector<3> a = TooN::unproject(first_point(m));
206  const TooN::Vector<2> & b = second_point(m);
207  const TooN::Vector<2> disp = TooN::project(H * a) - b;
208  return (disp*disp);
209  }
210 
211  template <class M> inline bool isInlier(const M& m, double r) const {
212  return this->score(m) <= r*r * noise(m);
213  }
214 };
215 
225 struct AffineHomography {
227  TooN::Matrix<2> A;
229  TooN::Vector<2> t;
231  static const int hypothesis_size = 3;
232 
233  AffineHomography() : A(TooN::Zeros), t(TooN::Zeros) {}
234 
235  template <class It> void estimate(It begin, It end) {
236  TooN::WLS<3> wls_x, wls_y;
237  wls_x.clear();
238  wls_y.clear();
239  for (It it = begin; it!= end; ++it) {
240  const TooN::Vector<2>& a = first_point(*it);
241  const TooN::Vector<2>& b = second_point(*it);
242  const double weight = 1.0 / noise(*it);
243  wls_x.add_mJ(b[0], TooN::unproject(a), weight);
244  wls_y.add_mJ(b[1], TooN::unproject(a), weight);
245  }
246  wls_x.compute();
247  wls_y.compute();
248  TooN::Vector<3> Atx = wls_x.get_mu();
249  TooN::Vector<3> Aty = wls_y.get_mu();
250  A[0] = Atx.template slice<0,2>();
251  A[1] = Aty.template slice<0,2>();
252  t[0] = Atx[2];
253  t[1] = Aty[2];
254  }
255 
256  template <class M> inline double score(const M& m) const {
257  const TooN::Vector<2>& a = first_point(m);
258  const TooN::Vector<2>& b = second_point(m);
259  const TooN::Vector<2> disp = A*a + t - b;
260  return (disp*disp);
261  }
262 
263  template <class M> inline bool isInlier(const M& m, double r) const {
264  return this->score(m) <= r*r * noise(m);
265  }
266 };
267 
278 struct CameraRotation {
280  TooN::SO3<> rotation;
282  static const int hypothesis_size = 2;
283 
284  template <class It> void estimate(It begin, It end) {
285  assert(std::distance(begin, end) == hypothesis_size);
286  if(TooN::norm_sq(first_point(begin[0])) == first_point(begin[0]) * first_point(begin[1]) ||
287  TooN::norm_sq(second_point(begin[0])) == second_point(begin[0]) * second_point(begin[1]))
288  return;
289  rotation = tag::computeOrientation(TooN::unproject(first_point(begin[0])),
290  TooN::unproject(second_point(begin[0])),
291  TooN::unproject(first_point(begin[1])),
292  TooN::unproject(second_point(begin[1])));
293  }
294 
295  template <class M> inline double score(const M& m) const {
296  const TooN::Vector<2> disp = TooN::project(rotation * TooN::unproject(first_point(m))) - second_point(m);
297  return (disp*disp);
298  }
299 
300  template <class M> inline bool isInlier(const M& m, double r) const {
301  return this->score(m) <= r*r * noise(m);
302  }
303 };
304 
315 struct PlaneFromPoints {
317  TooN::Vector<4> plane;
319  static const int hypothesis_size = 3;
320 
321  PlaneFromPoints() : plane(TooN::Zeros) {}
322 
323  template <class It> void estimate(It begin, It end){
324  assert(std::distance(begin, end) >= 3);
325  if( std::distance(begin, end) == 3 ){ // fast special case
326  const TooN::Vector<3> d1 = *(begin+1) - *begin;
327  const TooN::Vector<3> d2 = *(begin+2) - *begin;
328  plane.template slice<0,3>() = d1 ^ d2;
329  TooN::normalize(plane.template slice<0,3>());
330  plane[3] = -(*begin) * plane.template slice<0,3>();
331  } else {
332  TooN::Matrix<> design(std::distance(begin, end), 4);
333  for(It p = begin; p != end; ++p)
334  design[p-begin] = TooN::unproject(*p);
335  TooN::SVD<> s(design);
336  plane = s.get_VT()[3];
337  const double d = sqrt(plane.template slice<0,3>() * plane.template slice<0,3>());
338  if(d > 1e-10){
339  plane /= d;
340  } else {
341  plane = TooN::makeVector( 0, 0, 0, 1);
342  }
343  }
344  }
345 
346  template <class M> inline double score(const M & m) const {
347  const double d = plane * TooN::unproject(m);
348  return d*d;
349  }
350 
351  template <class M> inline bool isInlier(const M& m, double r) const {
352  return this->score(m) <= r*r * noise(m);
353  }
354 };
355 
356 } // namespace tag
357 
358 #endif
Generated on Tue Aug 13 2013 14:11:40 for TooN Algorithm Library - tag by   doxygen 1.8.3.1