github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:08:05 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix function dRV_dQuat of class Optimizer3D

Browse Source
This commit is contained in:
Ryohei Sasaki
2019-09-07 14:35:09 +09:00
committed by GitHub
parent cb8af3c2e4
commit 01a5bcb419
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py
View File

@@ -235,22 +235,28 @@ class Optimizer3D:
duy = dRdqw * dqdu[0,1] + dRdqx * dqdu[1, 1] + dRdqy * dqdu[2, 1] + dRdqz * dqdu[3, 1]
duz = dRdqw * dqdu[0,2] + dRdqx * dqdu[1, 2] + dRdqy * dqdu[2, 2] + dRdqz * dqdu[3, 2]
return dux, duy, duz
def dRV_dQuat(self, q):
qw = q.qw[0]
qx = q.qx[0]
qy = q.qy[0]
qz = q.qz[0]
if 1 - q.qw**2 < 1e-7:
if 1 - qw**2 < 1e-7:
ret = np.array(
[[ 0.0, 2.0, 0.0, 0.0],
[ 0.0, 0.0, 2.0, 0.0],
[ 0.0, 0.0, 0.0, 2.0]]
)
return ret
c = 1/(1 - q.qw**2)
d = np.arccos(q.qw)/(np.sqrt(1-q.qw**2))
c = 1/(1 - qw**2)
d = np.arccos(qw)/(np.sqrt(1-qw**2))
ret = 2.0 * np.array(
[[ c*q.qx*(d*q.qw-1), d, 0.0, 0.0],
[ c*q.qy*(d*q.qw-1), 0.0, d, 0.0],
[ c*q.qz*(d*q.qw-1), 0.0, 0.0, d]]
[[ c*qx*(d*qw-1), d, 0.0, 0.0],
[ c*qy*(d*qw-1), 0.0, d, 0.0],
[ c*qz*(d*qw-1), 0.0, 0.0, d]]
)
return ret