[Toon-members] tag/tag ransac.h fourpointpose.h kalmanfilter.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/06/13 17:19:27

Modified files:
        tag            : ransac.h fourpointpose.h kalmanfilter.h 

Log message:
        some fixes after ransac update:
        namespace qualifiers in ransac.h and kalmanfilter.h
        interface update for fourpointpose.h ransac estimator

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/ransac.h?cvsroot=toon&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/tag/tag/fourpointpose.h?cvsroot=toon&r1=1.3&r2=1.4
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.5&r2=1.6

Patches:
Index: ransac.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/ransac.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- ransac.h    13 Jun 2006 02:00:21 -0000      1.5
+++ ransac.h    13 Jun 2006 17:19:27 -0000      1.6
@@ -64,7 +64,7 @@
     std::vector<bool> thisInlier(observations.size());
     size_t bestScore = 0;
     std::vector<size_t> sample_index(sample_size);
-    vector<Obs> sample(sample_size);
+    std::vector<Obs> sample(sample_size);
     while (N--) {
        randomTuple(sample_index, observations.size());
        for (int i=0;i<sample_size; i++)
@@ -161,7 +161,7 @@
            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> >());
+           std::nth_element(score.begin(), score.end(), score.begin()+cutoff, 
std::greater<std::pair<int,size_t> >());
            score.resize(cutoff);
            m = end;
        }
@@ -235,7 +235,7 @@
            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> >());
+           std::nth_element(score.begin(), score.end(), score.begin()+cutoff, 
std::greater<std::pair<int,size_t> >());
            score.resize(cutoff);
            m = end;
        }

Index: fourpointpose.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/fourpointpose.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- fourpointpose.h     4 Jun 2006 12:09:19 -0000       1.3
+++ fourpointpose.h     13 Jun 2006 17:19:27 -0000      1.4
@@ -52,31 +52,37 @@
 /// @endcode
 /// @ingroup fourpointpose
 /// @ingroup ransac
-template <class Correspondence, int ImagePlaneZ = 1>
+template <int ImagePlaneZ = 1>
 struct Point4SE3Estimation {
     TooN::SE3 T;
     bool valid;
 
-    Point4SE3Estimation() : valid(false) {  }
+    inline Point4SE3Estimation() : valid(false) {  }
 
-    void estimate(const std::vector<Correspondence> & matches) {
-        assert(matches.size() >= 4);
+    template<class It> inline bool estimate(It begin, It end) {
+        assert(end - begin >= 4);
         valid = true;
 
         std::vector<TooN::Vector<3> > points(4);
         std::vector<TooN::Vector<3> > pixels(4);
-        for(unsigned int i = 0; i < 4; i ++){
-            pixels[i] = unproject(matches[i].pixel);
+        unsigned int i = 0;
+        for(It match = begin; match != end; match++, i++){
+            pixels[i] = unproject(match->pixel);
             pixels[i][2] *= ImagePlaneZ;
-            points[i] = matches[i].position;
+            points[i] = match->position;
         }
-        T = fourPointPose( points, pixels, valid );
+        T = fourPointPoseFromCamera( points, pixels, valid );
+        return valid;
     }
 
-    double getSqError(const Correspondence& m) const {
+    template<class Obs, class Tol> inline bool isInlier( const Obs& obs, const 
Tol& tolerance ) const {
+        return getSqError(obs) < tolerance;
+    }
+
+    template<class Obs> inline double getSqError(const Obs & obs) const {
         if(valid){
-            TooN::Vector<3> pos = T * m.position;
-            TooN::Vector<2> diff = project(pos) - m.pixel / ImagePlaneZ;
+            TooN::Vector<3> pos = T * obs.position;
+            TooN::Vector<2> diff = project(pos) - obs.pixel / ImagePlaneZ;
             double disp = diff*diff;
             return disp;
         }

Index: kalmanfilter.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/kalmanfilter.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- kalmanfilter.h      5 Jun 2006 18:04:57 -0000       1.5
+++ kalmanfilter.h      13 Jun 2006 17:19:27 -0000      1.6
@@ -106,7 +106,7 @@
         TooN::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
         model.updateFromMeasurement( state, stateInnovation );
         state.covariance = (identity - K * H) * state.covariance;
-        Symmetrize( state.covariance );
+        TooN::Symmetrize( state.covariance );
     }
 
     /// identity matrix of the right size, used in the measurement equations