:ȡתſ˱Ⱦ

ʽ:SLAM_SO3_Jr(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];
jr = SLAM_SO3_Jr(v)//سõ½
jr =
[ 0.85820040435419    0.37885194879410   -0.21905751854058
 -0.36291940995749    0.89643849756205    0.11009086526840
  0.24454958067916    0.01736944542445    0.95857539902482 ]