result[0:2, 2] = SO2.wedge(1).dot(p)
return result
elif len(p) == cls.dim:
result = np.zeros([3, 3])
result[0:2, 0:2] = p[2] * np.eye(2)
After Change
@classmethod
def odot(cls, p, **kwargs):
SE(2) \odot operator as defined by Barfoot.
p = np.atleast_2d(p)
result = np.zeros([p.shape[0], p.shape[1], cls.dof])
if p.shape[1] == cls.dim - 1:
// Assume scale parameter is 1 unless p is a direction
// vector, in which case the scale is 0
scale_is_zero = kwargs.get("directional", False)
if not scale_is_zero:
result[:, 0:2, 0:2] = np.eye(2)
result[:, 0:2, 2] = SO2.wedge(1).dot(p.T).T
elif p.shape[1] == cls.dim:
result[:, 0:2, 0:2] = p[:, 2] * np.eye(2)
result[:, 0:2, 2] = SO2.wedge(1).dot(p[:, 0:2].T).T
else:
raise ValueError("p must have shape ({},), ({},), (N,{}) or (N,{})".format(
cls.dim - 1, cls.dim, cls.dim - 1, cls.dim))
return np.squeeze(result)
@classmethod
def exp(cls, xi):
Exponential map for SE(2).
