LOST Triangulation Underconstrained

[travisdriver]

I encountered an instance where the iterated DLT solution would correctly triangulate an input point, while the LOST algorithm would return NaNs.

This seems to be related to a rank deficiency in the QR factorization. The rank tolerance was being set for the DLT methods, but it was not being set for LOST and was instead being computed automatically by Eigen.

Therefore, I added the rank_tol variable to the triangulateLOST method so that the user could explicitly set the tolerance. However, this change didn't seem to fix the rank deficiency issue.

Below is code to reproduce the issue. The measurements and camera data were derived from images and ancillary data from the Hayabusa mission to Asteroid 25143 Itokawa.

import numpy as np
import gtsam

from read_write_model import read_model

cameras, images, points3d = read_model("lost_underconstrained_data")

noisemodel = gtsam.noiseModel.Isotropic.Sigma(2, 1e-4)
error = []
for jj in points3d.keys():
    if len(points3d[jj].image_ids) < 5: 
        continue
    kpt_vec, cam_vec = [], []
    for idx, kk in enumerate(points3d[jj].image_ids):
        # Add keypoint measurements.
        pp = points3d[jj].point2D_idxs[idx];  # keypoint index
        kpt_vec.append(gtsam.Point2(images[kk].xys[pp].T.astype("double")))

        # Add cameras.
        R_BC = images[kk].qvec2rotmat().T
        r_CB_B = -R_BC @ images[kk].tvec[..., None]
        fx, fy, cx, cy = cameras[images[kk].camera_id].params
        cam_vec.append(
            gtsam.PinholeCameraCal3_S2(
                gtsam.Pose3(gtsam.Rot3(R_BC), gtsam.Point3(r_CB_B.flatten())),
                  gtsam.Cal3_S2(fx, fy, 0.0, cx, cy),
            )
        )

    # Triangulate.        
    kpt_vec = gtsam.Point2Vector(kpt_vec)
    cam_vec = gtsam.CameraSetCal3_S2(cam_vec)
    xyz_est = gtsam.triangulatePoint3(cam_vec, kpt_vec, rank_tol=1e-9, optimize=False, model=noisemodel, useLOST=True)
    # xyz_est = gtsam.triangulatePoint3(cam_vec, kpt_vec, rank_tol=1e-9, optimize=True, model=noisemodel)
    error.append(np.linalg.norm(points3d[jj].xyz - xyz_est))
    print(xyz_est)

print("Mean error:", np.mean(error))
print(error)

Data in COLMAP format:
lost_underconstrained_data.zip

The read_write_model module is defined here.

[akshay-krishnan]

Thank you for catching this!
On the gtsam repo, it is actually not encouraged to format entire files. Its also hard for reviewers to spot the relevant change. Other than setting the rank tolerance for Eigen, did I miss anything?

If you have a small example with only 2 cameras where this happens, it might be worth adding a unit test too.

[travisdriver]

The issue was due to the baseline vector coinciding with the jth measurement vector (wZj), which caused the denominator to be zero. I added a simple check that loops through other values for j if this occurs.

[akshay-krishnan]

could you revert the formatting changes from the first commit?

[akshay-krishnan]

is this line exceeding the max line length?

[travisdriver]

Yes, fixed

[travisdriver]

could you revert the formatting changes from the first commit?

Reverted all of the formatting.

[varunagrawal]

Hi @travisdriver. Sorry for the extra work, but can you please add a unit test as well. You can copy-paste the post-processed values from the Hayabusa mission to serve as your test fixture. This will allow us to maximize code coverage and guarantee correctness in the future as well.

[dellaert]

I'm going to merge - a unit test would be nice but we have those in GTSFM, and the code does have a set of unit tests already