Few bugs fixed. Multi slam has been tested with two bag files. Seems to work fine.