TooN Algorithm Library - tag: fourpointpose.h Source File

00001 #ifndef TAG_FOURPOINTPOSE_H_
00002 #define TAG_FOURPOINTPOSE_H_
00003 
00004 #include <vector>
00005 
00006 #include <TooN/TooN.h>
00007 #include <TooN/se3.h>
00008 
00009 namespace tag {
00010 
00014 
00030 TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError = 0.14 );
00031 
00046 TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > & points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError = 0.14 );
00047 
00059 template <int ImagePlaneZ = 1>
00060 struct Point4SE3Estimation {
00061     TooN::SE3 T;
00062     bool valid;
00063     double angularError;
00064 
00065     inline Point4SE3Estimation(double ang = 0.14) : valid(false), angularError(ang) {  }
00066 
00067     template<class It> inline bool estimate(It begin, It end) {
00068         assert(end - begin >= 4);
00069         valid = true;
00070 
00071         std::vector<TooN::Vector<3> > points(4);
00072         std::vector<TooN::Vector<3> > pixels(4);
00073         unsigned int i = 0;
00074         for(It match = begin; match != end; match++, i++){
00075             pixels[i] = unproject(match->pixel);
00076             pixels[i][2] *= ImagePlaneZ;
00077             points[i] = match->position;
00078         }
00079         T = fourPointPoseFromCamera( points, pixels, valid, angularError );
00080         return valid;
00081     }
00082 
00083     template<class Obs, class Tol> inline bool isInlier( const Obs& obs, const Tol& tolerance ) const {
00084         return getSqError(obs) < tolerance;
00085     }
00086 
00087     template<class Obs> inline double getSqError(const Obs & obs) const {
00088         if(valid){
00089             TooN::Vector<3> pos = T * obs.position;
00090             TooN::Vector<2> diff = project(pos) - obs.pixel / ImagePlaneZ;
00091             double disp = diff*diff;
00092             return disp;
00093         }
00094         return 100;
00095     }
00096 
00097     template <class Obs> inline double score(const Obs & obs) const {
00098         if(valid){
00099             TooN::Vector<3> pos = T * obs.position;
00100             TooN::Vector<2> diff = project(pos) - obs.pixel / ImagePlaneZ;
00101             double disp = diff*diff;
00102             return disp;
00103         }
00104         return 100;
00105     }
00106 
00107 
00108 };
00109 
00110 } // namespace tag
00111 
00112 #endif /*FOURPOINTPOSE*/