try to find a bug of transformation
refactoring of variable names
rewirte code
still rewriting
This version can give us a "seems" correct result, but the eqution is strange
Use the correct equation
use libviso2 to get camera pose, this give us correct result
update to new

contain the change of libviso2, can work now but not good yet