:ȡתſ˱Ⱦ

ʽ:SLAM_SO3_Jl(R)

R:3*3ת,1*33*1

˵: 
1жRתʣжRά
2ɹһRӦ3*3ſ˱Ⱦ

ԭ:
תΪd,һΪa,ſ˱ȾJΪ
J = sin(d)/d * I + (1 - sin(d)/d) * a * a^T + (1 - cos(d))/d * {SLAM_AntiSymMatrix<\SLAM_AntiSymMatrix>}(a)

:
v = [0.1,0.5,0.8];
jl = SLAM_SO3_Jl(v)//سõ½
jl =
[ 0.85820040435419   -0.36291940995749    0.24454958067916
  0.37885194879410    0.89643849756205    0.01736944542445
 -0.21905751854058    0.11009086526840    0.95857539902482 ]