search

JeffLIrion / python-graphslam

Graph SLAM solver in Python
MIT License
200 stars 27 forks source link

Edge information matrix is being incorrectly processed from .g2o files #28

Closed williamdwarke closed 2 years ago

williamdwarke commented 2 years ago

Hi, thanks for the awesome library! This could be python version dependent, but I noticed a bug in the code that processes edge information matrices from .g2o files. For SE3 quaternion-parameterized edges, the element from index (5,4) of the 6x6 matrix is showing up at index (6,3). I boiled it down to a problem with the upper_triangular_matrix_to_full_matrix function from line 50 of util.py. I did some testing and it appears that the issue is caused by an indexing issue on line 72:

mat[tril1] = mat[triu1]

I fixed it for myself by instead setting the lower triangular portion of the matrix using the indices of the lower triangular part of the transposed matrix:

mat[tril1] = mat.T[tril1]

I also removed the line where triu1 is computed as it is no longer needed. There may be a more elegant fix for this, but this is the best I could come up with. Cheers :)

JeffLIrion commented 2 years ago

Thanks for tracking down the bug!