fix mask index error and add trajectory saving#6

lastbasket wants to merge 2 commits into

endomapper:masterendomapper/NR-SLAM:masterfrom

lastbasket:masterlastbasket/NR-SLAM:masterCopy head branch name to clipboard