Bundle adjustment

Change-Id: Id66aaff756892f0c50ebbdf7daa3e54fb5273f4d