small corrections. #2
Add check to use only good pose estimation. #2