TrackerData.h

Go to the documentation of this file.

// -*- 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!

struct TrackerData
{
  TrackerData(MapPoint::Ptr pMapPoint)
  : Point(pMapPoint)
  {};

  MapPoint::Ptr 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