fix typo in g2o file format parsing for the information matrix

[mauricefallon]

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]

@mmattamala

[varunagrawal]

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.

[varunagrawal]

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

[mmattamala]

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

[varunagrawal]

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

[mmattamala]

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.

[varunagrawal]

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

[mauricefallon]

@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]

Fair enough. Approving.

[varunagrawal]

LGTM