return result
elif len(p) == cls.dim:
result = np.zeros([4, 6])
result[0:3, 0:3] = p[3] * np.eye(3)
result[0:3, 3:6] = -SO3.wedge(p[0:3])
return result
After Change
@classmethod
def odot(cls, p, **kwargs):
SE(3) \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 otherwise 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:3, 0:3] = np.eye(3)
result[:, 0:3, 3:6] = -SO3.wedge(p)
elif p.shape[1] == cls.dim:
result[:, 0:3, 0:3] = p[:, 3] * np.eye(3)
result[:, 0:3, 3:6] = -SO3.wedge(p[:, 0:3])
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
