About PinholeCameras and GPSFactors for monocular SfM + GPS

[Thiago Teixeira Santos]

Hi everyone,

I have a monocular SfM result presenting drift. Loop closure is not an option because the camera is facing different sides of the structure (ie, data association in visual features is not possible). However, I have GPS RTK readings.

In Python, I could load my COLMAP-based SfM model and create a factor graph using

:

• GeneralSFMFactorCal3Bundler for projective factors;
• Pose3 for landmarks L values L(j) and
• PinholeCameraCal3Bundler for state values X(i) 
  

Optimization using LevenberMarquardtOptimizer run fine, reducing the error but, as expected, it could not eliminate the drift (monocular case and a lot of previous bundle adjustment performed by COLMAP). Time to add GPS data.

I have added GPSFactors to X(i) :

factor = gtsam.GPSFactor(X(i), gtsam.Point3(lat, long, h), GPS_NOISE)

But the optmizer produced the following error:

RuntimeError: Attempting to retrieve value with key "x500", type stored in Values is gtsam::GenericValue<gtsam::PinholeCamera<gtsam::Cal3Bundler> > but requested type was gtsam::Pose3

I understand GTSAM is trying to compute an error between tow tridimentional poses, so expecting a Pose3, but getting a PinholeCamera - some "glue code"  should extract the pose (R ant t) from the PinholeCamera.

My guess is GTSAM can do it, absolutely. I have just not figured out how to do it right. Any tips? Could it be done in Python? Should I switch to C++?  Am I using the right classes for the task?

Any advice is welcome

--

Thiago

[Thiago Teixeira Santos]

Replacing  GeneralSFMFactorCal3Bundler  by GenericProjectionFactorCal3_S2  and PinholeCameraCal3Bundler  by Pose3 fixed the issue: LM is working fine.

--

th