Finished first version of 3D SLAM unknown features
Does not do feature sharing yet
Runs amazingly slowly
Not sure if working yet. Unknown features seem to have some correspondence to truth at the final time....