00001
00002 #ifndef vpgl_em_compute_5_point_txx_
00003 #define vpgl_em_compute_5_point_txx_
00004
00005
00006 #include "vpgl_em_compute_5_point.h"
00007
00008 #include <vcl_iostream.h>
00009 #include <vcl_cassert.h>
00010 #include <vcl_cstdlib.h>
00011
00012 #include <vnl/vnl_matrix_fixed.h>
00013 #include <vnl/vnl_inverse.h>
00014 #include <vnl/algo/vnl_complex_eigensystem.h>
00015 #include <vnl/algo/vnl_svd.h>
00016
00017
00018 template <class T>
00019 bool vpgl_em_compute_5_point<T>::compute(
00020 const vcl_vector<vgl_point_2d<T> > &right_points,
00021 const vpgl_calibration_matrix<T> &k_right,
00022 const vcl_vector<vgl_point_2d<T> > &left_points,
00023 const vpgl_calibration_matrix<T> &k_left,
00024 vcl_vector<vpgl_essential_matrix<T> > &ems) const
00025 {
00026 vcl_vector<vgl_point_2d<T> > normed_right_points, normed_left_points;
00027
00028 normalize(right_points, k_right, normed_right_points);
00029 normalize(left_points, k_left, normed_left_points);
00030
00031 return compute(normed_right_points, normed_left_points, ems);
00032 }
00033
00034
00035 template <class T>
00036 bool vpgl_em_compute_5_point<T>::compute(
00037 const vcl_vector<vgl_point_2d<T> > &normed_right_points,
00038 const vcl_vector<vgl_point_2d<T> > &normed_left_points,
00039 vcl_vector<vpgl_essential_matrix<T> > &ems) const
00040 {
00041
00042 if (normed_right_points.size() != 5 || normed_left_points.size() != 5) {
00043 if (verbose) {
00044 vcl_cerr<<"Wrong number of input points!\n" <<
00045 "right_points has "<<normed_right_points.size() <<
00046 " and left_points has "<<normed_left_points.size() << '\n';
00047 }
00048 return false;
00049 }
00050
00051
00052 vcl_vector<vnl_vector_fixed<T,9> > basis;
00053 compute_nullspace_basis(normed_right_points, normed_left_points, basis);
00054
00055
00056
00057 vcl_vector<vnl_real_npolynomial> constraints;
00058 compute_constraint_polynomials(basis, constraints);
00059
00060
00061 vnl_matrix<double> groebner_basis(10, 10);
00062 compute_groebner_basis(constraints, groebner_basis);
00063
00064
00065 vnl_matrix<double> action_matrix(10, 10);
00066 compute_action_matrix(groebner_basis, action_matrix);
00067
00068
00069
00070 compute_e_matrices(basis, action_matrix, ems);
00071
00072 return true;
00073 }
00074
00075
00076
00077
00078
00079
00080 template <class T>
00081 void vpgl_em_compute_5_point<T>::normalize(
00082 const vcl_vector<vgl_point_2d<T> > &points,
00083 const vpgl_calibration_matrix<T> &k,
00084 vcl_vector<vgl_point_2d<T> > &normed_points) const
00085 {
00086 for (unsigned int i = 0; i < points.size(); ++i)
00087 {
00088 vgl_point_2d<T> p = k.map_to_focal_plane(points[i]);
00089 normed_points.push_back(vgl_point_2d<T>(p.x(), p.y()));
00090 }
00091 }
00092
00093
00094
00095
00096
00097
00098 template <class T>
00099 void vpgl_em_compute_5_point<T>::compute_nullspace_basis(
00100 const vcl_vector<vgl_point_2d<T> > &right_points,
00101 const vcl_vector<vgl_point_2d<T> > &left_points,
00102 vcl_vector<vnl_vector_fixed<T, 9> > &basis) const
00103 {
00104
00105 vnl_matrix<T> A(5, 9);
00106
00107 for (int i = 0; i < 5; ++i) {
00108 A.put(i, 0, right_points[i].x() * left_points[i].x());
00109 A.put(i, 1, right_points[i].y() * left_points[i].x());
00110 A.put(i, 2, left_points[i].x());
00111
00112 A.put(i, 3, right_points[i].x() * left_points[i].y());
00113 A.put(i, 4, right_points[i].y() * left_points[i].y());
00114 A.put(i, 5, left_points[i].y());
00115
00116 A.put(i, 6, right_points[i].x());
00117 A.put(i, 7, right_points[i].y());
00118 A.put(i, 8, 1.0);
00119 }
00120
00121
00122
00123 vnl_svd<T> svd(A, tolerance);
00124 vnl_matrix<T> V = svd.V();
00125
00126
00127 for (int i = 5; i < 9; ++i) {
00128 vnl_vector_fixed<T,9> basis_vector;
00129
00130 for (int j = 0; j < 9; ++j) {
00131 basis_vector[j] = V.get(j, i);
00132 }
00133
00134 basis.push_back(basis_vector);
00135 }
00136 }
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 template <class T>
00149 void vpgl_em_compute_5_point<T>::compute_constraint_polynomials(
00150 const vcl_vector<vnl_vector_fixed<T,9> > &basis,
00151 vcl_vector<vnl_real_npolynomial> &constraints) const
00152 {
00153
00154
00155
00156
00157
00158
00159
00160
00161 vcl_vector<vnl_real_npolynomial> entry_polynomials(9);
00162 vnl_vector<double> coeffs(4);
00163
00164 vnl_matrix<unsigned> exps(4, 4);
00165 exps.set_identity();
00166 exps.put(3, 3, 0);
00167
00168 for (int i = 0; i < 9; ++i) {
00169 coeffs[0] = basis[0][i];
00170 coeffs[1] = basis[1][i];
00171 coeffs[2] = basis[2][i];
00172 coeffs[3] = basis[3][i];
00173
00174 entry_polynomials[i].set(coeffs, exps);
00175 }
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 vnl_real_npolynomial sum_of_squares =
00195 entry_polynomials[0] * entry_polynomials[0];
00196
00197 for (int i = 1; i < 9; ++i) {
00198 sum_of_squares = sum_of_squares +
00199 entry_polynomials[i] * entry_polynomials[i];
00200 }
00201
00202
00203
00204 for (int i = 0; i < 9; ++i) {
00205 constraints.push_back(
00206 entry_polynomials[i%3] *
00207 (entry_polynomials[0] * entry_polynomials[3*(i/3) + 0]*2 +
00208 entry_polynomials[1] * entry_polynomials[3*(i/3) + 1]*2 +
00209 entry_polynomials[2] * entry_polynomials[3*(i/3) + 2]*2)
00210
00211 - entry_polynomials[i] * sum_of_squares);
00212 }
00213
00214
00215 for (int i = 0; i < 9; ++i) {
00216 constraints[i] +=
00217 entry_polynomials[(i%3) + 3] *
00218 (entry_polynomials[3] * entry_polynomials[3*(i/3) + 0]*2 +
00219 entry_polynomials[4] * entry_polynomials[3*(i/3) + 1]*2 +
00220 entry_polynomials[5] * entry_polynomials[3*(i/3) + 2]*2);
00221 }
00222
00223
00224 for (int i = 0; i < 9; ++i) {
00225 constraints[i] = (constraints[i] +
00226 entry_polynomials[(i%3) + 6] *
00227 (entry_polynomials[6] * entry_polynomials[3*(i/3) + 0]*2 +
00228 entry_polynomials[7] * entry_polynomials[3*(i/3) + 1]*2 +
00229 entry_polynomials[8] * entry_polynomials[3*(i/3) + 2]*2)) * .5;
00230 }
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240 vnl_real_npolynomial det_term_1 = entry_polynomials[6] *
00241 (entry_polynomials[1] * entry_polynomials[5] -
00242 entry_polynomials[2] * entry_polynomials[4]);
00243
00244
00245 vnl_real_npolynomial det_term_2 = entry_polynomials[7] *
00246 (entry_polynomials[2] * entry_polynomials[3] -
00247 entry_polynomials[0] * entry_polynomials[5]);
00248
00249
00250 vnl_real_npolynomial det_term_3 = entry_polynomials[8] *
00251 (entry_polynomials[0] * entry_polynomials[4] -
00252 entry_polynomials[1] * entry_polynomials[3]);
00253
00254 constraints.push_back(det_term_1 + det_term_2 + det_term_3);
00255 }
00256
00257
00258
00259
00260
00261
00262 template <class T>
00263 double vpgl_em_compute_5_point<T>::get_coeff(
00264 const vnl_real_npolynomial &p,
00265 unsigned int x_p,
00266 unsigned int y_p,
00267 unsigned int z_p) const
00268 {
00269 for (unsigned int i = 0; i < p.polyn().rows(); ++i) {
00270 if (x_p == p.polyn().get(i, 0) &&
00271 y_p == p.polyn().get(i, 1) &&
00272 z_p == p.polyn().get(i, 2)) {
00273 return p.coefficients()[i];
00274 }
00275 }
00276 return -1.0;
00277 }
00278
00279 template <class T>
00280 void vpgl_em_compute_5_point<T>::compute_groebner_basis(
00281 const vcl_vector<vnl_real_npolynomial> &constraints,
00282 vnl_matrix<double> &groebner_basis) const
00283 {
00284 assert(groebner_basis.rows() == 10);
00285 assert(groebner_basis.cols() == 10);
00286
00287 vnl_matrix<double> A(10, 20);
00288
00289 for (int i = 0; i < 10; ++i) {
00290
00291 A.put(i, 0, get_coeff(constraints[i], 3, 0, 0));
00292 A.put(i, 1, get_coeff(constraints[i], 2, 1, 0));
00293 A.put(i, 2, get_coeff(constraints[i], 1, 2, 0));
00294 A.put(i, 3, get_coeff(constraints[i], 0, 3, 0));
00295 A.put(i, 4, get_coeff(constraints[i], 2, 0, 1));
00296 A.put(i, 5, get_coeff(constraints[i], 1, 1, 1));
00297 A.put(i, 6, get_coeff(constraints[i], 0, 2, 1));
00298 A.put(i, 7, get_coeff(constraints[i], 1, 0, 2));
00299 A.put(i, 8, get_coeff(constraints[i], 0, 1, 2));
00300 A.put(i, 9, get_coeff(constraints[i], 0, 0, 3));
00301 A.put(i, 10, get_coeff(constraints[i], 2, 0, 0));
00302 A.put(i, 11, get_coeff(constraints[i], 1, 1, 0));
00303 A.put(i, 12, get_coeff(constraints[i], 0, 2, 0));
00304 A.put(i, 13, get_coeff(constraints[i], 1, 0, 1));
00305 A.put(i, 14, get_coeff(constraints[i], 0, 1, 1));
00306 A.put(i, 15, get_coeff(constraints[i], 0, 0, 2));
00307 A.put(i, 16, get_coeff(constraints[i], 1, 0, 0));
00308 A.put(i, 17, get_coeff(constraints[i], 0, 1, 0));
00309 A.put(i, 18, get_coeff(constraints[i], 0, 0, 1));
00310 A.put(i, 19, get_coeff(constraints[i], 0, 0, 0));
00311 }
00312
00313
00314 for (int i = 0; i < 10; ++i)
00315 {
00316
00317 double leading = A.get(i, i);
00318 A.scale_row(i, 1/leading);
00319
00320
00321 for (int j = i+1; j < 10; ++j) {
00322 double leading2 = A.get(j, i);
00323 vnl_vector<double> new_row =
00324 A.get_row(j) - A.get_row(i) * leading2;
00325
00326 A.set_row(j, new_row);
00327 }
00328 }
00329
00330
00331 for (int i = 9; i >= 0; --i) {
00332 for (int j = 0; j < i; ++j) {
00333 double scale = A.get(j, i);
00334
00335 vnl_vector<double> new_row =
00336 A.get_row(j) - A.get_row(i) * scale;
00337
00338 A.set_row(j, new_row);
00339 }
00340 }
00341
00342
00343
00344
00345 for (int i = 0; i < 10; ++i) {
00346 for (int j = 0; j < 10; ++j) {
00347 groebner_basis.put(i, j, A.get(i, j+10));
00348 }
00349 }
00350 }
00351
00352
00353
00354 template <class T>
00355 void vpgl_em_compute_5_point<T>::compute_action_matrix(
00356 const vnl_matrix<double> &groebner_basis,
00357 vnl_matrix<double> &action_matrix) const
00358 {
00359 action_matrix.fill(0.0);
00360
00361 action_matrix.set_row(0, groebner_basis.get_row(0));
00362 action_matrix.set_row(1, groebner_basis.get_row(1));
00363 action_matrix.set_row(2, groebner_basis.get_row(2));
00364 action_matrix.set_row(3, groebner_basis.get_row(4));
00365 action_matrix.set_row(4, groebner_basis.get_row(5));
00366 action_matrix.set_row(5, groebner_basis.get_row(7));
00367 action_matrix *= -1;
00368
00369 action_matrix.put(6, 0, 1.0);
00370 action_matrix.put(7, 1, 1.0);
00371 action_matrix.put(8, 3, 1.0);
00372 action_matrix.put(9, 6, 1.0);
00373 }
00374
00375
00376
00377 template <class T>
00378 void vpgl_em_compute_5_point<T>::compute_e_matrices(
00379 const vcl_vector<vnl_vector_fixed<T, 9> > &basis,
00380 const vnl_matrix<double> &action_matrix,
00381 vcl_vector<vpgl_essential_matrix<T> > &ems) const
00382 {
00383 vnl_matrix<double> zeros(action_matrix.rows(), action_matrix.cols(), 0);
00384 vnl_complex_eigensystem eigs(action_matrix, zeros);
00385
00386 for (unsigned int i = 0; i < eigs.W.size(); ++i) {
00387 if (vcl_abs(eigs.W[i].imag()) <= tolerance)
00388 {
00389 vnl_vector_fixed<T, 9> linear_e;
00390
00391 double w_inv = 1.0 / eigs.R.get(i, 9).real();
00392 double x = eigs.R.get(i, 6).real() * w_inv;
00393 double y = eigs.R.get(i, 7).real() * w_inv;
00394 double z = eigs.R.get(i, 8).real() * w_inv;
00395
00396 linear_e =
00397 x * basis[0] + y * basis[1] + z * basis[2] + basis[3];
00398 linear_e /= linear_e[8];
00399
00400 ems.push_back(vpgl_essential_matrix<T>(
00401 vnl_matrix_fixed<T, 3, 3>(linear_e.data_block())));
00402 }
00403 }
00404 }
00405
00406
00407
00408 static void get_distinct_indices(
00409 int n, int *idxs, int number_entries)
00410 {
00411 for (int i = 0; i < n; ++i) {
00412 bool found = false;
00413 int idx;
00414
00415 while (!found) {
00416 found = true;
00417 idx = vcl_rand() % number_entries;
00418
00419 for (int j = 0; j < i; ++j) {
00420 if (idxs[j] == idx) {
00421 found = false;
00422 break;
00423 }
00424 }
00425 }
00426
00427 idxs[i] = idx;
00428 }
00429 }
00430
00431
00432 template <class T>
00433 bool vpgl_em_compute_5_point_ransac<T>::compute(
00434 vcl_vector<vgl_point_2d<T> > const& right_points,
00435 vpgl_calibration_matrix<T> const& right_k,
00436
00437 vcl_vector<vgl_point_2d<T> > const& left_points,
00438 vpgl_calibration_matrix<T> const& left_k,
00439
00440 vpgl_essential_matrix<T> &best_em) const
00441 {
00442
00443 if ( right_points.size() != left_points.size()) {
00444 if (verbose) {
00445 vcl_cerr
00446 << "The two vectors of points must be the same size!\n"
00447 << "right_points is size " << right_points.size()
00448 << " while left_points is size " << left_points.size()
00449 << ".\n";
00450 }
00451 return false;
00452 }
00453 else if (right_points.size() < 5) {
00454 if (verbose) {
00455 vcl_cerr
00456 << "There need to be at least 5 points to run the "
00457 << "five-point algorithm!\n"
00458 << "Input only contained " << right_points.size()
00459 << " points.\n";
00460 }
00461 return false;
00462 }
00463
00464
00465 const unsigned num_points = right_points.size();
00466
00467 unsigned best_inlier_count = 0u;
00468
00469 vpgl_em_compute_5_point<T> five_point;
00470
00471 int match_idxs[5];
00472 for (unsigned int r = 0; r < num_rounds; ++r)
00473 {
00474
00475
00476 vcl_vector<vgl_point_2d<T> > right_points_to_use;
00477 vcl_vector<vgl_point_2d<T> > left_points_to_use;
00478
00479 get_distinct_indices(5, match_idxs, num_points);
00480
00481 for (int idx = 0; idx < 5; ++idx) {
00482 right_points_to_use.push_back(right_points[match_idxs[idx]]);
00483 left_points_to_use.push_back(left_points[match_idxs[idx]]);
00484 }
00485 vcl_vector<vpgl_essential_matrix<T> > ems;
00486 five_point.compute(
00487 right_points_to_use, right_k,
00488 left_points_to_use, left_k,
00489 ems);
00490
00491
00492
00493 typename vcl_vector<vpgl_essential_matrix<T> >::const_iterator i;
00494 for (i = ems.begin(); i != ems.end(); ++i) {
00495 vpgl_fundamental_matrix<T> f(right_k, left_k, *i);
00496
00497 vnl_double_3x1 point_r, point_l;
00498
00499
00500 unsigned inlier_count = 0;
00501 for (unsigned j = 0; j < num_points; ++j) {
00502 point_r.put(0, 0, right_points[j].x());
00503 point_r.put(1, 0, right_points[j].y());
00504 point_r.put(2, 0, 1.0);
00505
00506 point_l.put(0, 0, left_points[j].x());
00507 point_l.put(1, 0, left_points[j].y());
00508 point_l.put(2, 0, 1.0);
00509
00510 vnl_double_3x1 f_r = f.get_matrix() * point_r;
00511 vnl_double_3x1 f_l = f.get_matrix().transpose() * point_l;
00512
00513
00514 double p = (point_r.transpose() * f_l).get(0,0);
00515 double error = (1.0 / (f_l.get(0,0) * f_l.get(0,0) + f_l.get(1,0) * f_l.get(1,0)) +
00516 1.0 / (f_r.get(0,0) * f_r.get(0,0) + f_r.get(1,0) * f_r.get(1,0))) * (p * p);
00517
00518 if ( error <= inlier_threshold) {
00519 ++inlier_count;
00520 }
00521 }
00522
00523 if (best_inlier_count < inlier_count) {
00524 best_em = *i;
00525 best_inlier_count = inlier_count;
00526 }
00527 }
00528 }
00529
00530 return true;
00531 }
00532
00533
00534 #define VPGL_EM_COMPUTE_5_POINT_INSTANTIATE(T) \
00535 template class vpgl_em_compute_5_point<T >; \
00536 template class vpgl_em_compute_5_point_ransac<T >
00537
00538 #endif // vpgl_em_compute_5_point_txx_