TrackerData.h

// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited
#ifndef __TRACKERDATA_H
#define __TRACKERDATA_H

#include "PatchFinder.h"
#include "ATANCamera.h"

// This class contains all the intermediate results associated with
// a map-point that the tracker keeps up-to-date. TrackerData
// basically handles all the tracker's point-projection jobs,
// and also contains the PatchFinder which does the image search.
// It's very code-heavy for an h-file (it's a bunch of methods really)
// but it's only included from Tracker.cc!

namespace PTAMM {

struct TrackerData
{
TrackerData(MapPoint *pMapPoint) 
: Point(*pMapPoint)
  {};
  
  MapPoint &Point;
  PatchFinder Finder;
  
  // Projection itermediates:
  Vector<3> v3Cam;        // Coords in current cam frame
  Vector<2> v2ImPlane;    // Coords in current cam z=1 plane
  Vector<2> v2Image;      // Pixel coords in LEVEL0
  Matrix<2> m2CamDerivs;  // Camera projection derivs
  bool bInImage;        
  bool bPotentiallyVisible;
  
  int nSearchLevel;
  bool bSearched;
  bool bFound;
  bool bDidSubPix;
  Vector<2> v2Found;      // Pixel coords of found patch (L0)
  double dSqrtInvNoise;   // Only depends on search level..
  
  
  // Stuff for pose update:
  Vector<2> v2Error_CovScaled;
  Matrix<2,6> m26Jacobian;   // Jacobian wrt camera position
  
  // Project point into image given certain pose and camera.
  // This can bail out at several stages if the point
  // will not be properly in the image.
  inline void Project(const SE3<> &se3CFromW, ATANCamera &Cam)
  {
    bInImage = bPotentiallyVisible = false;
    v3Cam = se3CFromW * Point.v3WorldPos;
    if(v3Cam[2] < 0.001)
      return;
    v2ImPlane = project(v3Cam);
    if( (v2ImPlane * v2ImPlane) > (Cam.LargestRadiusInImage() * Cam.LargestRadiusInImage()) )
      return;
    v2Image = Cam.Project(v2ImPlane);
    if(Cam.Invalid())
      return;
    
    if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
      return;
    bInImage = true;
  }
  
  // Get the projection derivatives (depend only on the camera.)
  // This is called Unsafe because it depends on the camera caching 
  // results from the previous projection:
  // Only do this right after the same point has been projected!
  inline void GetDerivsUnsafe(ATANCamera &Cam) 
  {
    m2CamDerivs = Cam.GetProjectionDerivs();
  }
  
  // Does projection and gets camera derivs all in one.
  inline void ProjectAndDerivs(SE3<> &se3, ATANCamera &Cam)
  {
    Project(se3, Cam);
    if(bFound)
      GetDerivsUnsafe(Cam);
  }
  
  // Jacobian of projection W.R.T. the camera position
  // I.e. if  p_cam = SE3Old * p_world, 
  //         SE3New = SE3Motion * SE3Old
  inline void CalcJacobian()
  {
    double dOneOverCameraZ = 1.0 / v3Cam[2];
    for(int m=0; m<6; m++)
      {
	const Vector<4> v4Motion = SE3<>::generator_field(m, unproject(v3Cam));
	Vector<2> v2CamFrameMotion;
	v2CamFrameMotion[0] = (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
	v2CamFrameMotion[1] = (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
	m26Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion;
      };
  }
  
  // Sometimes in tracker instead of reprojecting, just update the error linearly!
  inline void LinearUpdate(const Vector<6> &v6)
  {
    v2Image += m26Jacobian * v6;
  }
  
  // This static member is filled in by the tracker and allows in-image checks in this class above.
  static CVD::ImageRef irImageSize;
};


}

#endif