Class VisOdomMonoPlaneInfinity<T extends ImageBase<T>>


public class VisOdomMonoPlaneInfinity<T extends ImageBase<T>> extends Object
Estimates camera ego-motion by assuming the camera is viewing a flat plane and that off plane points are at infinity. These assumptions are often true for robots traveling over flat terrain outdoors. There are no assumptions for orientation of the camera, e.g. it could be upside down and at an angle relative to the plane.

A point feature tracker is used to extract sparse optical flow from the image sequence. Bad tracks are removed first through the use of geometric constraints and then through robust estimation techniques which search for maximum inlier sets. Before a track is discarded it has to be not used for X frames in a row, where X is user specified. Determination of inliers is done through a user specified threshold in pixels.

Points on the plane are used to estimate translation and rotation, while points at infinity are just used to estimate rotation. The two rotation estimates are merged together using a weighted vector average. Motion estimation does not require any off plane points at infinity, but does require points on the plane. All motion estimation is done internally in 2D, but can be converted into 3D.

It is possible to turn on a more rigid test for off plane points at infinity. If 'strictFar' is set to true then any points which contains motions that could not be generated by a rotation about the y-axis are discarded. This appears to be too strict and discard an excessive number of points when vehicle pitch up and down. Pitching motions will often not affect motion estimates from projected vectors on to the plane.
  • Constructor Details

    • VisOdomMonoPlaneInfinity

      public VisOdomMonoPlaneInfinity(int thresholdAdd, int thresholdRetire, double thresholdPixelError, ModelMatcher<Se2_F64,PlanePtPixel> planeMotion, PointTracker<T> tracker)
      Configures motion estimation.
      thresholdAdd - New points are spawned when the number of on plane inliers drops below this value.
      thresholdRetire - Tracks are dropped when they are not contained in the inlier set for this many frames in a row. Try 2
      thresholdPixelError - Threshold used to determine inliers. Try 1.5
      planeMotion - Motion estimator for points on plane
      tracker - Image feature tracker
  • Method Details

    • setIntrinsic

      public void setIntrinsic(CameraPinholeBrown intrinsic)
      Camera the camera's intrinsic parameters. Can be called at any time.
      intrinsic - Intrinsic camera parameters
    • setExtrinsic

      public void setExtrinsic(Se3_F64 planeToCamera)
      Camera the camera's extrinsic parameters. Can be called at any time.
      planeToCamera - Transform from the plane to camera.
    • reset

      public void reset()
      Resets the algorithm into its initial state
    • process

      public boolean process(T image)
      Estimates the motion which the camera undergoes relative to the first frame processed.
      image - Most recent camera image.
      true if motion was estimated or false if a fault occurred. Should reset after a fault.
    • isRotationFromAxisY

      protected boolean isRotationFromAxisY(PointTrack t, Vector3D_F64 pointing)
      Checks for motion which can't be caused by rotations along the y-axis alone. This is done by adjusting the pointing vector in the plane reference frame such that it has the same y component as when the track was spawned and that the x-z components are normalized to one, to ensure consistent units. That pointing vector is then projected back into the image and a pixel difference computed.
      pointing - Pointing vector of observation in plane reference frame
    • getFrameID

      public long getFrameID()
      Number of frames processed
      Number of frames processed
    • getCurrToWorld2D

      public Se2_F64 getCurrToWorld2D()
      Returns the 2D motion estimate
      Motion estimate from current frame into world frame in 2D
    • getWorldToCurr3D

      public Se3_F64 getWorldToCurr3D()
      Converts 2D motion estimate into a 3D motion estimate
      from world to current frame.
    • maximizeCountInSpread

      public static double maximizeCountInSpread(double[] data, int size, double maxSpread)
      Finds the value which has the largest number of points above and below it within the specified spread
      data - Input data. Is modified by sort
      size - number of elements in data
      maxSpread - the spread it's going after
      best value
    • getTracker

      public PointTracker<T> getTracker()
    • isStrictFar

      public boolean isStrictFar()
    • setStrictFar

      public void setStrictFar(boolean strictFar)