Skip to content

Commit 5e9fddb

Browse files
committed
fix bug in rotation estimation
1 parent b6e7f33 commit 5e9fddb

1 file changed

Lines changed: 5 additions & 6 deletions

File tree

‎spatialmath/base/transforms2d.py‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -773,6 +773,7 @@ def tr2jac2(T: SE2Array) -> R3x3:
773773
J[:2, :2] = smb.t2r(T)
774774
return J
775775

776+
776777
@overload
777778
def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float) -> SO2Array:
778779
...
@@ -782,6 +783,7 @@ def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float) -> SO2Array:
782783
def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float) -> SE2Array:
783784
...
784785

786+
785787
def trinterp2(start, end, s):
786788
"""
787789
Interpolate SE(2) or SO(2) matrices
@@ -977,15 +979,12 @@ def points2tr2(p1: NDArray, p2: NDArray) -> SE2Array:
977979
# compute moment matrix
978980
M = np.dot(p2_centered, p1_centered.T)
979981

980-
# get singular value decomposition of the cross covariance matrix
982+
# get singular value decomposition of the cross covariance matrix, use Umeyama trick
981983
U, W, VT = np.linalg.svd(M)
982984

983985
# get rotation between the two point clouds
984-
R = U @ VT
985-
# special reflection case
986-
if np.linalg.det(R) < 0:
987-
VT[-1, :] *= -1
988-
R = VT.T @ U.T
986+
s = [1, np.linalg.det(U) * np.linalg.det(VT)]
987+
R = U @ np.diag(s) @ VT
989988

990989
# get the translation
991990
t = p2_centroid - R @ p1_centroid

0 commit comments

Comments
 (0)