Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix typo in g2o file format parsing for the information matrix #1310

Merged
merged 3 commits into from
Oct 26, 2022

Conversation

mauricefallon
Copy link
Contributor

the g2o parser seems to have a bug when parsing the information matrix.

In brief:

  • g2o variable ordering is t, R - translation then rotation
  • GTSAM ordering is the other way around.
  • the g2o information matrix is parsed to form a 6x6 information matrix.
  • to convert it to [R, t] format the upper left 3x3 needs to be swapped with the lower right 3x3 (correct at present). But this also needs to be done with the offdiagonal blocks - which is where the bug is.

IMO the correct values would be:
g2o info matrix is: (input)
00 10 20 30 40 50
10 01 21 31 41 51
20 21 02 32 42 52
30 31 32 03 43 53
40 41 42 43 04 54
50 51 52 53 54 05

the correct gtsam information matrix is: (output, with my fix)
03 43 53 30 31 32
43 04 54 40 41 42
53 54 05 50 51 52
30 40 50 00 10 20
31 41 51 10 01 21
32 42 52 20 21 02

After the reordering the offdiagonal elements (e.g. 31) should still be aligned with corresponding dimensions (1 and 3)

A test string for g2o:
EDGE_SE3:QUAT 1054 1055 0.251779 0.0571828 0.0217246 -0.0167946 -0.0087881 -0.0277189 0.999436 0 10 20 30 40 50 1 21 31 41 51 2 32 42 52 3 43 53 4 54 5

Final question:
way are there no GTSAM tests failing? Do some of them not have off diagonal terms e.g. the spheres dataset?

For reference Ceres parser:
https://github.com/ceres-solver/ceres-solver/blob/master/examples/slam/common/read_g2o.h#L94

@mauricefallon
Copy link
Contributor Author

@mmattamala

@varunagrawal varunagrawal self-requested a review October 22, 2022 23:58
@varunagrawal
Copy link
Collaborator

varunagrawal commented Oct 23, 2022

Final question:
way are there no GTSAM tests failing? Do some of them not have off diagonal terms e.g. the spheres dataset?

@mauricefallon this is a very good question. Can @mmattamala or someone else please add a test which will also verify the fix?

I guess something simple would be to make a trivial graph that exposes the issue, write it to a g2o format string and parse it back, then we can check the parsed one against the original one.

mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // info rotation
mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // info translation
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(0, 3); // off diagonal swap
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess there should be a comment here specifying that we need to swap to read the matrix in R,t order?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I agree we should be more explicit on our comments to avoid future confusions

@varunagrawal
Copy link
Collaborator

Some tests are failing on the CI related to SLAM graphs.

@mmattamala
Copy link
Contributor

mmattamala commented Oct 23, 2022

I realized the tests are failing because in this PR we are just fixing the g2o-GTSAM conversions when reading Pose3 data but not when writing them. I made a PR against @mauricefallon's branch to fix it (mauricefallon#1), so we don't open more PRs here.

The test that's failing is writeG2o3DNonDiagonalNoise in testDataset.cpp, which implements exactly what @varunagrawal proposed before.

Fix in writeG2o when writing Pose3 measurement
@varunagrawal
Copy link
Collaborator

Awesome stuff. If we can get a simple test to verify this (and for posterity) then we can happily merge this in.

@mauricefallon
Copy link
Contributor Author

@varunagrawal , this bug fix is an error in file I/O - not in GTSAM itself.

the test writeG2o3DNonDiagonalNoise reads in a g2o file (which is non diagonal), parses its constrants into a GTSAM graph and then writes out the constraints and compares the output constraints to the expected result.

In brief:

  • this is read: pose3example-offdiagonal.txt
  • another file is created
  • this file is compared to pose3example-offdiagonal-rewritten.txt

The issue is that previously the test was just working because the input and output parsing code were BOTH wrong. "Two wrongs made a right"

Which this PR, the test still passes - the output is still the same. But the parsing is correct.

I dont think a test is needed.

@varunagrawal
Copy link
Collaborator

Fair enough. Approving.

Copy link
Collaborator

@varunagrawal varunagrawal left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@varunagrawal varunagrawal merged commit 74133b2 into borglab:develop Oct 26, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants