def erf(xgoal, x):
"""
Returns error e given two states xgoal and x.
Angle differences are taken properly on SO3.
"""
e = xgoal - x
c = np.cos(x[2])
s = np.sin(x[2])
cg = np.cos(xgoal[2])
sg = np.sin(xgoal[2])
e[2] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
return e
################################################# OBJECTIVES
# Initial condition and goal
python类arctan2()的实例源码
def erf(xgoal, x):
"""
Returns error e given two states xgoal and x.
"""
e = xgoal - x
c = np.cos(x[2])
s = np.sin(x[2])
cg = np.cos(xgoal[2])
sg = np.sin(xgoal[2])
e[2] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
return e
################################################# OBJECTIVES
# Initial condition and goal
def erf(xgoal, x):
"""
Returns error e given two states xgoal and x.
Angle differences are taken properly on SO3.
"""
e = xgoal - x
c = np.cos(x[2])
s = np.sin(x[2])
cg = np.cos(xgoal[2])
sg = np.sin(xgoal[2])
e[2] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
return e
################################################# OBJECTIVES
# Initial condition and goal
def erf(qgoal, q):
"""
Returns error e given two states qgoal and xq.
"""
e = qgoal - q
for i in [0, 1]:
c = np.cos(q[i])
s = np.sin(q[i])
cg = np.cos(qgoal[i])
sg = np.sin(qgoal[i])
e[i] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
return e
################################################# OBJECTIVES AND CONSTRAINTS
# What is goal
def SortByAngle(kNearestPoints, currentPoint, prevPoint):
''' Sorts the k nearest points given by angle '''
angles = np.zeros(kNearestPoints.shape[0])
i = 0
for NearestPoint in kNearestPoints:
# calculate the angle
angle = np.arctan2(NearestPoint[1]-currentPoint[1],
NearestPoint[0]-currentPoint[0]) - \
np.arctan2(prevPoint[1]-currentPoint[1],
prevPoint[0]-currentPoint[0])
angle = np.rad2deg(angle)
# only positive angles
angle = np.mod(angle+360,360)
#print NearestPoint[0], NearestPoint[1], angle
angles[i] = angle
i=i+1
return kNearestPoints[np.argsort(angles)]
def convert_to_euler(R):
"""Compute the euler angles of this rotation.
Refer to [http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf]"""
alpha, beta, gamma = 0, 0, 0
if not np.isclose(np.abs(R[2,0]), 1):
beta = - np.arcsin(R[2,0])
alpha = np.arctan2(R[2,1] / np.cos(beta), R[2,2] / np.cos(beta))
gamma = np.arctan2(R[1,0] / np.cos(beta), R[0,0] / np.cos(beta))
else:
gamma = 0
if np.isclose(R[2,0], -1):
beta = np.pi / 2
alpha = gamma + np.arctan2(R[0,1], R[0,2])
else:
beta = - np.pi / 2
alpha = - gamma + np.arctan2(-R[0,1], -R[0,2])
return np.array([alpha, beta, gamma])
def newmeans(datapointwts,seeds,theta):
newseeds = []; cost = 0; avgspeed = []; pointsperseed = [];
cluster, p2cluster = point2cluster(datapointwts, seeds,theta);
for cd in cluster:
if len(cluster[cd])>0:
hh = np.arctan2(sum([np.sin(xx[2]/360*2*np.pi) for xx in cluster[cd]]),sum([np.cos(xx[2]/360*2*np.pi) for xx in cluster[cd]]))*180/np.pi
newseeds.append((np.mean([xx[0] for xx in cluster[cd]]),np.mean([xx[1] for xx in cluster[cd]]),hh))
hh = [xx[3] for xx in cluster[cd] if xx[3]>0];
if len(hh)<1:
hh = [0]
avgspeed.append(np.mean(hh))
cost = cost+sum([taxidist(xx,newseeds[-1],theta) for xx in cluster[cd]])
else:
newseeds.append(seeds[cd])
avgspeed.append(0)
pointsperseed.append(len(cluster[cd]))
return(newseeds,cost,avgspeed,pointsperseed)
def _cart2polar(x, y):
"""
Transform Cartesian coordinates to polar
Parameters
----------
x, y : floats or arrays
Cartesian coordinates
Returns
-------
r, theta : floats or arrays
Polar coordinates
"""
r = np.sqrt(x**2 + y**2)
theta = np.arctan2(x, y) # ? referenced to vertical
return r, theta
def angle_map(self):
'''Returns a map of the angle for each pixel (w.r.t. origin).
0 degrees is vertical, +90 degrees is right, -90 degrees is left.'''
if self.angle_map_data is not None:
return self.angle_map_data
x = (np.arange(self.width) - self.x0)
y = (np.arange(self.height) - self.y0)
X,Y = np.meshgrid(x,y)
#M = np.degrees(np.arctan2(Y, X))
# Note intentional inversion of the usual (x,y) convention.
# This is so that 0 degrees is vertical.
#M = np.degrees(np.arctan2(X, Y))
# TODO: Lookup some internal parameter to determine direction
# of normal. (This is what should befine the angle convention.)
M = np.degrees(np.arctan2(X, -Y))
self.angle_map_data = M
return self.angle_map_data
def test_output_equation_function_kwarg():
with pytest.raises(ValueError, match=zero_dim_output_msg):
DynamicalSystem(input_=x)
args = np.random.rand(len(x)+1)
sys = DynamicalSystem(state=x,
state_equation=state_equation,
constants_values=constants)
npt.assert_allclose(
sys.output_equation_function(args[0], args[1:]).squeeze(),
args[1:]
)
sys = DynamicalSystem(state=x,
state_equation=state_equation,
output_equation=output_equation,
constants_values=constants)
npt.assert_allclose(
sys.output_equation_function(args[0], args[1:]).squeeze(),
np.r_[args[1]**2 + args[2]**2, np.arctan2(args[2], args[1])]
)
def calculate_sift_grid(self, image, gridH, gridW):
H, W = image.shape
Npatches = gridH.size
feaArr = np.zeros((Npatches, Nsamples * Nangles))
# calculate gradient
GH, GW = gen_dgauss(self.sigma)
IH = signal.convolve2d(image, GH, mode='same')
IW = signal.convolve2d(image, GW, mode='same')
Imag = np.sqrt(IH ** 2 + IW ** 2)
Itheta = np.arctan2(IH, IW)
Iorient = np.zeros((Nangles, H, W))
for i in range(Nangles):
Iorient[i] = Imag * np.maximum(np.cos(Itheta - angles[i]) ** alpha, 0)
for i in range(Npatches):
currFeature = np.zeros((Nangles, Nsamples))
for j in range(Nangles):
currFeature[j] = np.dot(self.weights, \
Iorient[j, gridH[i]:gridH[i] + self.pS, gridW[i]:gridW[i] + self.pS].flatten())
feaArr[i] = currFeature.flatten()
return feaArr
def extract_sift_patches(self, image, gridH, gridW):
# extracts the sift descriptor of patches
# in positions (gridH,gridW) in the image
H, W = image.shape
Npatches = gridH.size
feaArr = np.zeros((Npatches, Nsamples * Nangles))
# calculate gradient
GH, GW = gen_dgauss(self.sigma)
IH = signal.convolve2d(image, GH, mode='same')
IW = signal.convolve2d(image, GW, mode='same')
Imag = np.sqrt(IH ** 2 + IW ** 2)
Itheta = np.arctan2(IH, IW)
Iorient = np.zeros((Nangles, H, W))
for i in range(Nangles):
Iorient[i] = Imag * np.maximum(np.cos(Itheta - angles[i]) ** alpha, 0)
for i in range(Npatches):
currFeature = np.zeros((Nangles, Nsamples))
for j in range(Nangles):
currFeature[j] = np.dot(self.weights, \
Iorient[j, gridH[i]:gridH[i] + self.pS, gridW[i]:gridW[i] + self.pS].flatten())
feaArr[i] = currFeature.flatten()
# feaArr contains each descriptor in a row
feaArr = self.normalize_sift(feaArr)
return feaArr
def make_cosine_basis(self):
'''Makes a spatial cosine and sine basis.
Returns:
list: A list where each entry is a 2D array of size
:data:`(patch_size, patch_size)` specifing the spatial basis.
'''
patch_size = self.patch_size
cosine_mask = np.zeros((patch_size, patch_size))
sine_mask = np.zeros((patch_size, patch_size))
for row in np.arange(patch_size):
for col in np.arange(patch_size):
theta = np.arctan2(patch_size / 2 - row, col - patch_size / 2)
cosine_mask[row, col] = np.cos(theta)
sine_mask[row, col] = np.sin(theta)
spatial_basis = list()
spatial_basis.append(cosine_mask)
spatial_basis.append(sine_mask)
return spatial_basis
def compute_grad(self):
"""
precompute gradient's magnitude and angle of pyramid
where angle is between (0, 2?)
"""
for oct_ind, layer_ind, layer in self.enumerate():
# todo: better kernel can be used?
grad_x = cv2.filter2D(layer, cv2.CV_64F, np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]]))
grad_y = cv2.filter2D(layer, cv2.CV_64F, np.array([[-1, -2, -1], [0, 0, 0], [1, 2, 1]]))
grad_mag = np.sqrt(grad_x**2 + grad_y**2)
grad_ang = np.arctan2(grad_y, grad_x) # each element in (-?, ?)
grad_ang %= TAU # (-?, 0) is moved to (?, 2*?)
self._grad_mag[oct_ind][layer_ind] = grad_mag
self._grad_ang[oct_ind][layer_ind] = grad_ang
def process_coords_for_computations(self, coords_for_computations, t):
"""
"""
if self._teffext:
return coords_for_computations
x, y, z, r = coords_for_computations[:,0], coords_for_computations[:,1], coords_for_computations[:,2], np.sqrt((coords_for_computations**2).sum(axis=1))
theta = np.arccos(z/r)
phi = np.arctan2(y, x)
xi_r = self._radamp * Y(self._m, self._l, theta, phi) * np.exp(-1j*2*np.pi*self._freq*t)
xi_t = self._tanamp * self.dYdtheta(self._m, self._l, theta, phi) * np.exp(-1j*2*np.pi*self._freq*t)
xi_p = self._tanamp/np.sin(theta) * self.dYdphi(self._m, self._l, theta, phi) * np.exp(-1j*2*np.pi*self._freq*t)
new_coords = np.zeros(coords_for_computations.shape)
new_coords[:,0] = coords_for_computations[:,0] + xi_r * np.sin(theta) * np.cos(phi)
new_coords[:,1] = coords_for_computations[:,1] + xi_r * np.sin(theta) * np.sin(phi)
new_coords[:,2] = coords_for_computations[:,2] + xi_r * np.cos(theta)
return new_coords
def updateState(self, vessel, connection):
# self.altitude = numpy.linalg.norm(vessel.position(vessel.orbit.body.reference_frame)) # from center of body, not SL
# self.velocity = vessel.flight(vessel.orbit.body.non_rotating_reference_frame).horizontal_speed
# # self.velocity = math.sqrt(square(vessel.flight(vessel.orbit.body.non_rotating_reference_frame).speed) - square(vessel.flight(vessel.orbit.body.non_rotating_reference_frame).vertical_speed))
# self.verticalVelocity = vessel.flight(vessel.orbit.body.non_rotating_reference_frame).vertical_speed
self.angle = numpy.arctan2(self.velocity(), self.verticalVelocity())
self.thrust = 0.0
for engine in self.engineList:
self.thrust = self.thrust + engine.engine.max_thrust
self.acceleration = self.thrust / vessel.mass
if self.isp <= 0:
self.isp = self.StageVacuumSpecificImpulse(self.insertionStage)
if self.thrust > 0:
self.exhaustVelocity = self.isp * 9.80665# / self.thrust
else:
self.exhaustVelocity = 0.01
if self.mu <= 0.0:
self.mu = vessel.orbit.body.gravitational_parameter
def updateState(self, vessel, connection):
# self.altitude = numpy.linalg.norm(vessel.position(vessel.orbit.body.reference_frame)) # from center of body, not SL
# self.velocity = vessel.flight(vessel.orbit.body.non_rotating_reference_frame).horizontal_speed
# # self.velocity = math.sqrt(square(vessel.flight(vessel.orbit.body.non_rotating_reference_frame).speed) - square(vessel.flight(vessel.orbit.body.non_rotating_reference_frame).vertical_speed))
# self.verticalVelocity = vessel.flight(vessel.orbit.body.non_rotating_reference_frame).vertical_speed
self.angle = numpy.arctan2(self.velocity, self.verticalVelocity)
self.thrust = 0.0
for engine in self.engineList:
self.thrust = self.thrust + engine.thrust
self.acceleration = self.thrust / vessel.mass
if self.isp <= 0:
self.isp = self.StageVacuumSpecificImpulse(self.insertionStage)
if self.thrust > 0:
self.exhaustVelocity = self.isp * 9.80665# / self.thrust
else:
self.exhaustVelocity = 0.01
if self.mu <= 0.0:
self.mu = vessel.orbit.body.gravitational_parameter
def azimuth(_lAz_data):
_inc = _lAz_data[0]
_lat = _lAz_data[1]
velocity_eq = _lAz_data[2]
@jit(nopython=True)
def _az_calc():
inert_az = np.arcsin(max(min(np.cos(np.deg2rad(_inc)) / np.cos(np.deg2rad(_lat)), 1), -1))
_VXRot = _lAz_data[3] * np.sin(inert_az) - velocity_eq * np.cos(np.deg2rad(_lat))
_VYRot = _lAz_data[3] * np.cos(inert_az)
return np.rad2deg(np.fmod(np.arctan2(_VXRot, _VYRot) + (2 * pi), (2 * pi)))
_az = _az_calc()
if _lAz_data[4] == "Ascending": return _az
if _lAz_data[4] == "Descending":
if _az <= 90: return 180 - _az
elif _az >= 270: return 540 - _az
