SO3::ln and exp error
Opened this issue · 0 comments
zdzhaoyong commented
LOG(ERROR)<<"Origin:"<<curGPS.get_rotation()
<<",Noise:"<<gps_noise.get_rotation()
<<",After:"<<noised_gps.get_rotation();
LOG(ERROR)<<"Origin:"<<lnorigin
<<",Quat:"<<SO3::exp(lnorigin)
<<",After:"<<SO3::exp(lnorigin).ln();
Output as follows:
testOptimizer.cpp:420 Origin:0 0 -1 -1.71738e-16,Noise:-0.00122998 0.000986742 0.000211876 0.999999,After:-0.000782921 0.000628094 0.999999 -0.000212667
testOptimizer.cpp:423 Origin:-0.00245995 0.00197348 3.14202,Quat:-0.000782921 0.000628094 0.999999 -0.000212667,After:0.00245929 -0.00197295 -3.14117
The Origin and After should be equal!