Remove static identity matrices in lago.cpp to avoid memory corruption in downstream tasks

[BITVoyager]

I encountered a memory issue while rebuilding the repo gpslam against the latest GTSAM develop branch. There are some memory issues.

==15074== Invalid read of size 8
==15074==    at 0x10D55F: Eigen::internal::handmade_aligned_free(void*) (Memory.h:118)
==15074==    by 0x10D5D6: Eigen::internal::aligned_free(void*) (Memory.h:206)
==15074==    by 0x10F58F: void Eigen::internal::conditional_aligned_free<true>(void*) (Memory.h:259)
==15074==    by 0x10E642: void Eigen::internal::conditional_aligned_delete_auto<double, true>(double*, unsigned long) (Memory.h:446)
==15074==    by 0x10DF82: Eigen::DenseStorage<double, -1, -1, -1, 0>::~DenseStorage() (DenseStorage.h:465)
==15074==    by 0x10D66D: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::~PlainObjectBase() (PlainObjectBase.h:98)
==15074==    by 0x10D68D: Eigen::Matrix<double, -1, -1, 0, -1, -1>::~Matrix() (Matrix.h:178)
==15074==    by 0x5248381: __cxa_finalize (cxa_finalize.c:82)
==15074==    by 0x4A0D7E6: ??? (in /usr/local/lib/libgtsam.so.4.3a0)
==15074==    by 0x40010F1: _dl_call_fini (dl-call_fini.c:43)
==15074==    by 0x4005577: _dl_fini (dl-fini.c:114)
==15074==    by 0x5248A75: __run_exit_handlers (exit.c:108)
==15074==  Address 0x56afa48 is 8 bytes before a block of size 72 alloc'd
==15074==    at 0x4846828: malloc (in /usr/libexec/valgrind/vgpreload_memcheck-amd64-linux.so)
==15074==    by 0x4A0D25B: _GLOBAL__sub_I_lago.cpp (in /usr/local/lib/libgtsam.so.4.3a0)
==15074==    by 0x400571E: call_init.part.0 (dl-init.c:74)
==15074==    by 0x4005823: call_init (dl-init.c:120)
==15074==    by 0x4005823: _dl_init (dl-init.c:121)
==15074==    by 0x401F59F: ??? (in /usr/lib/x86_64-linux-gnu/ld-linux-x86-64.so.2)
==15074==    by 0x1: ???
==15074==    by 0x1FFEFFFEDA: ???
==15074==    by 0x1FFEFFFF0A: ???
==15074== 
==15074== 
==15074== HEAP SUMMARY:
==15074==     in use at exit: 80 bytes in 2 blocks
==15074==   total heap usage: 62 allocs, 60 frees, 93,013 bytes allocated
==15074== 
==15074== LEAK SUMMARY:
==15074==    definitely lost: 0 bytes in 0 blocks
==15074==    indirectly lost: 0 bytes in 0 blocks
==15074==      possibly lost: 0 bytes in 0 blocks
==15074==    still reachable: 80 bytes in 2 blocks
==15074==         suppressed: 0 bytes in 0 blocks
==15074== Rerun with --leak-check=full to see details of leaked memory
==15074== 
==15074== For lists of detected and suppressed errors, rerun with: -s
==15074== ERROR SUMMARY: 2 errors from 1 contexts (suppressed: 0 from 0)

GTSAM or Eigen is trying to free a pointer that was offset (ptr - 1) from an actual malloc'ed block. The free occurs after main() exits, during static/global object cleanup:

by __cxa_finalize
by _GLOBAL__sub_I_lago.cpp

This points to the two removed lines in lago.cpp, which are static Eigen matrices. Eigen stores metadata (like the original malloc pointer) just before the returned pointer. If that memory is improperly aligned, copied, or double-freed (as can happen with global destruction order issues), handmade_aligned_free() crashes. In order to avoid this issue, I removed the two static variables and directly used the type aliases in function calls.

[dellaert]

Thanks for debugging! Are there performance issues related to creating those identity matrices over and over? I know they are template expressions but the factors themselves don't take Eigen template expressions, so they get transcribed into actual memory. And then copied? So - maybe we can at least allocate const Matrix variables on the stack outside the loop?

[ProfFan]

@BITVoyager Could you rebase on develop?

[ProfFan]

Thank you!