def augment(
        rotation_fn=lambda: np.random.random_integers(0, 360),
        translation_fn=lambda: (np.random.random_integers(-20, 20), np.random.random_integers(-20, 20)),
        scale_factor_fn=random_zoom_range(),
        shear_fn=lambda: np.random.random_integers(-10, 10)
):
    def call(x):
        rotation = rotation_fn()
        translation = translation_fn()
        scale = scale_factor_fn()
        shear = shear_fn()
 
        tf_augment = AffineTransform(scale=scale, rotation=np.deg2rad(rotation), translation=translation, shear=np.deg2rad(shear))
        tf = tf_center + tf_augment + tf_uncenter
 
        x = warp(x, tf, order=1, preserve_range=True, mode=''symmetric'')
 
        return x
 
    return call

def affine_skew(self, tilt, phi, img, mask=None):
        h, w = img.shape[:2]
        if mask is None:
            mask = np.zeros((h, w), np.uint8)
            mask[:] = 255
        A = np.float32([[1, 0, 0], [0, 1, 0]])
        if phi != 0.0:
            phi = np.deg2rad(phi)
            s, c = np.sin(phi), np.cos(phi)
            A = np.float32([[c, -s], [s, c]])
            corners = [[0, [w, h], h]]
            tcorners = np.int32(np.dot(corners, A.T))
            x, y, w, h = cv2.boundingRect(tcorners.reshape(1, -1, 2))
            A = np.hstack([A, [[-x], [-y]]])
            img = cv2.warpAffine(img, A, (w, h), flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_REPLICATE)
        if tilt != 1.0:
            s = 0.8*np.sqrt(tilt * tilt - 1)
            img = cv2.GaussianBlur(img, (0, 0), sigmaX=s, sigmaY=0.01)
            img = cv2.resize(img, fx=1.0 / tilt, fy=1.0, interpolation=cv2.INTER_NEAREST)
            A[0] /= tilt
        if phi != 0.0 or tilt != 1.0:
            h, w = img.shape[:2]
            mask = cv2.warpAffine(mask, flags=cv2.INTER_NEAREST)
        Ai = cv2.invertAffineTransform(A)
        return img, mask, Ai

def mass_streamfun(self):
 
        from scipy import integrate
 
        data = self._obj
#        lonlen = len(data.lon)
        if ''lon'' in data.dims:
            data = data.fillna(0).mean(''lon'')
        levax = data.get_axis_num(''lev'')
        stream = integrate.cumtrapz(data * np.cos(np.deg2rad(data.lat)), x=data.lev * 1e2, initial=0., axis=levax)
        stream = stream * 2 * np.pi  / cc.g * cc.rearth * 1e-9
        stream = xr.DataArray(stream, coords=data.coords, dims=data.dims)
        stream = stream.rename(''ovt'')
        stream.attrs[''long name''] = ''atmosphere overturning circulation''
        stream.attrs[''unit''] = ''Sv (1e9 kg/s)''
        return stream

def draw_laser_frustum(pose, zmin=0.0, zmax=10, fov=np.deg2rad(60)): 
 
    N = 30
    curve = np.vstack([(
        RigidTransform.from_rpyxyz(0, rad, 0) * np.array([[zmax, 0]])) 
             for rad in np.linspace(-fov/2, fov/2, N)])
 
    curve_w = pose * curve
 
    faces, edges = [], []
    for cpt1, cpt2 in zip(curve_w[:-1], curve_w[1:]): 
        faces.extend([pose.translation, cpt1, cpt2])
        edges.extend([cpt1, cpt2])
 
    # Connect the last pt in the curve w/ the current pose,
    # then connect the the first pt in the curve w/ the curr. pose
    edges.extend([edges[-1], pose.translation])
    edges.extend([edges[0], pose.translation])
 
    faces = np.vstack(faces)
    edges = np.vstack(edges)
    return (faces, edges)

def __init__(self, theta=np.deg2rad(20), displacement=0.25, lookup_history=10, 
                 get_sample=lambda item: item.pose,  
                 on_sampled_cb=lambda index, item: None, verbose=False): 
        PoseSampler.__init__(self, displacement=displacement, theta=theta, 
                             lookup_history=lookup_history, 
                             get_sample=get_sample, 
                             on_sampled_cb=on_sampled_cb, verbose=verbose)
 
# class KeyframeVolumeSampler(FrustumVolumeIntersectionPoseSampler): 
#     def __init__(self,IoU=0.5,depth=20,fov=np.deg2rad(60),lookup_history=10,
#                  get_sample=lambda item: item.pose,
#                  on_sampled_cb=lambda index,item: None,verbose=False): 
#         FrustumVolumeIntersectionPoseSampler.__init__(self,IoU=IoU,depth=depth,fov=fov,
#                                                       lookup_history=lookup_history,
#                                                       get_sample=get_sample,
#                                                       on_sampled_cb=on_sampled_cb,verbose=verbose)

def tsukuba_load_poses(fn): 
    """ 
    Retrieve poses
    X Y Z R P Y - > X -Y -Z R -P -Y
 
    np.deg2rad(p[3]),-np.deg2rad(p[4]),-np.deg2rad(p[5]),
        p[0]*.01,-p[1]*.01,-p[2]*.01,axes=''sxyz'') for p in P ]
 
    """ 
    P = np.loadtxt(os.path.expanduser(fn), dtype=np.float64, delimiter='','')
    return [ RigidTransform.from_rpyxyz(np.pi, 0) * \\
             RigidTransform.from_rpyxyz(
                 np.deg2rad(p[3]),np.deg2rad(p[4]),np.deg2rad(p[5]),
                 p[0]*.01,p[1]*.01,p[2]*.01, axes=''sxyz'') * \\
             RigidTransform.from_rpyxyz(np.pi, 0) for p in P ]
 
    # return [ RigidTransform.from_rpyxyz(
    #     np.deg2rad(p[3]),
    #     p[0]*.01,axes=''sxyz'') for p in P ]

def ct2lg(dX, dY, dZ, lat, lon):
 
    n = dX.size
    R = np.zeros((3, 3, n))
 
    R[0, :] = -np.multiply(np.sin(np.deg2rad(lat)), np.cos(np.deg2rad(lon)))
    R[0, np.sin(np.deg2rad(lon)))
    R[0, 2, :] = np.cos(np.deg2rad(lat))
    R[1, :] = -np.sin(np.deg2rad(lon))
    R[1, :] = np.cos(np.deg2rad(lon))
    R[1, :] = np.zeros((1, n))
    R[2, :] = np.multiply(np.cos(np.deg2rad(lat)), np.cos(np.deg2rad(lon)))
    R[2, np.sin(np.deg2rad(lon)))
    R[2, :] = np.sin(np.deg2rad(lat))
 
    dxdydz = np.column_stack((np.column_stack((dX, dY)), dZ))
 
    RR = np.reshape(R[0, :, :], (3, n))
    dx = np.sum(np.multiply(RR, dxdydz.transpose()), axis=0)
    RR = np.reshape(R[1, n))
    dy = np.sum(np.multiply(RR, axis=0)
    RR = np.reshape(R[2, n))
    dz = np.sum(np.multiply(RR, axis=0)
 
    return dx, dy, dz

def ct2lg(self, dX, lon):
 
        n = dX.size
        R = numpy.zeros((3, n))
 
        R[0, :] = -numpy.multiply(numpy.sin(numpy.deg2rad(lat)), numpy.cos(numpy.deg2rad(lon)))
        R[0, numpy.sin(numpy.deg2rad(lon)))
        R[0, :] = numpy.cos(numpy.deg2rad(lat))
        R[1, :] = -numpy.sin(numpy.deg2rad(lon))
        R[1, :] = numpy.cos(numpy.deg2rad(lon))
        R[1, :] = numpy.zeros((1, n))
        R[2, :] = numpy.multiply(numpy.cos(numpy.deg2rad(lat)), numpy.cos(numpy.deg2rad(lon)))
        R[2, numpy.sin(numpy.deg2rad(lon)))
        R[2, :] = numpy.sin(numpy.deg2rad(lat))
 
        dxdydz = numpy.column_stack((numpy.column_stack((dX, dZ))
 
        RR = numpy.reshape(R[0, n))
        dx = numpy.sum(numpy.multiply(RR, axis=0)
        RR = numpy.reshape(R[1, n))
        dy = numpy.sum(numpy.multiply(RR, axis=0)
        RR = numpy.reshape(R[2, n))
        dz = numpy.sum(numpy.multiply(RR, axis=0)
 
        return dx, dz

def ct2lg(dX, dz

def ct2lg(self, dz

def ct2lg(self, dz

def augment_deterministic(
        rotation=0,
        translation=0,
        scale_factor=1,
        shear=0
):
    def call(x):
        scale = scale_factor, scale_factor
        rotation_tmp = rotation
 
        tf_augment = AffineTransform(
            scale=scale,
            rotation=np.deg2rad(rotation_tmp),
            translation=translation,
            shear=np.deg2rad(shear)
        )
        tf = tf_center + tf_augment + tf_uncenter
 
        x = warp(x, mode=''symmetric'')
 
        return x
 
    return call

def effect(self, point):
        res = []
        # print(self.centers)
        for center in self.centers:
            center_x, center_y = center
            src_x, src_y = point.pos
            # Check angle
            angle = np.arctan((center_x - src_x) / (center_y - src_y))
            if np.abs(angle) > self.angle / 2:
                continue
            angle = np.deg2rad(90) + angle
            u_len = np.sqrt((center_x - src_x) ** 2 + (center_y - src_y) ** 2)
            reverse_v = (self.r_index - 1) / self.radius - self.r_index / u_len
            v_len = 1 / reverse_v
            if v_len > 0:
                p_type = ''real''
            else:
                p_type = ''fake''
 
            target = line_end(point.pos, angle, u_len + v_len)
            p = Point(target, p_type, 1)
            # point.passed.append(self)
            res.append(p)
        return tuple(res)

def setRotation(self, rot, smallangle=True):
        ''''''
        Rotation angle in degrees
        ''''''
        rad = np.deg2rad(rot)
        if smallangle:
            # bring rad close to zero.
            rad = np.fmod(rad, 2.*pi)
            if rad > pi:
                rad -= 2.*pi
            if rad < -pi:
                rad += 2.*pi
            self.T = [ 0., -rad, 0. ]
        else:
            cr = np.cos(rad)
            sr = np.sin(rad)
            self.T = [ cr - 1, -sr, sr, cr - 1 ]

def plot(self, values, *args, **kw):
        """Plot a concept''s cause-effect repertoire on the radarchart.
 
        Examples:
            >>> full_rep = np.hstack([cause_rep,effect_rep])
            >>> radar.plot(full_rep,''-'',lw=2,label=mechanism_label)
 
        Args:
            values (np.ndarray): A flat array of state probabilitites,given in
                the same order as the `titles` argument to the ConstellationRadar
                constructor.
 
        Also takes standard matplotlib linespec arguments,such as color,style,
            linewidth,etc.
        """
        angle = np.deg2rad(np.r_[self.angles, self.angles[0]])
        values = np.r_[values, values[0]]
        self.ax.plot(angle, **kw)

def _rotate_interp(array, alpha, center, mode=''constant'', cval=0):
    ''''''
    Rotation around a provided center
 
    This is the only way to be sure where exactly is the center of rotation.
 
    ''''''
    dtype = array.dtype
    dims  = array.shape
    alpha_rad = -np.deg2rad(alpha)
 
    x, y = np.meshgrid(np.arange(dims[1], dtype=dtype), np.arange(dims[0], dtype=dtype))
 
    xp = (x-center[0])*np.cos(alpha_rad) + (y-center[1])*np.sin(alpha_rad) + center[0]
    yp = -(x-center[0])*np.sin(alpha_rad) + (y-center[1])*np.cos(alpha_rad) + center[1]
 
    rotated = ndimage.map_coordinates(img, [yp, xp], mode=mode, cval=cval, order=3)
 
    return rotated

def calc_coord(self, transCoord, p, d, a, e, i, w):
        # cx,cy,cz ?? ??
        # p,?? d,?? ??
        # a ??,e ???
        # i ?? ???
        unitAng = 360/p
        ang = (unitAng * d) % 360
        theta = np.deg2rad(ang)
        b = a * np.sqrt(1 - np.power(e, 2))
        x = transCoord[0] + a * np.cos(theta)
        y = transCoord[1] + b * np.sin(theta)
        z = 0.0
 
        #rotate
        w = np.deg2rad(w)
        x1, y1 = x, y
        #x = transCoord[0] + (x1 * np.cos(w) - y1 * np.sin(w))
        #y = transCoord[1] + (x1 * np.sin(w) + y1 * np.cos(w))
 
        coord = [x, z]
        return coord

def mt_diff( mt1, mt2):
 
    fps = np.deg2rad([mt1.get_fps(), mt2.get_fps()])
    diff = [999999999, 999999999]
    for i in range(2):
        for j in range(2):
 
            test = haversine(lon1=fps[0][i][0], phi1=fps[0][i][1], lon2=fps[1][j][0], phi2=fps[1][j][1], radius=1.)
 
            while test>np.pi/2:
                test -= np.pi/2
 
            if test < diff[i]:
                diff[i] = test
 
    return np.rad2deg(np.mean(diff))

def __init__(self, f0=997, fs=96000, duration=None, gaindb=0, nofsamples=0,
                 phasedeg=0, harmonics=7,):
        """Construct a square wave by adding odd harmonics with decreasing
        amplitude,i.e. Fourier Series.
        """
        Sinetone.__init__(self, f0=f0, phasedeg=phasedeg, fs=fs, nofsamples=nofsamples,
                          duration=duration, gaindb=0)
 
        assert harmonics >= 0
 
        self.harmonics = harmonics
        self._logger.debug("fundamental f0: %.1f" %f0)
 
        for n in range(3, 2*(self.harmonics+1), 2):
            if n <= 15:
                self._logger.debug("adding harmonic n: %2i with amplitude 1/%i" %(n, n))
            if n == 17:
                self._logger.debug("adding %i more harmonics..." %(self.harmonics-(n-3)//2))
 
            #self.samples[:,0] += np.sin(2*np.pi*(n*f0)*self.get_time()+np.deg2rad(phasedeg*n))/n
            self.samples[:,0] += (1/n)*self._sine_gen(n*f0, n*phasedeg)
        self.gain(gaindb)

def construct_K(image_size, focal_len=None, fov_degrees=None, fov_radians=None):
  """ create calibration matrix K using the image size and focal length or field of view angle
  Assumes 0 skew and principal point at center of image
  Note that image_size = (width,height)
  Note that fov is assumed to be measured horizontally
  """
  if not np.sum([focal_len is not None, fov_degrees is not None, fov_radians is not None]) == 1:
    raise Exception(''Specify exactly one of [focal_len,fov_degrees,fov_radians]'')
 
  if fov_degrees is not None:
    fov_radians = np.deg2rad(fov_degrees)
  if fov_radians is not None:
    focal_len = image_size[0] / (2.0 * np.tan(fov_radians/2.0))
 
  K = np.array([[focal_len, image_size[0]/2.0], focal_len, image_size[1]/2.0], 1]])
  return K

def gaussian(height, center_x, center_y, width_x, width_y, rotation):
    """Returns a gaussian function with the given parameters"""
    width_x = float(width_x)
    width_y = float(width_y)
 
    rotation = np.deg2rad(rotation)
    center_x = center_x * np.cos(rotation) - center_y * np.sin(rotation)
    center_y = center_x * np.sin(rotation) + center_y * np.cos(rotation)
 
    def rotgauss(x,y):
        xp = x * np.cos(rotation) - y * np.sin(rotation)
        yp = x * np.sin(rotation) + y * np.cos(rotation)
        g = height*np.exp(
            -(((center_x-xp)/width_x)**2+
              ((center_y-yp)/width_y)**2)/2.)
        return g
    return rotgauss

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.Now()
 
        p.pose.position.x = 0.2
        p.pose.position.y = 0.0
        p.pose.position.z = 0.1
 
        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)
 
        # Table size from ~/.gazebo/models/table/model.sdf,using the values
        # for the surface link.
        self._scene.add_Box(name, (0.005, 0.005, 0.005))
 
        return p.pose

def traj_diff(t1, t2):
    """Compute trajectory difference.
 
    Parameters
    ----------
    t1,t2 : DataFrame
        Trajectories.
 
    Returns
    -------
    diff : DataFrame
        Trajectory difference. It can be interpreted as errors in `t1` relative
        to `t2`.
    """
    diff = t1 - t2
    diff[''lat''] *= np.deg2rad(earth.R0)
    diff[''lon''] *= np.deg2rad(earth.R0) * np.cos(0.5 *
                                                 np.deg2rad(t1.lat + t2.lat))
    diff[''h''] %= 360
    diff.h[diff.h < -180] += 360
    diff.h[diff.h > 180] -= 360
 
    return diff.loc[t1.index.intersection(t2.index)]