Working with ISAM2

[Yotam Stern]

Hi all, I'm trying to solve standard visual SLAM using feature points matching on a monocular video (500-1000 frames per video).

Since I don't have a good initial guess for the poses (for now i calculated essential matrix between consecutive frames for that) trying to solve the entire video in batch doesn't converge well.

So I thought about trying ISAM2 to solve this incrementally, but I keep getting the infamous IndeterminantLinearSystem exception, even when using QR linear solver.

I created a small test example with only 10 frames, and a baseline of ~1m movement between them so I won't have degenerate images, when I solve this small set in Batch it converges fine with guass-netwon, doglog and LM solvers (DL and GN with QR solver, cholesky throws indeterminant error).

However when I try to use ISAM2 and add the measurements incrementally it throws the error. I think what happens is that the problematic landmarks are with 3 observation that due to matching noise don't all align well, but when the batch solvers see all the observations for those landmarks (6-7) its not a problem.

My main issue is that the program crashes when incrementally adding the measurements to ISAM2 and I don't know how to proceed? how can I recover from this? can I remove the problematic landmark from ISAM2 until I get more observation for it and it'll become stable? I didn't see a way to do it.

Is this a common known problem when trying to solve feature points based visual-SLAM with ISAM2?

I'm using gtsam through the python wrappers.

[J Zhang]

I am interested in this issue as I am in the middle of studying the code of iSAM2.

Are you able to provide a minimal test code?

How do you form your factors? Using EssentialMatrixConstraint()? I assume you have added some prior factors or constant factors for the first two poses or the first pose and its landmarks to _lock_ the scale?

Thanks

JZ

[Yotam Stern]

I will try to provide a minimal test code, but its going to be in python.

I use the Generic projection factor, i calculate essential matrix using opencv and feed it at initial values for isam when i add a new pose.

I did add a prior on the first pose and on the first landmark like the isam2 example from the gtsam repo.

I did some more digging and realized that already on the first 2 images I add to isam2, some of the landmarks get negative Z values, from this point onwards the GenericProjectionFactor just retruns 0 error and 0 jacobians for this landmark and the optimization can't move it to new meaningful value.

I checked before adding the two first frames and the points between them to isam if any of my trackings yielded negative Z by trying to solve this simple graph with batch solver, but non of the trackins got negative Z values, its only when I add them to isam that they become negative, even if I feed isam with the values from the batch optimization as its values still some of them become negative, and the total error of the graph rises.

I feel like I'm missing something crucial here.

[J Zhang]

I might be very wrong here but fixing the prior for just one landmark might not be enough for generic VO system?

In the ISAM2 example, they have a fake object and all the measurements are from that single object so one prior for the first landmark is enough. 

Sounds in your case you might want to add prior for the first two poses or priors for the first pose and all the landmarks anchored on this first pose.

JZ

[Yotam Stern]

Ok i think i cleared all the issues i had and can start working with this.

One important note, the problem of landmark moving behind all cameras that observe it is a serious one and should be avoided at all cost by making double sure the trackings are sane before adding them to the graph.

I had some issue with solving the first 2 frames of the video because gauss-newton optimization is very crude for such a small problem of 2 poses and only ~1000 observations, as a result few landmarks got negative Z values from the initial 2 frames and i couldn't continue adding information to the isam graph at all, switching to DogLeg solved this issue and from then on if i was super carful not to add bad trackings that can end up behind all cameras that observe them i was good to do.

If someone still wants to work with GN and not DogLog i recommend solving the first 10-20 frames using batch optimizations and then letting isam take it from there, once the problem is not so small and well constrained GN can handle it well.

--
You received this message because you are subscribed to a topic in the Google Groups "gtsam users" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/gtsam-users/cna5Up8aa6w/unsubscribe.
To unsubscribe from this group and all its topics, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/69ae8a85-7c38-4c73-86fc-192bb0414dc0n%40googlegroups.com.

[J Zhang]

I am fighting the same issue with you now in the VO factor graph. I am constantly running into under constraint exceptions with ISAM2-GN even after solving the first 100 frames with batch. 

I am getting much more stable results with ISAM2-DogLeg. I am super careful of not adding any 3d points behind cameras and all the KF frames are with decent baselines. 

[Thomas Eberhard]

Hi I encounter what I believe to be a similar problem with a minimal reproducible example:
https://groups.google.com/g/gtsam-users/c/yO3cpAl5CAg/m/O5Pc7Z50HAAJ

[Nicolas Perez]

I am having the same issue: https://github.com/borglab/gtsam/issues/1179 
If anyone has any tips on how I could make my code work, that'd be appreciated. I will also send an update if I get my code to work.

[Nicolas Perez]

Update: it looks like if I 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.

[Dellaert, Frank]

Interesting! Hypothesis on what went wrong?

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Nicolas Perez <nicolasa...@gmail.com>
Sent: Monday, May 9, 2022 12:51
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Re: Working with ISAM2

 

You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/859364ae-10c4-4b26-9b8d-bbb82a7fcfaen%40googlegroups.com.

[Yotam Stern]

Frank, one of the main issues is that both GN and DL optimizers are prone to indeterminant system error due to some points getting negative Z relative to the images they are observed in, or some landmarks that are infinitely far away give near zero jacobians.

can iSAM2 be modified to use LM optimizer? LM can overcome and "ignore" those issues since the J.T*J term has the added lamba * I part which removes singularities in the jacobians.

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/BN7PR07MB48355B6C6A22A7BAC43FDAF7CFC69%40BN7PR07MB4835.namprd07.prod.outlook.com.

[Dellaert, Frank]

Yes, that can definitely be done by a motivated open source contributor :-) it should not be that hard.

Frank

---

From: Yotam Stern <sht...@gmail.com>
Sent: Monday, May 9, 2022 1:25:37 PM
To: Dellaert, Frank <frank.d...@cc.gatech.edu>
Cc: Nicolas Perez <nicolasa...@gmail.com>; gtsam users <gtsam...@googlegroups.com>

[Nicolas Perez]

I don't have much of a hypothesis as to why my situation works the way it does. I am guessing it has something to do with a matrix inversion being numerically infeasible. I will investigate more if I have time... my current research project is very application-based as opposed to working with the theory and numbers behind SLAM. Although I do also find the math very interesting!