zdzhaoyong/GSLAM

SO3::ln and exp error

Opened this issue · 0 comments

 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!