Mystery Nan appears from Sophus crashing the program

[DavidPetkovsek]

This seems to happen midway through running. Happens more frequently on high acceleration/velocity movement.

The following are all related:

• Sophus exp failed! UZ-SLAMLab/ORB_SLAM3#439
• SO3::exp failed UZ-SLAMLab/ORB_SLAM3#451
• SO3::exp failed! omega: -nan -nan -nan, real: -nan, img: -nan UZ-SLAMLab/ORB_SLAM3#753
• Solving Sophus -nan error UZ-SLAMLab/ORB_SLAM3#608

UZ-SLAMLab/ORB_SLAM3#608 seems to have a number of potential solutions.

[DavidPetkovsek]

Solved

This issue has been resolved in the Clean slam branch. This will be closed now since the branch is a larger update and will definitely be merged to master.

Other Nan issues that people reported either only exist in their local copy or have been resolved by the other changes made in our fork.

Solution commit: 8b22f88

Solution PR: #16

Problem Summary:

Sim3Solver is a custom implementation of a RANSAC algorithm. Inside of the iterate step of RANSAC, after selecting the points at random, ComputeSim3 figures out the ideal model for the selected points so then later on the inliers can be computed.

The error occurs inside ComputeSim3. It implements the Horn 1987 algorithm which is a closed form mathematical solution to solving overdetermined systems of equations that minimizes the sum of squared error or residuals.

The vec local variable (which represents the imaginary portion of a quaternion) becomes the 0 vector. Which is then used as the denominator in a division and then a check done by sophus catches the resulting Nan and throws an exception crashing the program.

The 0 vector occurs because the randomly selected points may form a mathematical degenerate edge-case. Meaning the points selected at random are either identical or coplanar, or colinear, etc. Making there be no single solution to the system of equations.

Solution Summary:

Since the CompueSim3 is only used in the iteration step inside the while loop we know that it doesn't make a difference if we throw out the one randomly selected points iteration. And since iterate has been built to return a boolean for success it will handle the case where there is no true solution to the RANSAC data.

To achieve this, ComputeSim3 was modified to return a bool. Return true at the end of the function, and return false if vec.norm() is evaluated to exactly 0 before any division or use of vec.norm is used. Then in the two uses in iterate check if the return value is false and if it is continue to the next iteration.

Other Notes

One of the issue branches was close but their implemented solution does not handle the issue properly at all!