Stereo optimisation crash when using stereo factors

[nashj]

This has been an ongoing issue, characterised by an IndeterminantLinearSystem error during optimisation when stereo factors are enabled. Here I detail my findings, suspicions, and ideas to fix it.

Findings:

• The crash is detected usually while working near a stereo landmark variable (or if a pose, then this is likely due to a stereo landmark variable), which may have been tracked for a number of frames.
• The landmark causing the crash is reported to be behind the camera viewpoints from which the feature was seen. It is not completely clear if the optimisation fails because the value has already been updated to be behind the camera views, or if the first time it is optimised behind the camera views the crash occurs. I suspect both occur in practice.
• It is suspected, but not confirmed, that the feature is an outlier. It is very difficult to confirm this because backtracing the 2D features in the relevant frames from the 3D landmark causing the crash, e.g. for visual inspection, is very difficult.
• The error of the factors linked to the crash are presumably relatively high, and equal. The errors are equal because when they are behind the camera the error is a constant (see later notes).
• Documentation on this error: https://gtsam.org/doxygen/a03816.html
• Perhaps a related issue: https://groups.google.com/g/gtsam-users/c/i9RxopA2C0Y?fbclid=IwAR0lrMUNUhsBwq7BGTMIwflA2fex2dwsSoFPnjGR6VkiFv6VqZ697f-3ZfA
  • This explains how when a point moves behind the camera it can be ignored or trigger a Cheirality error. The code where this happens is in the evaluateError function in https://github.com/borglab/gtsam/blob/develop/gtsam/slam/StereoFactor.h. If the cheirality exception occurs, the error is a constant scaled by the focal length Vector3::Constant(2.0 * K_->fx()).
• We notice that the backprojection of the measurements from the optimised camera poses, poses which are approximately correct visually, are very different from each other, further proof that the points are outliers.

Theories and possible ideas to investigate/implement:

1. Caused by too many (or too problematic) outliers.

• Stereo RANSAC. Currently there is a motion prediction being used as a model (transform) to filter out features that have poor reprojection, however unlike RANSAC this transform is not updated to find a more accurate inlier set. Could one also use an form of RANSAC that is informed by a motion prior?
• Look at reimplementing the stereo pipeline to match others.

2. Stereo factors are incompatible with LiDAR or IMU factors because calibration (timing offset or transform) is not accurate enough.

• How accurate are the sensor timestamps?
• Can we refactor SERPENT to run in just a stereo-only mode?

3. Some internal bug, such as match ids not being managed properly, confusing the backend.

• Colour the feature point cloud by the id, and look for strange colour changes in rviz

4. Perhaps this is unavoidable with the GTSAM implementation of stereo factors.

• Catch the error, then check the error of all factors via evaluateError and remove the high error factors (or the factors associated with features which get projected behind the camera).
• After each optimisation, check for high-error/bad-projection factors/features, and remove them.
• (Note that removing factors via a call to iSAM2::update is quite difficult as it requires careful book-keeping of factor indices, and it is not clear from the documentation what these indices are exactly).