iSAM (or iSAM2) + GLC in GTSAM

[Enrique Fernández Perdomo]

I'm wondering if there's any intention to have some node removal algorithm in GTSAM, or maybe there's already something and I haven't notice. I'm thinking of something like the Generic Linear Constraint (GLC) Node Removal (http://robots.engin.umich.edu/SoftwareData/GLC) which is already implemented on Dr. Michael Kaess' iSAM repository (http://people.csail.mit.edu/kaess/isam/); but I haven't seen it on GTSAM.

Assigning to @cbeall3, as I've told with him about this by e-mail. I've created this issue following his suggestion, but I could be able to help on that. Indeed, I'd really like to see node removal algorithms on GTSAM, and I'd be happy to help on its development, but I also don't want to duplicate work, re-invent the wheel or implement an obsolete approach.

CC @dellaert

Feel free to edit the title, milestone and version of this issue, and even the priority ;)

[Frank Dellaert]

@richardroberts or @kaess care to comment?

[Richard Roberts]

Hi guys, hopefully this is relevant, there's a factor and node removal functionality in iSAM2. If you remove all factors attached to a node, that node will be deleted. You can also do things like replace factors on a node with a linear "summary" factor.

[Enrique Fernández Perdomo]

@richardroberts Thanks for your answer, but from your comment it seems that such functionality doesn't perform any of the actions explained on the GLC paper (marginalization of the remaining factors on the elimination clique) or composition (as in other papers mentioned in the GLC one).

The "summary" factor might be useful to implement any of them, but I don't know what is it? Where is that explained (either paper or the code)?

[Richard Roberts]

I see, one other thing that may be relevant is the marginalization of iSAM2 leaf nodes, which is performed by the function ISAM2::marginalizeLeaves. This creates summary factors in the place of nodes that it is requested to marginalize away. Summary factors are simply linear factors that combine information from all factors attached to a group of nodes linearized about a fixed point. Is that closer to what you want to do? Note that because of the convention in GTSAM that Jacobians are represented in the "body" frame of each node, the nodes themselves can be updated with large deltas and the summary covariances will automatically rotate.

If you're looking for automatic map maintenance, we definitely don't have that implemented (at least not when I was there... @dellaert anything built since then? :-) ). But the iSAM2 data structure should be able to do the calculations required for it, though it may require some modifications if marginalizeLeaves can't do what you want...

Also take a look at the 'findUnusedFactorSlots' parameter of iSAM2 - you'll want to switch that on if you're going to be removing and adding a lot of nodes/factors.

[Michael Kaess]

We currently do not have an implementation of GLC for gtsam...

[Enrique Fernández Perdomo]

Thanks for your comments @richardroberts.

@kaess, I was mainly wondering if it's worth implementing GLC in GTSAM, as there are other approaches which provide a consistent sparsification. Indeed, one of them it's your collaboration on Huang's ECMR'13 paper.

Do you know if this is also in your iSAM repository but not in GTSAM?

I don't know if Huang has done anything newer on this topic. Maybe you do.

The only another paper I know about is Durrant-Whyte's and Tim Bailey's reference one, which is cited in most of the papers I've already mentioned.

My intention is ---time permitting--- to add either GLC or the ECMR'13 approach to GTSAM, so there's some algorithm for node marginalization + edge sparsification.

Thanks for your help and comments.

[Frank Dellaert]

OK, I'll close this as this is really about new functionality.

[Frank Dellaert]

Enrique will examine adding this functionality

[Dhiraj]

I apologize because the issue is marked closed but I have a doubt with respect to the statement by Richard Roberts. Please let me know if I need to open a new issue for the same.

As per Richard Roberts in the above post there's a factor and node removal functionality in iSAM2.

To test the same I added the following test in testGaussianISAM2.cpp to remove the prior and the node. (Note this is a modification of the testcase removeVariables)

TEST(ISAM2, removePrior)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));

  FastVector<size_t> toRemove;
  // Remove the prior
  toRemove.push_back(0);
  //Also the betweenfactor between 0 and 1
  toRemove.push_back(1);
  isam.update(NonlinearFactorGraph(), Values(), toRemove);

  // Remove the factors and variable from the full system
  fullgraph.remove(0);
  fullgraph.remove(1);
  fullinit.erase(1);

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

But the above code gives me the following error:

gtsam-3.2.1/tests/testGaussianISAM2.cpp:457: Failure: "Exception: Indeterminant linear system detected while working near variable 4.

Thrown when a linear system is ill-posed. The most common cause for this error is having underconstrained variables. Mathematically, the system is underdetermined. See the GTSAM Doxygen documentation at http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for more information. There were 1 failures

Can somebody please explain me what am i doing wrong here? I am using GTSAM version 3.2.1

Thanks & Regards, -dgulati

[Frank Dellaert]

No worries. The ILS exception occurs when there is not enough information on a particular variable. Can you verify that by removing the prior the system is not underconstrained?

[Dhiraj]

Thank you for your answer. I am a newbie in GTSAM world so I am not sure how can I find if the system became underconstrained.

Also I read the documentation of IndeterminantLinearSystemException which does mentions one of the reason of such exception as missing a prior.

But what I did not understand is why will it still not smoothen with the available information. Although now it may have bigger covariances. Maybe I lack understanding of certain basic principle.

Thanks & Regards,

-Dhiraj

[Frank Dellaert]

I apologize but won't be able to give extended help here: but, to give an example, you cannot triangulate a 3D point from a measurement in a single camera. In that case, the depth is unobservable and the covariance is infinite, not just larger. That will yield a singular matrix in the optimization, and is why adding a prior can help.

[Dhiraj]

OK. I think I understand the perspective, the difference between large and infinite. Thanks for the example. I will read more and in case have specific query will get back here.

Thanks & Regards, -Dhiraj

[cheng.wang]

deer professor Dellaert: I'm using GTSAM4.0 to build a SLAM system now, like Dhiraj, I get the ill-posed problem, for example, there are 1000 frames, every time the SLAM processes No.500 frame, GTSAM4.0 throws ill-posed exception, even I start at No.480/No.490 frame, the same exception happens. I write many logs in my code, and find out that the function gtsam::ISAM2 isam2.calculateEstimate() throws out ill-posed exception, I don't know the reason, so I just add "try/catch" to this function, and backup isam2(because only after function update(), I can know whether calculateEstimate() is ill-posed). However, there is no operater= in the class gtsam::ISAM2, so I can't backup isam2. Can you give me some suggestions about how to avoid “ ill-posed exception”? thank you very much! @"@dellaert"

[chang liu]

Hi, cheng, "@cheng.wang" Have you Solved this problem? It seems like i meet the same issue with you, and i will be real appreciate with your reply.