784586f02d063d39360a287422a6384f30f10d12,liegroups/se2.py,SE2,odot,#Any#Any#,77

Before Change

            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).

Instances