vSLAM dev approach

• What?
  • develop a vSLAM pipeline from scratch
    • Problem:
      • gtsam::IndeterminantLinearSystemException
        • https://gtsam.org/doxygen/4.0.0/a03203.html
  • Why vSLAM from scratch?
    • Being able to model optimization problems is a niche but very valuable skill in robotics
    • We can focus on building a very lean MVD (minimal viable demo) from the grounds up which helps with being able to debug since we understand the entire code base
    • Using what we consider to be the best in class off-the-shelf VIO/SLAM systems are not as easy as one would think! Virtana’s engineers have spent months fighting with KimeraVIO.
  • How is this project novel?
    • Getting a vSLAM implementation running on the IMX chip has not been done before as far as we know.
• Constraints / Guiding Principles
  • Try the quickest routes first
  • As simple an implementation as possible
  • Must use iSAM for performance
• Ideation

  • Approaches

    • ✅ Pattern match against examples
    • Add in between factors for camera poses
      • no need for IMU, just use PnP
    • Simplify the input
      • try 1 tag on a single flat wall, from many, distinct viewing angles, at many different depth distances
        • then increase the number of tags
    • Try the MATLAB (or other, like numpy / scipy) debug route - use dense?
      • https://gtsam.org/doxygen/4.0.0/a03203.html

    Problem	Jacobian Sign	Hessian Sign
    Underconstrained System	Near-zero rows in Jacobian	Some eigenvalues near zero
    Ill-conditioned System	Large differences in row magnitudes	Very large condition number
    Non-Positive-Semidefinite Hessian	–	Negative eigenvalues (unstable system)

    • look at the jacobian

      • check the rank of jacobian ≥ the number of unknowns (cam poses + landmark poses)

    • look at the hessian

      • check the rank of hessian ≥ the number of unknowns
      • check for ill conditioning (EVs span several orders of magnitude)
        • Ill-conditioning refers to the sensitivity of the solution of a system to small changes or errors in the input data.
        • This is typically quantified by the condition number of the matrix, which is the ratio of the largest eigenvalue to the smallest eigenvalue.
        • If the condition number is very high, it means that even though all eigenvalues are positive, one or more of them are extremely small compared to the largest one.
      • we look at the eigenvalues of hessian to check for PSD:
        • If all eigenvalues are positive: The function is convex (curves upward in every direction).
        • If at least one eigenvalue is negative: The function curves downward in at least one direction—meaning it is concave in that direction.
      • Curvature of the Error Surface:
      • • *Large Eigenvalues:**If an eigenvalue is large, it indicates that the error function has a steep curvature in the direction of its corresponding eigenvector. In practical terms, the system is very sensitive to changes in that direction—the error increases rapidly.
        • *Small Eigenvalues:**A small eigenvalue suggests that the error function is relatively flat along the associated direction. This means that the variable combination represented by the corresponding eigenvector is not very well constrained by the measurements. In extreme cases, a near-zero eigenvalue points to an almost unconstrained (or ambiguous) direction.
        • *Negative Eigenvalues:**If any eigenvalue is negative, it indicates that the cost function curves downward in that direction (i.e., it is concave rather than convex). This can be a sign of issues such as poor linearization or that the problem is not well posed (e.g., due to outliers or an insufficient number of constraints).

      

      • Condition Number:
        • The condition number of a matrix is the ratio of the largest to the smallest eigenvalue (for a symmetric positive definite matrix). A very high condition number means that there is a huge disparity between the “stiff” directions (large eigenvalues) and the “soft” or nearly free directions (small eigenvalues). This disparity is a hallmark of an ill-conditioned system, where small errors in the data can lead to large variations in the solution.

    • Exhaust suggestions from the community

      • https://github.com/borglab/gtsam/issues/1179
      • https://groups.google.com/g/gtsam-users/c/cna5Up8aa6w?pli=1
        • solve the first 100 frames using batch optimization, then switch to isam2 afterwards I no longer get the error. I accomplished this by using gauss newton for isam2 and levenberg marguardt for the batch optimization. I also didn't need to turn on QR factorization.
        • I also tried adding a prior (like you mentioned) to get iSAM2 to work, and indeed that also resolves the issue without needing to do any batch optimization.
        • https://groups.google.com/g/gtsam-users/c/yO3cpAl5CAg/m/O5Pc7Z50HAAJ

    • Understand the concepts deeper

      • Factor graphs
      • Resources
        • https://github.com/changh95/visual-slam-roadmap
        • https://gtsam-jlblanco-docs.readthedocs.io/en/latest/iSAM.html
        • https://github.com/SLAM-Handbook-contributors/slam-handbook-public-release/blob/main/main.pdf
        • https://gtsam.org/tutorials/intro.html
        • https://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf
        • https://www.roboticsbook.org/intro.html
          • https://www.roboticsbook.org/S34_vacuum_perception.html#factor-graphs
          • https://www.roboticsbook.org/S44_logistics_perception.html#factor-graphs-and-least-squares
          • https://www.roboticsbook.org/S74_drone_perception.html#
        • https://gtbook.github.io/gtsam-examples/intro.html

    • Ask Virtana experts

      • Shiva
      • Vijay

    • Ask GTSAM experts

      • MIT spark team (Kimera)
      • GTSAM team
        • Frank
• Choose
• Try
• Repeat