[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/tag ransac.h ransac_estimators.h
|
From: |
Ethan Eade |
|
Subject: |
[Toon-members] tag/tag ransac.h ransac_estimators.h |
|
Date: |
Tue, 13 Jun 2006 01:50:44 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Ethan Eade <ethaneade> 06/06/13 01:50:44
Modified files:
tag : ransac.h ransac_estimators.h
Log message:
Updated RANSAC algorithms and estimators to fix bugs and improve
efficiency.
There are now (only) two estimators defined:
- EssentialMatrix estimates the essential matrix given at least 8
correspondences (using the Torr-Fitzgibbon algorithm) and gives
decomposition into SE(3)
- AffineHomography estimates a transformation of the form Ax+T given at
least 4 correspondences
More estimators will be added as they are tested. The required
interface is
straightforward.
There are three RANSAC algorithms defined:
- find_RANSAC_inliers is classic RANSAC
- find_RANSAC_inliers_breadth_first is preemptive RANSAC
- find_RANSAC_inliers_guided_breadth_first is preemptive RANSAC with a
non-uniform sampling density
The interfaces of estimators and correspondences are slightly changed;
see
the doxygen for details.
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/ransac.h?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/tag/tag/ransac_estimators.h?cvsroot=toon&r1=1.2&r2=1.3
Patches:
Index: ransac.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/ransac.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- ransac.h 31 May 2006 16:46:20 -0000 1.2
+++ ransac.h 13 Jun 2006 01:50:44 -0000 1.3
@@ -12,207 +12,246 @@
/// The functions are both templated on the correspondence data type and
/// the estimator for the transformation.
-template <class T>
-unsigned int getSample(const std::vector<T>& cdf, const T& p){
- return std::lower_bound(cdf.begin(), cdf.end(), p) - cdf.begin();
-}
-
-inline void randomTuple(std::vector<unsigned int>& t, unsigned int bound){
- for (unsigned int i=0; i<t.size(); i++) {
+ template <class T> void randomTuple(const std::vector<T>& cdf,
std::vector<unsigned int>& t, T maxp) {
+ for (size_t i=0; i<t.size(); i++) {
try_again:
- unsigned int r = rand()%bound;
- for (unsigned int j=0; j<i; j++)
+ double x = drand48()* maxp;
+ size_t r = std::min(cdf.size()-1,
(size_t)(std::lower_bound(cdf.begin(), cdf.end(), (T)x) - cdf.begin()));
+ for (size_t j=0; j<i; j++)
if (r == t[j])
goto try_again;
t[i] = r;
}
-}
+ }
-template <class T>
-void randomTuple(const std::vector<T>& cdf, std::vector<unsigned int>& t, T
maxp){
- for (unsigned int i=0; i<t.size(); i++) {
+ template <class T> void randomTuple(T& t, unsigned int bound)
+ {
+ for (size_t i=0; i<t.size(); i++) {
try_again:
- double x = double(rand()*maxp)/RAND_MAX;
- unsigned int r = std::min((unsigned int)cdf.size()-1,
getSample(cdf,(T)x));
- for (unsigned int j=0; j<i; j++)
+ size_t r = (size_t)(drand48() * bound);
+ for (size_t j=0; j<i; j++)
if (r == t[j])
goto try_again;
t[i] = r;
}
-}
+ }
+
-/// basic RANSAC implementation. The function is templated on the
correspondence data type
+/// basic RANSAC implementation. The function is templated on the observation
data type
/// and the transformation data type which must conform to the following
interface:
/// @code
/// class Estimator {
-/// void estimate(const std::vector<Correspondence>& matches);
-/// double getSqError(const Correspondence& m);
+/// Estimator();
+/// // Estimate from a sequence of observations
+/// template <class It> bool estimate(It begin, It End);
+/// // Check whether the given observation is an inlier for this estimate
(with specified tolerance)
+/// template <class Obs, class Tol> bool isInlier(const Obs& obs, const
Tol& tolerance) const;
/// };
/// @endcode
/// see the file @ref ransac_estimators.h for some Estimator classes for
various transformations.
-/// @param[in] matches a vector of matches of type Correspondence
-/// @param[in] dist threshold distance for inliers, the square of dist ist
compared with Estimator::getSqError
-/// @param[in] sampleSize the number of correspondences for a single hypothesis
-/// @param[in] tries the number of hypotheses to test
-/// @param[in] bestT the Estimator object used to compute the transformation
-/// @param[out] isInlier a vector of bools that is set to describe the inlier
set of the winning hypothesis
+/// @param[in] observations a vector of observations (usually point matches)
+/// @param[in] sample_size the number of samples used estimate a transformation
+/// @param[in] tolerance the tolerance (passed with each observation) to the
transformation to check for inliers
+/// @param[in] N the number of hypotheses to test
+/// @param[out] best the transformation hypothesis with the highest inlier
count
+/// @param[out] inlier a vector of bools that describes the inlier set of the
winning hypothesis
/// @return the number of inliers for the winning hypothesis
/// @ingroup ransac
-template <class Correspondence, class Transform>
-int findRANSACInliers(const std::vector<Correspondence>& matches, double dist,
unsigned int sampleSize, unsigned int tries, Transform& bestT,
std::vector<bool>& isInlier){
- assert(isInlier.size() >= matches.size());
- std::vector<bool> tInlier(isInlier.size());
- Transform T = bestT;
- unsigned int i;
- unsigned int most = 0;
- std::vector<unsigned int> index(sampleSize);
- std::vector<Correspondence> sample(sampleSize);
- for (i=0; i<tries; i++) {
- randomTuple(index, matches.size());
- for (unsigned int j=0; j<sampleSize; j++) {
- sample[j] = matches[index[j]];
- }
- T.estimate(sample);
- unsigned int votes = 0;
- for (unsigned int p=0; p<matches.size() && most+p <
matches.size()+votes; p++) {
- double d2 = T.getSqError(matches[p]);
- if (d2 < dist*dist) {
- tInlier[p] = true;
- votes++;
+
+template <class Obs, class Trans, class Tol> size_t find_RANSAC_inliers(const
std::vector<Obs>& observations, int sample_size, const Tol& tolerance, size_t N,
+ Trans&
best, std::vector<bool>& inlier)
+{
+ std::vector<bool> thisInlier(observations.size(),best);
+ size_t bestScore = 0;
+ std::vector<size_t> sample_index(sample_size);
+ vector<Obs> sample(sample_size);
+ while (N--) {
+ randomTuple(sample_index, observations.size());
+ for (int i=0;i<sample_size; i++)
+ sample[i] = observations[sample_index[i]];
+ Trans thisT;
+ thisT.estimate(sample.begin(), sample.end());
+ size_t score = 0;
+ for (size_t i=0; i<observations.size(); i++) {
+ const Obs& o = observations[i];
+ if (thisT.isInlier(o, tolerance)) {
+ inlier[i] = true;
+ score++;
} else
- tInlier[p] = false;
- }
- if (votes > most) {
- most = votes;
- isInlier = tInlier;
- bestT = T;
- if (most == matches.size())
- break;
+ inlier[i] = false;
}
+ if (score > bestScore) {
+ bestScore = score;
+ inlier = thisInlier;
+ best = thisT;
}
- if (most > sampleSize) {
- std::vector<Correspondence> sample(most);
- unsigned int j=0;
- for (i=0;i<matches.size();i++)
- if (isInlier[i])
- sample[j++] = matches[i];
- //bestT.estimate(sample);
}
- return most;
+ return bestScore;
}
-template <class Correspondence, class Transform, class Prob>
-int findRANSACInliersGuidedBFS(const std::vector<Correspondence>& matches,
const Prob& prob, double dist, unsigned int sampleSize, unsigned int tries,
unsigned int perPass, Transform& bestT, std::vector<bool>& isInlier)
-{
- assert(isInlier.size() >= matches.size());
- unsigned int i,j;
+
+ inline double getShrinkRatio(unsigned int H, unsigned int N, unsigned int B)
+ {
+ return pow(double(H), -double(B)/N);
+ }
+
+/// Guided breadth-first RANSAC implementation. The function is templated on
the observation data type,
+/// the probability density for observation correctness, the tolerance for
inliers,
+/// and the transformation data type which must conform to the following
interface:
+/// @code
+/// class Estimator {
+/// Estimator();
+/// // Estimate from a sequence of observations
+/// template <class It> bool estimate(It begin, It End);
+/// // Check whether the given observation is an inlier for this estimate
(with specified tolerance)
+/// template <class Obs, class Tol> bool isInlier(const Obs& obs, const
Tol& tolerance) const;
+/// };
+/// @endcode
+/// All hypotheses are generated first, and preemptively discarded as more
observations are examined.
+/// The sample sets for hypotheses are drawn probabilistically using the
specified probability density.
+/// see the file @ref ransac_estimators.h for some Estimator classes for
various transformations.
+/// @param[in] observations a vector of observations (usually point matches)
+/// @param[in] prob the probability predicate applied (as a function object)
to each observation to compute its prior probability of being correct
+/// @param[in] sample_size the number of samples used estimate a transformation
+/// @param[in] tolerance the tolerance (passed with each observation) to the
transformation to check for inliers
+/// @param[in] N the number of hypotheses to test
+/// @param[in] block_size the number of hypotheses to test in a block (between
culling hypotheses)
+/// @param[out] best the transformation hypothesis with the highest inlier
count
+/// @param[out] inlier a vector of bools that describes the inlier set of the
winning hypothesis
+/// @return the number of inliers for the winning hypothesis
+/// @ingroup ransac
+
+
+ template <class Obs, class Trans, class Tol, class Prob>
+ size_t find_RANSAC_inliers_guided_breadth_first(const std::vector<Obs>&
observations, const Prob& prob, int sample_size, const Tol& tolerance, size_t
N, size_t block_size,
+ Trans& best,
std::vector<bool>& inlier)
+ {
+ std::vector<Trans> hypotheses(N,best);
+ std::vector<std::pair<int,size_t> > score(N);
+ std::vector<size_t> sample_index(sample_size);
+ std::vector<Obs> sample(sample_size);
std::vector<double> cdf(matches.size());
- double psum = 0;
- for (i=0; i<matches.size(); i++) {
- psum += prob(matches[i]);
- cdf[i] = psum;
- }
- std::vector<Transform> T(tries,bestT);
- std::vector<std::pair<int, unsigned int> > score(tries);
- std::vector<Correspondence> sample(sampleSize);
- for (i=0; i<tries; i++) {
- std::vector<unsigned int> index(sampleSize);
- randomTuple(cdf, index, psum);
- for (j=0; j<sampleSize; j++)
- sample[j] = matches[index[j]];
- T[i].estimate(sample);
- score[i].first = 0;
- score[i].second = i;
- }
- unsigned int curr = 0;
- double distSq = dist*dist;
- int toCut = 1;
- while (curr < matches.size()) {
- if (matches.size() - curr < perPass)
- perPass = matches.size() - curr;
- for (i=0; i<score.size(); i++) {
- const Transform& t = T[score[i].second];
- for (unsigned int p=0; p<perPass; p++) {
- double d2 = t.getSqError(matches[curr+p]);
- if (d2 < distSq)
- score[i].first++;
- }
- }
- curr += perPass;
- int cutoff = (score.size()*2)/3;//tries-((toCut*tries)>>passes);
- toCut *= 2;
- if (cutoff > 1) {
- nth_element(score.begin(), score.begin()+cutoff, score.end(),
std::greater<std::pair<int, unsigned int> >());
- score.resize(cutoff);
- } else
+ cdf[0] = prob(observations[0]);
+ for (i=1; i<observations.size(); ++i)
+ cdf[i] = cdf[i-1] + prob(matches[i]);
+ const double psum = cdf.back();
+
+ for (size_t i=0; i<hypotheses.size(); i++) {
+ do {
+ randomTuple(cdf, sample_index, psum);
+ for (int s=0; s<sample_size; ++s)
+ sample[s] = observations[sample_index[s]];
+ }
+ while (!hypotheses[i].estimate(sample.begin(), sample.end()));
+ score[i] = std::make_pair(0,i);
+ }
+ size_t m = 0;
+ const double factor = getShrinkRatio(N, observations.size(),
block_size);
+ while (m < observations.size()) {
+ size_t end = std::min(observations.size(), m+block_size);
+ for (size_t i=0; i<score.size(); i++) {
+ const Trans& thisT = hypotheses[score[i].second];
+ size_t s = 0;
+ for (size_t j=m; j!=end; j++) {
+ if (thisT.isInlier(observations[j], tolerance))
+ ++s;
+ }
+ score[i].first += s;
+ }
+ unsigned int cutoff = (unsigned int)(score.size() * factor);
+ if (cutoff == 0)
break;
+ std::nth_element(score.begin(), score.end(), score.begin()+cutoff,
greater<std::pair<int,size_t> >());
+ score.resize(cutoff);
+ m = end;
}
- bestT = T[max_element(score.begin(), score.end())->second];
- int count = 0;
- for (i=0; i<matches.size(); i++) {
- double d2 = bestT.getSqError(matches[i]);
- if (d2 < distSq) {
- isInlier[i] = true;
- count++;
+ size_t best_index = std::max_element(score.begin(),
score.end())->second;
+ best = hypotheses[best_index];
+ size_t count = 0;
+ inlier.resize(observations.size());
+ for (size_t i=0; i<observations.size(); i++) {
+ if (best.isInlier(observations[i], tolerance)) {
+ inlier[i] = true;
+ ++count;
} else
- isInlier[i] = false;
+ inlier[i] = false;
}
return count;
-}
+ }
-template <class Correspondence, class Transform>
-int findRANSACInliersBFS(const std::vector<Correspondence>& matches, double
dist, unsigned int sampleSize, unsigned int tries, unsigned int perPass,
Transform& bestT, std::vector<bool>& isInlier) {
- assert(isInlier.size() >= matches.size());
- unsigned int i,j;
- std::vector<Transform> T(tries,bestT);
- std::vector<std::pair<int, unsigned int> > score(tries);
- std::vector<Correspondence> sample(sampleSize);
- for (i=0; i<tries; i++) {
- std::vector<unsigned int> index(sampleSize);
- randomTuple(index, matches.size());
- for (j=0; j<sampleSize; j++)
- sample[j] = matches[index[j]];
- T[i].estimate(sample);
- score[i].first = 0;
- score[i].second = i;
- }
- unsigned int curr = 0;
- double distSq = dist*dist;
- int toCut = 1;
- unsigned int passes = tries/perPass + 1;
- while (curr < matches.size()) {
- if (matches.size() - curr < perPass)
- perPass = matches.size() - curr;
- for (i=0; i<score.size(); i++) {
- unsigned int t = score[i].second;
- for (unsigned int p=0; p<perPass; p++) {
- double d2 = T[t].getSqError(matches[curr+p]);
- if (d2 < distSq)
- score[i].first++;
- }
- }
- curr += perPass;
- int cutoff = tries-((toCut*tries)>>passes);
- toCut *= 2;
- if (cutoff > 1) {
- nth_element(score.begin(), score.begin()+cutoff, score.end(),
std::greater<std::pair<int, unsigned int> >());
- score.resize(cutoff);
- } else
+/// Breadth-first RANSAC implementation. The function is templated on the
observation data type, the tolerance for inliers,
+/// and the transformation data type which must conform to the following
interface:
+/// @code
+/// class Estimator {
+/// Estimator();
+/// // Estimate from a sequence of observations
+/// template <class It> bool estimate(It begin, It End);
+/// // Check whether the given observation is an inlier for this estimate
(with specified tolerance)
+/// template <class Obs, class Tol> bool isInlier(const Obs& obs, const
Tol& tolerance) const;
+/// };
+/// @endcode
+/// All hypotheses are generated first, and preemptively discarded as more
observations are examined.
+/// see the file @ref ransac_estimators.h for some Estimator classes for
various transformations.
+/// @param[in] observations a vector of observations (usually point matches)
+/// @param[in] sample_size the number of samples used estimate a transformation
+/// @param[in] tolerance the tolerance (passed with each observation) to the
transformation to check for inliers
+/// @param[in] N the number of hypotheses to test
+/// @param[in] block_size the number of hypotheses to test in a block (between
culling hypotheses)
+/// @param[out] best the transformation hypothesis with the highest inlier
count
+/// @param[out] inlier a vector of bools that describes the inlier set of the
winning hypothesis
+/// @return the number of inliers for the winning hypothesis
+/// @ingroup ransac
+
+
+ template <class Obs, class Trans, class Tol> size_t
find_RANSAC_inliers_breadth_first(const std::vector<Obs>& observations, int
sample_size, const Tol& tolerance, size_t N, size_t block_size,
+
Trans& best, std::vector<bool>& inlier)
+ {
+ std::vector<Trans> hypotheses(N,best);
+ std::vector<std::pair<int,size_t> > score(N);
+ std::vector<size_t> sample_index(sample_size);
+ std::vector<Obs> sample(sample_size);
+ for (size_t i=0; i<hypotheses.size(); i++) {
+ do {
+ randomTuple(sample_index, observations.size());
+ for (int s=0; s<sample_size; ++s)
+ sample[s] = observations[sample_index[s]];
+ }
+ while (!hypotheses[i].estimate(sample.begin(), sample.end()));
+ score[i] = std::make_pair(0,i);
+ }
+ size_t m = 0;
+ const double factor = getShrinkRatio(N, observations.size(),
block_size);
+ while (m < observations.size()) {
+ size_t end = std::min(observations.size(), m+block_size);
+ for (size_t i=0; i<score.size(); i++) {
+ const Trans& thisT = hypotheses[score[i].second];
+ size_t s = 0;
+ for (size_t j=m; j!=end; j++) {
+ if (thisT.isInlier(observations[j], tolerance))
+ ++s;
+ }
+ score[i].first += s;
+ }
+ unsigned int cutoff = (unsigned int)(score.size() * factor);
+ if (cutoff == 0)
break;
+ std::nth_element(score.begin(), score.end(), score.begin()+cutoff,
greater<std::pair<int,size_t> >());
+ score.resize(cutoff);
+ m = end;
}
- int bestIndex = max_element(score.begin(), score.end())-score.begin();
- bestT = T[score[bestIndex].second];
- int count = 0;
- for (i=0; i<matches.size(); i++) {
- double d2 = bestT.getSqError(matches[i]);
- if (d2 < distSq) {
- isInlier[i] = true;
- count++;
+ size_t best_index = std::max_element(score.begin(),
score.end())->second;
+ best = hypotheses[best_index];
+ size_t count = 0;
+ inlier.resize(observations.size());
+ for (size_t i=0; i<observations.size(); i++) {
+ if (best.isInlier(observations[i], tolerance)) {
+ inlier[i] = true;
+ ++count;
} else
- isInlier[i] = false;
+ inlier[i] = false;
}
return count;
-}
+ }
/// helper function for use with ransac functions. It removes elements
Index: ransac_estimators.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/ransac_estimators.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- ransac_estimators.h 31 May 2006 16:46:20 -0000 1.2
+++ ransac_estimators.h 13 Jun 2006 01:50:44 -0000 1.3
@@ -1,267 +1,242 @@
#ifndef TAG_RANSAC_ESTIMATORS_H
#define TAG_RANSAC_ESTIMATORS_H
+#include <vector>
+#include <cassert>
+
#include <TooN/TooN.h>
+#include <TooN/helpers.h>
+#include <TooN/Cholesky.h>
+#include <TooN/wls.h>
+#include <TooN/LU.h>
#include <TooN/SVD.h>
#include <TooN/SymEigen.h>
namespace tag {
-/// RANSAC estimator to compute an affine transformation between a set of
2D-2D correspondences.
-/// The Correspondence datatype has to implement the following interface:
+ template <class T> inline const Vector<2>& first_point(const T& t) {
return t.first; }
+ template <class T> inline const Vector<2>& second_point(const T& t) {
return t.second; }
+ template <class T> inline double noise(const T& t) { return 1.0; }
+
+
+namespace essential_matrix {
+
+ // For degenerate case
+ template <class It> void getProjectiveHomography(It begin, It end,
TooN::Matrix<3>& H)
+ {
+ assert(std::distance(begin,end) >= 4);
+
+ TooN::WLS<8> wls;
+ for (It it=begin; it!=end; it++) {
+ const Vector<2>& a = first_point(*it);
+ const Vector<2>& b = first_point(*it);
+ const double rows[2][8] = {{a[0], a[1], 1, 0, 0, 0, -b[0]*a[0],
-b[0]*a[1]},
+ {0, 0, 0, a[0], a[1], 1, -b[1]*a[0],
-b[1]*a[1]}};
+ wls.add_df(b[0], Vector<8>(rows[0]));
+ wls.add_df(b[1], Vector<8>(rows[1]));
+ }
+ wls.compute();
+ TooN::Vector<8> h = wls.get_mu();
+ H[0] = h.template slice<0,3>();
+ H[1] = h.template slice<3,3>();
+ H[2][0] = h[6];
+ H[2][1] = h[7];
+ H[2][2] = 1;
+ }
+
+
+ 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)
+ {
+ TooN::Vector<2> dm = m.b-e;
+ TooN::Vector<3> ha = TooN::unproject(m.a);
+ TooN::Vector<3> inf1 = R1*ha;
+ TooN::Vector<3> inf2 = R2*ha;
+ double zp1 = inf1[2];
+ double zp2 = inf2[2];
+ TooN::Vector<2> pinf1 = project(inf1);
+ TooN::Vector<2> pinf2 = project(inf2);
+ if (zp1*dm*(pinf1 - e) >= 0) {
+ // R1
+ if (zp1 < 0)
+ return z1 <= 0 ? 0 : 1;
+ // check for sign match
+ return ((pinf1-m.b)*dm*z1 >= 0) ? 0 : 1;
+ } else {
+ //R2
+ if (zp2 < 0)
+ return z1 <= 0 ? 2 : 3;
+ return ((pinf2-m.b)*dm*z1 >= 0) ? 2 : 3;
+ }
+ }
+
+ template <int N, class Accessor> double determinant(const
TooN::FixedMatrix<N,N,Accessor>& M)
+ {
+ TooN::LU<N> lu(M);
+ double det = 1;
+ for (int i=0; i<N; i++)
+ det *= lu.get_lu()[i][i];
+ return det;
+ }
+
+/// RANSAC estimator to compute the essential matrix from a set of 2D-2D
correspondences
+/// The observations passed (via iterators) to the estimate method must allow:
/// @code
-/// struct Correspondence {
-/// TooN::Vector<2> a;
-/// TooN::Vector<2> b;
-/// };
+/// TooN::Vector<2> a = first_point(*it); // default value is "(*it).first"
+/// TooN::Vector<2> b = second_point(*it); // default value is "(*it).second"
+/// double R = noise(*it); // default value is "1.0"
/// @endcode
/// The resulting transformation will map from a -> b.
/// @ingroup ransac
-template <class Correspondence>
-struct AffineHomography {
- /// the linear part of the resulting affine transformation
- TooN::Matrix<2> A;
- /// the translation part of the resulting affine transformation
- TooN::Vector<2> t;
- AffineHomography() { A[0][0] = A[0][1] = A[1][0] = A[1][1] = t[0] = t[1] =
0; }
-
- void estimate(const std::vector<Correspondence>& matches) {
- TooN::Matrix<> Ad(matches.size(), 3);
- TooN::Vector<> bx(matches.size()), by(matches.size());
- assert(matches.size() >= 3);
- for (unsigned int i=0; i<matches.size(); i++) {
- const Correspondence& m = matches[i];
- Ad[i].template slice<0,2>() = m.a;
- Ad[i][2] = 1;
- bx[i] = m.b[0];
- by[i] = m.b[1];
- }
- //TooN::Matrix<3> ATA = Ad.T()*Ad;
- TooN::SVD<> svd(Ad);
- TooN::Vector<3> coefsX = svd.backsub(bx);
- TooN::Vector<3> coefsY = svd.backsub(by);
- A[0][0] = coefsX[0];
- A[0][1] = coefsX[1];
- t[0] = coefsX[2];
- A[1][0] = coefsY[0];
- A[1][1] = coefsY[1];
- t[1] = coefsY[2];
- }
-
- inline double getSqError(const Correspondence& m) const {
- TooN::Vector<2> disp = A*m.a + t - m.b;
- return disp*disp;
+ struct EssentialMatrix {
+ TooN::Matrix<3> E;
+ template <class It> bool estimate(It begin, It end) {
+ TooN::Matrix<9> M = zeros<9,9>();
+ for (It it=begin; it!= end; ++it) {
+ const TooN::Vector<2>& a = first_point(*it);
+ const TooN::Vector<2>& b = second_point(*it);
+ const double factor = 1.0/noise(*it);
+ 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};
+ for (int j=0; j<9; j++) {
+ for (int k=j; k<9; k++) {
+ M[j][k] += m[j]*m[k] * factor;
+ }
+ }
+ }
+ for (int j=0; j<9;j++)
+ for (int k=j; k<9; k++)
+ M[k][j] = M[j][k];
+ TooN::Matrix<4> M11 = M.template slice<0,0,4,4>();
+ TooN::Matrix<4,5> M12 = M.template slice<0,4,4,5>();
+ TooN::Matrix<5> M22 = M.template slice<4,4,5,5>();
+ TooN::Cholesky<5> chol(M22);
+ if (chol.get_rank() < 5) {
+ if (chol.get_rank() < 3)
+ return false;
+ TooN::Matrix<3> R;
+ // Translation is zero (choose t = [0,0,1])
+ essential_matrix::getProjectiveHomography(begin, end, R);
+ TooN::SO3::coerce(R);
+ E[0] = -R[1];
+ E[1] = R[0];
+ E[2][0] = E[2][1] = E[2][2] = 0;
+ return true;
+ }
+ TooN::Matrix<5,4> K = chol.inverse_times(M12.T());
+ TooN::Matrix<4> Q = M11 - M12*K;
+ TooN::SymEigen<4> eigen(Q);
+ TooN::Vector<4> e1 = eigen.get_evectors()[0];
+ TooN::Vector<5> e2 = -(K * e1);
+ E[0][0] = e1[0];
+ E[0][1] = e1[1];
+ E[1][0] = e1[2];
+ E[1][1] = e1[3];
+ E[0][2] = e2[0];
+ E[1][2] = e2[1];
+ E[2] = e2.template slice<2,3>();
+
+ TooN::SVD<3> svdE(E);
+ E = svdE.get_U()*TooN::diagmult(makeVector(1,1,0),svdE.get_VT());
+ return true;
+ }
+
+ template <class Match> inline bool isInlier(const Match& m, double r)
const {
+ TooN::Vector<3> line = E.template slice<0,0,3,2>()*first_point(m) +
E.T()[2];
+ double dot = line.template slice<0,2>() * second_point(m) + line[2];
+ return (dot*dot <= (line[0]*line[0] + line[1]*line[1]) * r*r *
noise(m));
+ }
+
+ /// Decompose the essential matrix into four possible SE3s
+ /// @param[in] begin beginning iterator for observations
+ /// @param[in] end ending iterator for observations
+ /// @param[out] group where to store the membership info (0,1,2, or 3)
for each observation
+ /// @return four pairs of the form {number of votes, SE3}
+ template <class It> std::vector<std::pair<size_t, TooN::SE3> >
decompose(It begin, It end, std::vector<int>& group)
+ {
+ static const double vals[9]={0,-1,0,1,0,0,0,0,1};
+ static const TooN::Matrix<3> Rz(vals);
+
+ const size_t N = std::distance(begin,end);
+ assert(group.size() >= N);
+
+ TooN::SVD<3> svdE(E);
+ TooN::Matrix<3> R1 = svdE.get_U()*Rz.T()*svdE.get_VT();
+ TooN::Matrix<3> R2 = svdE.get_U()*Rz*svdE.get_VT();
+ if (essential_matrix::determinant(R1) < 0) {
+ R1 = -1*R1;
+ R2 = -1*R2;
+ }
+ TooN::Vector<3> t1 = svdE.get_U().T()[2];
+ TooN::Vector<3> t2 = -t1;
+ TooN::Vector<2> epipole = project(t1); // which is the same as
project(t2)
+ std::vector<std::pair<size_t, TooN::SE3> >
result(4,std::make_pair(0,TooN::SE3()));
+ int i=0;
+ for (It it = begin; it!=end; ++it, ++i) {
+ int index = getValidPair(R1, R2, epipole, t1[2], *it);
+ result[index].first++;
+ group[i] = index;
+ }
+ result[0].second.get_rotation() = result[1].second.get_rotation() =
R1;
+ result[2].second.get_rotation() = result[3].second.get_rotation() =
R2;
+ result[0].second.get_translation() =
result[2].second.get_translation() = t1;
+ result[1].second.get_translation() =
result[3].second.get_translation() = t2;
+ return result;
}
-};
-/// RANSAC estimator to compute the fundamental matrix between two views based
on 2D-2D correspondences.
-/// The correspondences are coordinates in camera frame (after undistortion
etc.)
-/// The Correspondence datatype has to implement the following interface:
-/// @code
-/// struct Correspondence {
-/// TooN::Vector<2> a;
-/// TooN::Vector<2> b;
-/// };
-/// @endcode
-/// @ingroup ransac
-template <class Correspondence>
-struct FundamentalMatrix {
- TooN::Matrix<3> F;
- TooN::Matrix<3> T;
- TooN::Vector<9> diag;
-
- void setScale(double scale, bool translate=true) {
- T[0][0] = T[1][1] = 1/scale;
- T[0][1] = T[1][0] = T[2][0] = T[2][1] = 0;
- T[0][2] = translate ? -0.5 : 0;
- T[1][2] = translate ? -0.5 : 0;
- T[2][2] = 1;
- }
-
- FundamentalMatrix() {}
-
- void estimate(const std::vector<Correspondence>& matches) {
- TooN::Matrix<> A(std::max(matches.size(),(unsigned)9),9);
- for (unsigned int i=0; i<matches.size(); i++) {
- TooN::Vector<3> a,b;
- a.template slice<0,2>() = matches[i].a; a[2] = 1;
- b.template slice<0,2>() = matches[i].b; b[2] = 1;
- a = T*a;
- b = T*b;
- A[i][0] = a[0]*b[0];
- A[i][1] = a[0]*b[1];
- A[i][2] = a[0];
- A[i][3] = a[1]*b[0];
- A[i][4] = a[1]*b[1];
- A[i][5] = a[1];
- A[i][6] = b[0];
- A[i][7] = b[1];
- A[i][8] = -1;
- }
- for (unsigned int i=matches.size(); i<9; i++) {
- for (int j=0; j<9; j++)
- A[i][j] = 0;
- }
- TooN::SVD<> svdA(A);
- diag = svdA.get_diagonal();
- TooN::Vector<9> f = svdA.get_VT()[8];
- for (int i=0; i<9; i++)
- F[i/3][i%3] = f[i];
- TooN::SVD<3> svdF(F);
- TooN::Vector<3> fdiag = svdF.get_diagonal();
- TooN::Matrix<3> DVT = svdF.get_VT();
- fdiag[2] = 0;
- for (int i=0; i<3; i++)
- DVT[i] *= fdiag[i]/fdiag[0];
- F = T.T() * svdF.get_U()*DVT * T;
- }
-
- inline double getSqError(const Correspondence& m) const {
- TooN::Vector<3> u;
- u[0] = m.b[0];
- u[1] = m.b[1];
- u[2] = 1;
- TooN::Vector<3> line = F*u;
- double dot = line[0]*m.a[0] + line[1]*m.a[1] + line[2];
- double a2 = line[0]*line[0];
- double b2 = line[1]*line[1];
- return (dot*dot)*1/(a2+b2);
- }
-};
+ };
+} // close namespace essential_matrix
+
+ using essential_matrix::EssentialMatrix;
-/// RANSAC estimator to compute ???
-/// The Correspondence datatype has to implement the following interface:
+/// RANSAC estimator to compute an affine homography from a set of 2D-2D
correspondences
+/// The observations passed (via iterators) to the estimate method must allow:
/// @code
-/// struct Correspondence {
-/// TooN::Vector<2> a;
-/// TooN::Vector<2> b;
-/// };
+/// TooN::Vector<2> a = first_point(*it); // default value is "(*it).first"
+/// TooN::Vector<2> b = second_point(*it); // default value is "(*it).second"
+/// double R = noise(*it); // default value is "1.0"
/// @endcode
+/// The resulting transformation will map from a -> b.
/// @ingroup ransac
-template <class Correspondence>
-struct DifferentialEssentialMatrix {
- TooN::Matrix<3> s;
- TooN::Vector<3> v,w;
- TooN::Vector<9> diag;
-
- DifferentialEssentialMatrix() {}
-
- void estimate(const std::vector<Correspondence>& matches) {
- TooN::Matrix<> A(std::max(matches.size(),(unsigned)9),9);
- for (unsigned int i=0; i<matches.size(); i++) {
- TooN::Vector<2> q=matches[i].a, u=matches[i].b-matches[i].a;
- A[i][0] = -u[1];
- A[i][1] = u[0];
- A[i][2] = u[1]*q[0]-u[0]*q[1];
- A[i][3] = q[0]*q[0];
- A[i][4] = 2*q[0]*q[1];
- A[i][5] = 2*q[0];
- A[i][6] = q[1]*q[1];
- A[i][7] = 2*q[1];
- A[i][8] = 1;
- }
- for (unsigned int i=matches.size(); i<9; i++) {
- for (int j=0; j<9; j++)
- A[i][j] = 0;
- }
- TooN::SVD<> svdA(A);
- diag = svdA.get_diagonal();
- TooN::Vector<9> e = svdA.get_VT()[8];
- // renormalize ?
- e *= 1.0/sqrt(e.template slice<0,3>() * e.template slice<0,3>());
- // Velocity
- v = e.template slice<0,3>();
- // Symmetric TooN::Matrix s
- s[0][0]=e[3];
- s[0][1]=s[1][0]=e[4];
- s[0][2]=s[2][0]=e[5];
- s[1][1]=e[6];
- s[1][2]=s[2][1]=e[7];
- s[2][2]=e[8];
- TooN::SymEigen<3> eigen(s);
- // We want e1 >= e2 >= e3, e1 >= 0, e3 <= 0
- TooN::Vector<3> lambda = eigen.get_evalues();
- lambda[0] = std::max(0.0,lambda[0]);
- lambda[2] = std::min(0.0,lambda[2]);
- // Project to special symmetric TooN::Matrix
- TooN::Vector<3> sigma = (TooN::make_Vector, 2*lambda[0] + lambda[1] -
lambda[2],
- lambda[0] + 2*lambda[1] + lambda[2],
- -lambda[0] + lambda[1] + 2*lambda[2]);
- sigma /= 3;
- s = eigen.get_evectors().T() * diagmult(sigma,eigen.get_evectors());
- // recover w
- double lam = sigma[0] - sigma[2];
- double theta = acos(-sigma[1]/lam);
- TooN::Matrix<3> Ry1, Ry2, Rz;
- rotationY(theta/2 - M_PI/2, Ry1);
- rotationY(theta, Ry2);
- zero(Rz);
- Rz[0][1] = -1;
- Rz[1][0] = 1;
- Rz[2][2] = 1;
- TooN::Vector<3> diagLambda = (TooN::make_Vector,lam, lam,0);
- TooN::Vector<3> diag1 = (TooN::make_Vector,1,1,0);
- TooN::Matrix<3> V = eigen.get_evectors().T() * Ry1.T();
- TooN::Matrix<3> U = -1* V * Ry2;
- TooN::Vector<3> vhat[4] = { uncross(V*Rz*diagmult(diag1,V.T())),
- uncross(V*Rz.T()*diagmult(diag1,V.T())),
- uncross(U*Rz*diagmult(diag1,U.T())),
- uncross(U*Rz.T()*diagmult(diag1,U.T())) };
- double dots[4];
- for (int k=0; k<4; k++)
- dots[k] = vhat[k]*v;
- int maxdot = std::max_element(dots, dots+4)-dots;
- switch (maxdot) {
- case 0: w = uncross(U*Rz*diagmult(diagLambda, U.T())); break;
- case 1: w = uncross(U*Rz.T()*diagmult(diagLambda, U.T())); break;
- case 2: w = uncross(V*Rz*diagmult(diagLambda, V.T())); break;
- case 3: w = uncross(V*Rz.T()*diagmult(diagLambda, V.T())); break;
- }
- }
-
- inline double getSqError(const Correspondence& m) const {
- TooN::Vector<3> q = (TooN::make_Vector, m.a[0], m.a[1], 1);
- TooN::Vector<3> u = (TooN::make_Vector, m.b[0]-m.a[0], m.b[1]-m.a[1],
0);
- TooN::Vector<3> top = v^q;
- TooN::Vector<3> bottom = s*q;
- return u*top + q*bottom;
- }
-
- TooN::Vector<3> recoverAngularVelocity() {
- TooN::SymEigen<3> eigen(s);
- TooN::Vector<3> sigma = eigen.get_evalues();
- double lambda = sigma[0] - sigma[2];
- double theta = acos(-sigma[1]/lambda);
- TooN::Matrix<3> Ry1, Ry2, Rz;
- rotationY(theta/2 - M_PI/2, Ry1);
- rotationY(theta, Ry2);
- zero(Rz);
- Rz[0][1] = -1;
- Rz[1][0] = 1;
- Rz[2][2] = 1;
- TooN::Vector<3> diagLambda = (TooN::make_Vector, lambda, lambda,0);
- TooN::Vector<3> diag1 = (TooN::make_Vector,1,1,0);
- TooN::Matrix<3> V = eigen.get_evectors().T() * Ry1.T();
- TooN::Matrix<3> U = -1* V * Ry2;
- TooN::Vector<3> vhat[4] = { uncross(V*Rz*diagmult(diag1,V.T())),
- uncross(V*Rz.T()*diagmult(diag1,V.T())),
- uncross(U*Rz*diagmult(diag1,U.T())),
- uncross(U*Rz.T()*diagmult(diag1,U.T())) };
- double dots[4];
- for (int k=0; k<4; k++)
- dots[k] = vhat[k]*v;
- int maxdot = std::max_element(dots, dots+4)-dots;
- switch (maxdot) {
- case 0: w = uncross(U*Rz*diagmult(diagLambda, U.T())); break;
- case 1: w = uncross(U*Rz.T()*diagmult(diagLambda, U.T())); break;
- case 2: w = uncross(V*Rz*diagmult(diagLambda, V.T())); break;
- case 3: w = uncross(V*Rz.T()*diagmult(diagLambda, V.T())); break;
- }
- return w;
+
+struct AffineHomography {
+ /// the linear part of the affine transformation
+ TooN::Matrix<2> A;
+ /// the translation part of the affine transformation
+ TooN::Vector<2> t;
+
+ AffineHomography() : A(TooN::zeros<2,2>()), t(TooN::zeros<2>()) {}
+
+ template <class It> void estimate(It begin, It end) {
+ TooN::WLS<3> wls_x, wls_y;
+ wls_x.clear();
+ wls_y.clear();
+ size_t i=0;
+ for (It it = begin; it!= end; ++it, ++i) {
+ const TooN::Vector<2>& a = first_point(matches[i]);
+ const TooN::Vector<2>& b = second_point(matches[i]);
+ const double weight = 1.0 / noise(matches[i]);
+ wls_x.add_df(b[0], TooN::unproject(a), weight);
+ wls_y.add_df(b[1], TooN::unproject(a), weight);
+ }
+ wls_x.compute();
+ wls_y.compute();
+ TooN::Vector<3> Atx = wls_x.get_mu();
+ TooN::Vector<3> Aty = wls_y.get_mu();
+ A[0] = Atx.template slice<0,2>();
+ A[1] = Aty.template slice<0,2>();
+ t[0] = Atx[2];
+ t[1] = Aty[2];
+ }
+
+ template <class M> inline bool isInlier(const M& m, double r) const {
+ const TooN::Vector<2>& a = first_point(m);
+ const TooN::Vector<2>& b = second_point(m);
+ const TooN::Vector<2> disp = A*a + t - b;
+ return (disp*disp) <= r*r * noise(m);
}
};
+
} // namespace tag
#endif
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/tag ransac.h ransac_estimators.h,
Ethan Eade <=