
===================
 General
===================

Get load/save working again

Check to see if a loop closure exists before trying to calculate it.
 * This would allow you to fill in a few missed links using a lower thresh, 
   without trying to calculate all LCs
 * The function exists but is slow. Hash map? What hashing function?

Prevent multiple threads from trying to load images within 
precalc_slam_keypoints and seabed_slam LC search
 * Disk thrashing is probably reducing throughput
 * A simple mutex on image loads would probably work somewhat

Multi-thread hypothesis generation
 * Get some timing information on how long this takes on a big mission
 * Is this why there were large single-threaded pauses?
 * The most expensive part of this would be the recovery of the current vehicle
   columns, which cannot be multi-threaded

Segment loop closure areas, and mark if they have successfully been closed with
green/red circles.
 * Perhaps at the end, searching for loop-closures within failed regions could 
   be done with more aggressive parameters

Improved the way the option for the minimum number of features is handled
 * When no pose exists, the minimum value of number of features should be saved
   in the cache
 * Enable the min number of feature option to be changed in the GUI LC Search 
   dialog

Faster feature matching
 * Using matrix-matrix multiply
 * Using approximate nearest neighbour
   - Can the tree structure also be saved to cache?
   - It could be kept in the in-memory cache


===================
 SLAM Filter
===================

Try manually eliminating all nodes of degree 1 or 2 before asking AMD for an 
ordering

Faster observation of multiple relative poses
 * Multi-thread calculation of information vector and information matrix 
   updates?
 * Consider faster ways to modify the information vector and matrix
   - Assemble a new matrix once using all update matrices?

Consider what state recovery options are caused by GPS
 * I think there is a recovery of the complete state vector
 * Perhaps only do state recovery for a jump larger than a threshold?

Can the nasty USBL observation be made into a standard vehicle observation?

Get the logic for when to refactorise and perform state recovery out of the
EIF_SLAM class
 * Make the models have functions which specify the following options:
   - Recalculate factorisation
   - Recover suboptimal est
   - Recover suboptimal pose covariances
 * What information would these functions require?
   - For vehicle observations - Change in current vehicle pose
   - For relative pose observations - This is probably known from whether 
     it is a cross-track loop closure or odometry constraint.

Implement recovering only the last n blocks of the vehicle columns of the 
covariance matrix
 * Circle test finds a limited set of poses without using the correlations
 * This limited set of poses will define how many blocks need to be recovered
 * Will poses in the previous leg be ordered towards the end of the state
   vector?

Augment stereo poses instead of vehicle poses
 * Lots of time is spent converting the vehicle poses to stereo poses
 * The SLAM classes EKF_SLAM and EIF_SLAM should take in an initialisation 
   functor which could allow arbitrary state augmentation.
 * Can this be done in the information form?

  

===================
 GUI
===================

Enable triggering of an automatic map optimisation from the GUI
 * Allow the user to set min, max distances and distance increment

Mouse motion for 3D view

Get rid of lib3dObjects and lib3dRender.

Add a save menu item to seabed_slam, allowing exporting of a preliminary result 
 * Specify a directory
 * Append a note to the log files saying it was not a final result


===================
 Caches
===================

Save triangulated feature positions in feature cache. 
 * Currently this is recalculated every time for every loop-closure hypothesis.
 * When running ningaloo 06 with map optimisation, less than 0.2 seconds out
   of 250 seconds is used in triangulation, so this is not important. It may
   even be advantageous to triangulate the small number of matched features on
   demand instead of triangulating all features and loading the 3d position
   estimates and covariances.

Get rid of the libsnapper Feature framework.
 * The feature cache should return:
     - vector<CvPoint2d32f> left_coords
     - vector<CvPoint2d32f> right_coords
   And perhaps (if triangulated positions are saved):
     - vector<Vector> est (estimated 3d position relative to camera)
     - vector<Matrix> cov (covariance of estimated 3d position)
 * This will requried cleaning up the triangulation and motion estimation
   functions in libsnapper to remove the libsnapper::Feature rubbish.



===================
 Clean-up
===================

* DeltaT file handling code has been cut-and-paste into:
     seabed_slam.cpp
     copy_deltaT.cpp
     seabed_deltaT.cpp
     seabed_pipe.cpp
     process_deltaT.cpp
  Wrap this stuff up in a class where you can just call get_next() like
  Raw_File in auv_data_tools


===================
 Tuning
===================

* Get a better value for the variable 'max_fov' which is the approximate 
  circular field of view of the stereo-rig used in the loop-closure hypothesis
  test in seabed_slam. Do this by going through a whole mission, and find the 
  maximum angle from the principal axis for a feature observed by the stereo rig
  in both images. 

* Currently the process noise variances are way too high. They are probably 
  too high to compensate for things like heading biases etc. The current 
  configuration works ok when the DVL has bottom lock, however the velocity
  then position variances grow extremely quickly when no velocity observations
  are present. Currently we fake velocity observations to control the
  uncertainty growth when bottom lock is lost, however this is an ugly 
  workaround. Basically, the filter is poorly tuned. As a first pass, instead 
  of injecting uncertainty into the system with inflated process noise, the
  variance of the DVL velocity observations should be increased. The same
  uncertainty should be added to the system to compensate for heading biases
  etc. however the filter shouldn't go crazy when no velocity observations are
  available. 
