"""

theta = base.getunit(theta, unit)

ct = base.sym.cos(theta)

st = base.sym.sin(theta)

# fmt: off

R = np.array([

[ct, -st],

[st, ct]])

# fmt: on

"""

T = np.pad(rot2(theta, unit), (0, 1), mode="constant")

if t is not None:

T[:2, 2] = base.getvector(t, 2, "array")

T[2, 2] = 1 # integer to be symbolic friendly

"""

xyt = base.getvector(xyt, 3)

T = np.pad(rot2(xyt[2], unit), (0, 1), mode="constant")

T[:2, 2] = xyt[0:2]

T[2, 2] = 1.0

"""

if T.dtype == "O" and _symbolics:

angle = sympy.atan2(T[1, 0], T[0, 0])

else:

angle = math.atan2(T[1, 0], T[0, 0])

"""

if base.isscalar(x) and base.isscalar(y):

# (x, y) -> SE(2)

t = np.r_[x, y]

elif base.isvector(x, 2):

# R2 -> SE(2)

t = base.getvector(x, 2)

elif base.ismatrix(x, (3, 3)):

# SE(2) -> R2

return x[:2, 2]

else:

raise ValueError("bad argument")

if t.dtype != "O":

t = t.astype("float64")

T = np.identity(3, dtype=t.dtype)

T[:2, 2] = t

"""

return (

isinstance(T, np.ndarray)

and T.shape == (3, 3)

and (

not check

or (base.isR(T[:2, :2]) and np.all(T[2, :] == np.array([0, 0, 1])))

)

"""

return (

isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or base.isR(R))

r"""

if not ishom2(T):

raise ValueError("expecting SE(2) matrix")

# inline this code for speed, don't use tr2rt and rt2tr

R = T[:2, :2]

t = T[:2, 2]

Ti = np.zeros((3, 3), dtype=T.dtype)

Ti[:2, :2] = R.T

Ti[:2, 2] = -R.T @ t

Ti[2, 2] = 1

"""

if ishom2(T, check=check):

# SE(2) matrix

if base.iseye(T):

# is identity matrix

if twist:

return np.zeros((3,))

else:

return np.zeros((3, 3))

else:

if twist:

return base.vexa(scipy.linalg.logm(T))

else:

return scipy.linalg.logm(T)

elif isrot2(T, check=check):

# SO(2) rotation matrix

if twist:

return base.vex(scipy.linalg.logm(T))

else:

return scipy.linalg.logm(T)

else:

"""

if base.ismatrix(S, (3, 3)) or base.isvector(S, 3):

# se(2) case

if base.ismatrix(S, (3, 3)):

# augmentented skew matrix

if check and not base.isskewa(S):

raise ValueError("argument must be a valid se(2) element")

tw = base.vexa(S)

else:

# 3 vector

tw = base.getvector(S)

if base.iszerovec(tw):

return np.eye(3)

if theta is None:

(tw, theta) = base.unittwist2_norm(tw)

elif not base.isunittwist2(tw):

raise ValueError("If theta is specified S must be a unit twist")

t = tw[0:2]

w = tw[2]

R = base.rodrigues(w, theta)

skw = base.skew(w)

V = (

np.eye(2) * theta

+ (1.0 - math.cos(theta)) * skw

+ (theta - math.sin(theta)) * skw @ skw

)

return base.rt2tr(R, V @ t)

elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1):

# so(2) case

if base.ismatrix(S, (2, 2)):

# skew symmetric matrix

if check and not base.isskew(S):

raise ValueError("argument must be a valid so(2) element")

w = base.vex(S)

else:

# 1 vector

w = base.getvector(S)

if theta is not None and not base.isunitvec(w):

raise ValueError("If theta is specified S must be a unit twist")

# do Rodrigues' formula for rotation

return base.rodrigues(w, theta)

else:

# http://ethaneade.com/lie.pdf

if T.shape == (3, 3):

# SO(2) adjoint

return np.identity(2)

elif T.shape == (3, 3):

# SE(2) adjoint

(R, t) = base.tr2rt(T)

# fmt: off

return np.block([

[R, np.c_[t[1], -t[0]].T],

[0, 0, 1]

])

# fmt: on

else:

r"""

if not ishom2(T):

raise ValueError("expecting an SE(2) matrix")

J = np.eye(3, dtype=T.dtype)

J[:2, :2] = base.t2r(T)

"""

if base.ismatrix(end, (2, 2)):

# SO(2) case

if start is None:

# TRINTERP2(T, s)

th0 = math.atan2(end[1, 0], end[0, 0])

th = s * th0

else:

# TRINTERP2(T1, start= s)

if start.shape != end.shape:

raise ValueError("start and end matrices must be same shape")

th0 = math.atan2(start[1, 0], start[0, 0])

th1 = math.atan2(end[1, 0], end[0, 0])

th = th0 * (1 - s) + s * th1

return rot2(th)

elif base.ismatrix(end, (3, 3)):

if start is None:

# TRINTERP2(T, s)

th0 = math.atan2(end[1, 0], end[0, 0])

p0 = transl2(end)

th = s * th0

pr = s * p0

else:

# TRINTERP2(T0, T1, s)

if start.shape != end.shape:

raise ValueError("both matrices must be same shape")

th0 = math.atan2(start[1, 0], start[0, 0])

th1 = math.atan2(end[1, 0], end[0, 0])

p0 = transl2(start)

p1 = transl2(end)

pr = p0 * (1 - s) + s * p1

th = th0 * (1 - s) + s * th1

return base.rt2tr(rot2(th), pr)

else:

"""

s = ""

if label is not None:

s += "{:s}: ".format(label)

# print the translational part if it exists

if ishom2(T):

s += "t = {};".format(_vec2s(fmt, transl2(T)))

angle = math.atan2(T[1, 0], T[0, 0])

if unit == "deg":

angle *= 180.0 / math.pi

s += " {}°".format(_vec2s(fmt, [angle]))

else:

s += " {} rad".format(_vec2s(fmt, [angle]))

if file:

print(s, file=file)

"""

# first find the centroids of both point clouds

p1_centroid = np.mean(p1, axis=0)

p2_centroid = np.mean(p2, axis=0)

# get the point clouds in reference to their centroids

p1_centered = p1 - p1_centroid

p2_centered = p2 - p2_centroid

# compute moment matrix

M = np.dot(p2_centered.T, p1_centered)

# get singular value decomposition of the cross covariance matrix

U, W, VT = np.linalg.svd(M)

# get rotation between the two point clouds

R = U @ VT

# special reflection case

if np.linalg.det(R) < 0:

VT[-1, :] *= -1

R = VT.T @ U.T

# get the translation

t = np.expand_dims(p2_centroid, 0).T - np.dot(R, np.expand_dims(p1_centroid, 0).T)

# assemble translation and rotation into a transformation matrix

T = np.identity(3)

T[:2, 2] = np.squeeze(t)

T[:2, :2] = R

mean_sq_error = 1.0e6 # initialize error as large number

delta_err = 1.0e6 # change in error (used in stopping condition)

num_iter = 0 # number of iterations

if T is None:

T = np.eye(3)

ref_kdtree = KDTree(reference.T)

tf_source = source

source_hom = np.vstack((source, np.ones(source.shape[1])))

while delta_err > min_delta_err and num_iter < max_iter:

# find correspondences via nearest-neighbor search

matched_ref_pts, matched_source, indices = _FindCorrespondences(ref_kdtree, tf_source, reference)

# find alingment between source and corresponding reference points via SVD

# note: svd step doesn't use homogeneous points

new_T = _AlignSVD(matched_source, matched_ref_pts)

# update transformation between point sets

T = T @ new_T

# apply transformation to the source points

tf_source = T @ source_hom

tf_source = tf_source[:2, :]

# find mean squared error between transformed source points and reference points

# TODO: do this with fancy indexing

new_err = 0

for i in range(len(indices)):

if indices[i] != -1:

diff = tf_source[:, i] - reference[:, indices[i]]

new_err += np.dot(diff,diff.T)

new_err /= float(len(matched_ref_pts))

# update error and calculate delta error

delta_err = abs(mean_sq_error - new_err)

mean_sq_error = new_err

print('ITER', num_iter, delta_err, mean_sq_error)

num_iter += 1

# get distances to nearest neighbors and indices of nearest neighbors

dist, indices = tree.query(source.T)

# remove multiple associatons from index list

# only retain closest associations

unique = False

matched_src = source.copy()

while not unique:

unique = True

for i, idxi in enumerate(indices):

if idxi == -1:

continue

# could do this with np.nonzero

for j in range(i+1,len(indices)):

if idxi == indices[j]:

if dist[i] < dist[j]:

indices[j] = -1

else:

indices[i] = -1

break

# build array of nearest neighbor reference points

# and remove unmatched source points

point_list = []

src_idx = 0

for idx in indices:

if idx != -1:

point_list.append(reference[:,idx])

src_idx += 1

else:

matched_src = np.delete(matched_src, src_idx, axis=1)

matched_ref = np.array(point_list).T

# first find the centroids of both point clouds

src_centroid = source.mean(axis=1)

ref_centroid = reference.mean(axis=1)

# get the point clouds in reference to their centroids

source_centered = source - src_centroid[:, np.newaxis]

reference_centered = reference - ref_centroid[:, np.newaxis]

# compute the moment matrix

M = reference_centered @ source_centered.T

# do the singular value decomposition

U, W, V_t = np.linalg.svd(M)

# get rotation between the two point clouds

R = U @ V_t

if np.linalg.det(R) < 0:

raise RuntimeError('bad rotation matrix')

# translation is the difference between the point clound centroids

t = ref_centroid - R @ src_centroid

T,

color="blue",

frame=None,

axislabel=True,

axissubscript=True,

textcolor=None,

labels=("X", "Y"),

length=1,

arrow=True,

rviz=False,

ax=None,

block=False,

dims=None,

wtl=0.2,

width=1,

d1=0.1,

d2=1.15,

**kwargs

):

"""