TrackerData.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 #ifndef __TRACKERDATA_H
00004 #define __TRACKERDATA_H
00005 
00006 #include "PatchFinder.h"
00007 #include "ATANCamera.h"
00008 
00009 // This class contains all the intermediate results associated with
00010 // a map-point that the tracker keeps up-to-date. TrackerData
00011 // basically handles all the tracker's point-projection jobs,
00012 // and also contains the PatchFinder which does the image search.
00013 // It's very code-heavy for an h-file (it's a bunch of methods really)
00014 // but it's only included from Tracker.cc!
00015 
00016 struct TrackerData
00017 {
00018 TrackerData(MapPoint *pMapPoint) 
00019 : Point(*pMapPoint)
00020   {};
00021   
00022   MapPoint &Point;
00023   PatchFinder Finder;
00024   
00025   // Projection itermediates:
00026   Vector<3> v3Cam;        // Coords in current cam frame
00027   Vector<2> v2ImPlane;    // Coords in current cam z=1 plane
00028   Vector<2> v2Image;      // Pixel coords in LEVEL0
00029   Matrix<2> m2CamDerivs;  // Camera projection derivs
00030   bool bInImage;        
00031   bool bPotentiallyVisible;
00032   
00033   int nSearchLevel;
00034   bool bSearched;
00035   bool bFound;
00036   bool bDidSubPix;
00037   Vector<2> v2Found;      // Pixel coords of found patch (L0)
00038   double dSqrtInvNoise;   // Only depends on search level..
00039   
00040   
00041   // Stuff for pose update:
00042   Vector<2> v2Error_CovScaled;
00043   Matrix<2,6> m26Jacobian;   // Jacobian wrt camera position
00044   
00045   // Project point into image given certain pose and camera.
00046   // This can bail out at several stages if the point
00047   // will not be properly in the image.
00048   inline void Project(const SE3<> &se3CFromW, ATANCamera &Cam)
00049   {
00050     bInImage = bPotentiallyVisible = false;
00051     v3Cam = se3CFromW * Point.v3WorldPos;
00052     if(v3Cam[2] < 0.001)
00053       return;
00054     v2ImPlane = project(v3Cam);
00055     if(v2ImPlane*v2ImPlane > Cam.LargestRadiusInImage() * Cam.LargestRadiusInImage())
00056       return;
00057     v2Image = Cam.Project(v2ImPlane);
00058     if(Cam.Invalid())
00059       return;
00060     
00061     if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
00062       return;
00063     bInImage = true;
00064   }
00065   
00066   // Get the projection derivatives (depend only on the camera.)
00067   // This is called Unsafe because it depends on the camera caching 
00068   // results from the previous projection:
00069   // Only do this right after the same point has been projected!
00070   inline void GetDerivsUnsafe(ATANCamera &Cam) 
00071   {
00072     m2CamDerivs = Cam.GetProjectionDerivs();
00073   }
00074   
00075   // Does projection and gets camera derivs all in one.
00076   inline void ProjectAndDerivs(SE3<> &se3, ATANCamera &Cam)
00077   {
00078     Project(se3, Cam);
00079     if(bFound)
00080       GetDerivsUnsafe(Cam);
00081   }
00082   
00083   // Jacobian of projection W.R.T. the camera position
00084   // I.e. if  p_cam = SE3Old * p_world, 
00085   //         SE3New = SE3Motion * SE3Old
00086   inline void CalcJacobian()
00087   {
00088     double dOneOverCameraZ = 1.0 / v3Cam[2];
00089     for(int m=0; m<6; m++)
00090       {
00091         const Vector<4> v4Motion = SE3<>::generator_field(m, unproject(v3Cam));
00092         Vector<2> v2CamFrameMotion;
00093         v2CamFrameMotion[0] = (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00094         v2CamFrameMotion[1] = (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00095         m26Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion;
00096       };
00097   }
00098   
00099   // Sometimes in tracker instead of reprojecting, just update the error linearly!
00100   inline void LinearUpdate(const Vector<6> &v6)
00101   {
00102     v2Image += m26Jacobian * v6;
00103   }
00104   
00105   // This static member is filled in by the tracker and allows in-image checks in this class above.
00106   static CVD::ImageRef irImageSize;
00107 };
00108 
00109 
00110 
00111 
00112 
00113 
00114 #endif
00115 
00116 
00117 
00118 


uga_tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:30:11