Many code changes
Update TODO list
Improve 3D reconstruction
Remove unused code