Skip to content

Commit 43f34e0

Browse files
committed
add function to compute transform from corresponding points
1 parent 3423907 commit 43f34e0

1 file changed

Lines changed: 42 additions & 0 deletions

File tree

‎spatialmath/base/transforms2d.py‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -718,6 +718,48 @@ def _vec2s(fmt, v):
718718
return ", ".join([fmt.format(x) for x in v])
719719

720720

721+
def points2tr2(p1, p2):
722+
"""
723+
SE(2) transform from corresponding points
724+
725+
:param p1: first set of points
726+
:type p1: array_like(2,N)
727+
:param p2: second set of points
728+
:type p2: array_like(2,N)
729+
:return: transform from ``p1`` to ``p2``
730+
:rtype: ndarray(3,3)
731+
732+
Compute an SE(2) matrix that transforms the point set ``p1`` to ``p2``.
733+
p1 and p2 must have the same number of columns, and columns correspond
734+
to the same point.
735+
"""
736+
737+
# first find the centroids of both point clouds
738+
p1_centroid = np.mean(p1, axis=0)
739+
p2_centroid = np.mean(p2, axis=0)
740+
741+
# get the point clouds in reference to their centroids
742+
p1_centered = p1 - p1_centroid
743+
p2_centered = p2 - p2_centroid
744+
745+
# compute moment matrix
746+
M = np.dot(p2_centered.T, p1_centered)
747+
748+
# get singular value decomposition of the cross covariance matrix
749+
U, W, V_t = np.linalg.svd(M)
750+
751+
# get rotation between the two point clouds
752+
R = np.dot(U, V_t)
753+
754+
# get the translation
755+
t = np.expand_dims(p2_centroid,0).T - np.dot(R, np.expand_dims(p1_centroid,0).T)
756+
757+
# assemble translation and rotation into a transformation matrix
758+
T = np.identity(3)
759+
T[:2,2] = np.squeeze(t)
760+
T[:2,:2] = R
761+
762+
return T
721763

722764
def trplot2(
723765
T,

0 commit comments

Comments
 (0)