[Toon-members] tag/tag ransac_estimators.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      07/08/17 16:06:28

Modified files:
        tag            : ransac_estimators.h 

Log message:
        homography estimator and some optimisations and fixes

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/ransac_estimators.h?cvsroot=toon&r1=1.5&r2=1.6

Patches:
Index: ransac_estimators.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/ransac_estimators.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- ransac_estimators.h 12 Jun 2007 20:05:08 -0000      1.5
+++ ransac_estimators.h 17 Aug 2007 16:06:28 -0000      1.6
@@ -13,6 +13,7 @@
 #include <TooN/SVD.h>
 #include <TooN/SymEigen.h>
 #include <TooN/se3.h>
+#include <TooN/wls_cholesky.h>
 
 namespace tag {
 
@@ -28,10 +29,10 @@
     {
        assert(std::distance(begin,end) >= 4);
 
-       TooN::WLS<8> wls;
+       TooN::WLSCholesky<8> wls;
        for (It it=begin; it!=end; it++) {
            const TooN::Vector<2>& a = first_point(*it);
-           const TooN::Vector<2>& b = first_point(*it);
+           const TooN::Vector<2>& b = second_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], TooN::Vector<8>(rows[0]));
@@ -149,6 +150,13 @@
            return (dot*dot <= (line[0]*line[0] + line[1]*line[1]) * r*r * 
noise(m));
        }
        
+    template <class Match> inline double score(const Match& m) 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]);
+    }
+
+
        /// Decompose the essential matrix into four possible SE3s
        /// @param[in] begin beginning iterator for observations
        /// @param[in] end ending iterator for observations
@@ -191,7 +199,7 @@
 
  using essential_matrix::EssentialMatrix;
 
-/// RANSAC estimator to compute an affine homography from a set of 2D-2D 
correspondences
+/// RANSAC estimator to compute an homography from a set of 2D-2D 
correspondences
 /// The observations passed (via iterators) to the estimate method must allow:
 /// @code
 /// TooN::Vector<2> a = first_point(*it); // default value is "(*it).first"
@@ -200,7 +208,37 @@
 /// @endcode
 /// The resulting transformation will map from a -> b.
 /// @ingroup ransac
+struct Homography {
+    /// homography
+    TooN::Matrix<3> H;
+
+    Homography() { TooN::Identity(H); }
+
+    template <class It> void estimate(It begin, It end) {
+        essential_matrix::getProjectiveHomography(begin, end, H);
+    }
+
+    template <class M> inline double score(const M& m) const {
+        TooN::Vector<3> a = TooN::unproject(first_point(m));
+        const TooN::Vector<2> & b = second_point(m);
+        const TooN::Vector<2> disp = TooN::project(H * a)  - b;
+        return (disp*disp);
+    }
 
+    template <class M> inline bool isInlier(const M& m, double r) const {
+        return this->score(m) <= r*r * noise(m);
+    }
+};
+
+/// 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
+/// 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
 struct AffineHomography {
     /// the linear part of the affine transformation
     TooN::Matrix<2> A;
@@ -210,7 +248,7 @@
     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;
+       TooN::WLSCholesky<3> wls_x, wls_y;
        wls_x.clear();
        wls_y.clear();
         for (It it = begin; it!= end; ++it) {