784586f02d063d39360a287422a6384f30f10d12,liegroups/se3.py,SE3,odot,#Any#Any#,66

Before Change

    @classmethod
    def odot(cls, p, **kwargs):
        SE(3) \odot operator as defined by Barfoot.
        if len(p) == cls.dim - 1:
            result = np.zeros([3, 6])

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

            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

        else:
            raise ValueError("p must have dimension 3 or 4")

    @classmethod
    def vee(cls, Xi):
        SE(3) vee operator as defined by Barfoot.

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

Instances