def get_movement_from_cont(controls, pose):
"""Calculates new pose from controller-input ans returns it as a list
`controls`:dict inputs from controller
`currentPose`:list poselist of current pose"""
# 0Z---> y
# |
# V x
dof = len(pose)
if dof < 6:
pose += [0] * (6 - dof)
# Create a copy so we do not save state here
pose = list(pose)
# speedfactors
rot_fac = 0.25
trans_fac = 1
# add controller inputs to values
pose[0] += controls['LS_UD'] * trans_fac
pose[1] += controls['LS_LR'] * trans_fac
pose[2] += controls['RS_UD'] * trans_fac * -1
pose[3] = deg(pose[3]) + (controls['LEFT'] - controls['RIGHT']) * rot_fac
pose[4] = deg(pose[4]) + (controls['DOWN'] - controls['UP']) * rot_fac
pose[5] = deg(pose[5]) + (controls['L1'] - controls['R1']) * rot_fac
pose[3] = rad(pose[3])
pose[4] = rad(pose[4])
pose[5] = rad(pose[5])
return pose
def get_subpoint(self, t):
pos = self.get_pos(t)
# Converts from mjd
jd = t + 2400000.5
tu = jd - 2451545.0
# Calculates era and decides current rotation of earth
era = (0.7790572732640 + 1.00273781191135448 * tu)
rot = 2 * pi * (era % 1)
# Calculates latitude and reference longitude
lat = atan2(pos.z, sqrt(pos.x**2 + pos.y**2))
reflon = atan2(pos.y, pos.x)
# Adjusts for negative reference longitude
if reflon < 0:
reflon = (2 * pi) + reflon
# Adjusts for longitude above 180 degrees
lon = (reflon - rot) % (2 * pi)
if lon > pi:
lon = lon - (2 * pi)
return deg(lat), deg(lon)
def talker():
rospy.init_node('moonros', anonymous=True) # node name talker
pub = rospy.Publisher('motor_1', UInt8, queue_size=10)
pub1 = rospy.Publisher('motor_2', UInt8, queue_size=10)
rate = rospy.Rate(10) # 10hz send
while not rospy.is_shutdown(): # test if shutdown
now = datetime.datetime.utcnow()
home = ephem.Observer()
home.date = now
home.lat, home.lon = '42.593948', '-81.174415'
moon = ephem.Moon()
moon.compute(home)
moon_azimuth = round(deg(float(moon.az)), 1)
moon_altitude = round(deg(float(moon.alt)), 1)
print("%s" % (now))
print("%s %s" % (moon_altitude, moon_azimuth))
motor_1 = moon_altitude * 160 # for gearbox
motor_2 = moon_azimuth
pub.publish(motor_1)
pub1.publish(motor_2)
rate.sleep()
def mainloop():
rospy.init_node('moonros', anonymous=True) # node name talker
pub = rospy.Publisher('motor_1', Int16, queue_size=10)
pub1 = rospy.Publisher('motor_2', Int16, queue_size=10)
rate = rospy.Rate(0.5) # hz send
while not rospy.is_shutdown(): # test if shutdown
now = datetime.datetime.utcnow()
home = ephem.Observer()
home.date = now
if z1 == 0:
home.lat, home.lon = '42.593948', '-81.174415'
else:
home.lat = lat
home.lon = lon
sun = ephem.Sun()
sun.compute(home)
sun_azimuth = round(deg(float(sun.az)), 0)
sun_altitude = round(deg(float(sun.alt)), 0)
motor_1 = sun_altitude # for gearbox
if sun_azimuth > 180:
motor_2 = sun_azimuth - 360
else:
motor_2 = sun_azimuth
pub.publish(motor_1)
pub1.publish(motor_2)
hello_str = "azal%s %s" % (motor_1, motor_2)
rospy.loginfo(hello_str)
rospy.Subscriber('GPSlocation', Vector3, callback0)
rate.sleep()
def calcSunTimes(onDate):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
onDate is a datetime object
The results are returned as a dict
{sunrise, sunset, solarnoon}
"""
timezone = 1 # in hours, east is positive
longitude = 4.257188 # in decimal degrees, east is positive #default coordinates for Scheveningen, NL
latitude = 52.10155 # in decimal degrees, north is positive
time = 0.5 # percentage past midnight, i.e. noon is 0.5
daysDelta = onDate - datetime.datetime(1900, 1, 1, 0, 0)
day = daysDelta.days # daynumber 1=1/1/1900
Jday = day + 2415018.5 + time - timezone / 24 # Julian day
Jcent = (Jday - 2451545) / 36525 # Julian century
Manom = 357.52911 + Jcent * (35999.05029 - 0.0001537 * Jcent)
Mlong = 280.46646 + Jcent * (36000.76983 + Jcent * 0.0003032) % 360
Eccent = 0.016708634 - Jcent * (0.000042037 + 0.0001537 * Jcent)
Mobliq = 23 + (26 + ((21.448 - Jcent *
(46.815 + Jcent *
(0.00059 - Jcent * 0.001813)))) / 60) / 60
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * Jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = sin(rad(Manom)) * (
1.914602 - Jcent *
(0.004817 + 0.000014 * Jcent)) + sin(rad(2 * Manom)) * (
0.019993 - 0.000101 * Jcent) + sin(rad(3 * Manom)) * 0.000289
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569 - 0.00478 * sin(
rad(125.04 - 1934.136 * Jcent))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(vary * sin(2 * rad(Mlong)) -
2 * Eccent * sin(rad(Manom)) + 4 * Eccent * vary *
sin(rad(Manom)) * cos(2 * rad(Mlong)) -
0.5 * vary * vary * sin(4 * rad(Mlong)) -
1.25 * Eccent * Eccent * sin(2 * rad(Manom)))
hourangle = deg(
acos(
cos(rad(90.833)) / (cos(rad(latitude)) * cos(rad(declination))) -
tan(rad(latitude)) * tan(rad(declination))))
solarnoon = (720 - 4 * longitude - eqtime + timezone * 60) / 1440
sunrise = solarnoon - hourangle * 4 / 1440
sunset = solarnoon + hourangle * 4 / 1440
return {
'solarnoon': timefromdecimalday(solarnoon),
'sunrise': timefromdecimalday(sunrise),
'sunset': timefromdecimalday(sunset)
}
def add_euler_angle_updaters(self, euler_angles):
theta, phi, gamma = euler_angles
frame = self.camera.frame
# why doesn't this work in a loop? Add this method to source?
theta.add_updater(lambda t: t.set_value(deg(frame.euler_angles[0])))
phi.add_updater(lambda p: p.set_value(deg(frame.euler_angles[1])))
gamma.add_updater(lambda g: g.set_value(deg(frame.euler_angles[2])))
self.add(theta, phi, gamma)
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
timezone = self.timezone # in hours, east is positive
longitude = self.long # in decimal degrees, east is positive
latitude = self.lat # in decimal degrees, north is positive
time = self.time # percentage past midnight, i.e. noon is 0.5
day = self.day # daynumber 1=1/1/1900
jday = day + 2415018.5 + time - timezone / 24 # Julian day
jcent = (jday - 2451545) / 36525 # Julian century
# wizardry
manom = 357.52911 + jcent * (35999.05029 - 0.0001537 * jcent)
mlong = 280.46646 + jcent * (36000.76983 + jcent * 0.0003032) % 360
eccent = 0.016708634 - jcent * (0.000042037 + 0.0001537 * jcent)
mobliq = 23 + (26 + ((21.448 - jcent *
(46.815 + jcent *
(0.00059 - jcent * 0.001813)))) / 60) / 60
obliq = mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
seqcent = sin(rad(manom)) * (
1.914602 - jcent * (0.004817 + 0.000014 * jcent)) + \
sin(rad(2 * manom)) * (
0.019993 - 0.000101 * jcent) + sin(rad(3 * manom)) * 0.000289
struelong = mlong + seqcent
sapplong = struelong - 0.00569 - 0.00478 * \
sin(rad(125.04 - 1934.136 * jcent))
declination = deg(asin(sin(rad(obliq)) * sin(rad(sapplong))))
eqtime = 4 * deg(vary * sin(2 * rad(mlong)) -
2 * eccent * sin(rad(manom)) + 4 * eccent * vary *
sin(rad(manom)) * cos(2 * rad(mlong)) -
0.5 * vary * vary * sin(4 * rad(mlong)) -
1.25 * eccent * eccent * sin(2 * rad(manom)))
angle = (cos(rad(90.833)) /
(cos(rad(latitude)) * cos(rad(declination))) -
tan(rad(latitude)) * tan(rad(declination)))
self.solarnoon_t = (720 - 4 * longitude - eqtime +
timezone * 60) / 1440
if angle > 1.0:
self.up_all_day = True
return
elif angle < -1.0:
self.down_all_day = True
return
else:
hourangle = deg(acos(angle))
self.sunrise_t = self.solarnoon_t - hourangle * 4 / 1440
self.sunset_t = self.solarnoon_t + hourangle * 4 / 1440
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
timezone = self.timezone # in hours, east is positive
longitude = self.long # in decimal degrees, east is positive
latitude = self.lat # in decimal degrees, north is positive
time = self.time # percentage past midnight, i.e. noon is 0.5
day = self.day # daynumber 1=1/1/1900
Jday = day + 2415018.5 + time - timezone / 24 # Julian day
Jcent = (Jday - 2451545) / 36525 # Julian century
Manom = 357.52911 + Jcent * (35999.05029 - 0.0001537 * Jcent)
Mlong = 280.46646 + Jcent * (36000.76983 + Jcent * 0.0003032) % 360
Eccent = 0.016708634 - Jcent * (0.000042037 + 0.0001537 * Jcent)
Mobliq = (
23
+ (26 + ((21.448 - Jcent * (46.815 + Jcent * (0.00059 - Jcent * 0.001813)))) / 60) / 60
)
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * Jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = (
sin(rad(Manom)) * (1.914602 - Jcent * (0.004817 + 0.000014 * Jcent))
+ sin(rad(2 * Manom)) * (0.019993 - 0.000101 * Jcent)
+ sin(rad(3 * Manom)) * 0.000289
)
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569 - 0.00478 * sin(rad(125.04 - 1934.136 * Jcent))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(
vary * sin(2 * rad(Mlong))
- 2 * Eccent * sin(rad(Manom))
+ 4 * Eccent * vary * sin(rad(Manom)) * cos(2 * rad(Mlong))
- 0.5 * vary * vary * sin(4 * rad(Mlong))
- 1.25 * Eccent * Eccent * sin(2 * rad(Manom))
)
hourangle = deg(
acos(
cos(rad(90.833)) / (cos(rad(latitude)) * cos(rad(declination)))
- tan(rad(latitude)) * tan(rad(declination))
)
)
self.solarnoon_t = (720 - 4 * longitude - eqtime + timezone * 60) / 1440
self.sunrise_t = self.solarnoon_t - hourangle * 4 / 1440
self.sunset_t = self.solarnoon_t + hourangle * 4 / 1440
def modify_angle(self, angle: str, degrees: int):
if angle == 'AB':
init_AB = self.AB
self.AB += degrees
if self.AB != init_AB:
self.C = math.sqrt(self.A ** 2 + self.B ** 2 - 2 * self.A * self.B * cos(self.AB))
self.BC = acos((self.B ** 2 + self.C ** 2 - self.A ** 2) / (2 * self.B * self.C))
self.BC = deg(self.BC)
self.CA = acos((self.C ** 2 + self.A ** 2 - self.B ** 2) / (2 * self.C * self.A))
self.CA = deg(self.CA)
print('Angle AB is:', triangle.AB)
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
timezone = self.timezone # in hours, east is positive
longitude = self.long # in decimal degrees, east is positive
latitude = self.lat # in decimal degrees, north is positive
time = self.time # percentage past midnight, i.e. noon is 0.5
day = self.day # daynumber 1=1/1/1900
# NOAA Spreadsheet
Jday = day + 2415018.5 + time - timezone / 24.0 # Julian day
Jcent = (Jday - 2451545) / 36525 # Julian century
Manom = 357.52911 + Jcent * (35999.05029 - 0.0001537 * Jcent)
Mlong = 280.46646 + Jcent * (36000.76983 + Jcent * 0.0003032) % 360
Eccent = 0.016708634 - Jcent * (0.000042037 + 0.0001537 * Jcent)
Mobliq = 23 + (26 + ((21.448 - Jcent * (46.815 + Jcent * (0.00059 - Jcent * 0.001813)))) / 60) / 60
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * Jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = (
sin(rad(Manom)) * (1.914602 - Jcent * (0.004817 + 0.000014 * Jcent))
+ sin(rad(2 * Manom)) * (0.019993 - 0.000101 * Jcent)
+ sin(rad(3 * Manom)) * 0.000289
)
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569 - 0.00478 * sin(rad(125.04 - 1934.136 * Jcent))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(
vary * sin(2 * rad(Mlong))
- 2 * Eccent * sin(rad(Manom))
+ 4 * Eccent * vary * sin(rad(Manom)) * cos(2 * rad(Mlong))
- 0.5 * vary * vary * sin(4 * rad(Mlong))
- 1.25 * Eccent * Eccent * sin(2 * rad(Manom))
)
hourangle = deg(
acos(
cos(rad(90.833)) / (cos(rad(latitude)) * cos(rad(declination)))
- tan(rad(latitude)) * tan(rad(declination))
)
)
self.solarnoon_t = (720 - 4 * longitude - eqtime + timezone * 60) / 1440.0
self.sunrise_t = self.solarnoon_t - hourangle * 4 / 1440.0
self.sunset_t = self.solarnoon_t + hourangle * 4 / 1440.0
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
# in hours, east is positive
latitude = self.coord[0]
longitude = self.coord[1]
# daynumber 1=1/1/1900
some_day = self.day
# Julian day
jday = some_day + 2415020.5
# Julian century
jcent = (jday - 2451545) / 36525
Manom = 357.52911 + jcent * (35999.05029 - 0.0001537 * jcent)
Mlong = 280.46646 + jcent * (36000.76983 + jcent * 0.0003032) % 360
Eccent = 0.016708634 - jcent * (0.000042037 + 0.0001537 * jcent)
Mobliq = 23 + (26 + ((21.448 - jcent *
(46.815 + jcent *
(0.00059 - jcent * 0.001813)))) / 60) / 60
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = sin(rad(Manom)) * (
1.914602 - jcent *
(0.004817 + 0.000014 * jcent)) + sin(rad(2 * Manom)) * (
0.019993 - 0.000101 * jcent) + sin(rad(3 * Manom)) * 0.000289
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569 - 0.00478 * sin(
rad(125.04 - (1934.136 * jcent)))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(vary * sin(2 * rad(Mlong)) -
2 * Eccent * sin(rad(Manom)) + 4 * Eccent * vary *
sin(rad(Manom)) * cos(2 * rad(Mlong)) -
0.5 * vary * vary * sin(4 * rad(Mlong)) -
1.25 * Eccent * Eccent * sin(2 * rad(Manom)))
hourangle = deg(
acos(
cos(rad(90.833)) /
(cos(rad(latitude)) * cos(rad(declination))) -
tan(rad(latitude)) * tan(rad(declination))))
self.solarnoon_t = (720 - 4 * longitude -
eqtime) / 1440 + some_day - 25567
self.sunrise_t = self.solarnoon_t - hourangle * 4 / 1440
self.sunset_t = self.solarnoon_t + hourangle * 4 / 1440
def send_cmd_to_arduino(self, x, angular):
now = datetime.datetime.utcnow()
home = ephem.Observer()
home.date = now
home.lat, home.lon = '42.593948', '-81.174415'
moon = ephem.Moon()
moon.compute(home)
moon_azimuth = round(deg(float(moon.az)), 0)
moon_altitude = round(deg(float(moon.alt)), 0)
motor_1 = moon_altitude # for gearbox
motor_2 = moon_azimuth
hello_str = "azal%s %s" % (moon_azimuth, moon_altitude)
rospy.loginfo(hello_str)
def calcSunTimes(onDate):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
onDate is a datetime object
The results are returned as a dict
{sunrise, sunset, solarnoon}
"""
timezone = 1 # in hours, east is positive #+1for european summertime. TODO: find a more elegant solution for this.
longitude= 4.257188 # in decimal degrees, east is positive #default coordinates for Scheveningen, NL
latitude = 52.10155 # in decimal degrees, north is positive
time = 0.5 # percentage past midnight, i.e. noon is 0.5
daysDelta = onDate-datetime.datetime(1900,1,1,0,0)
day = daysDelta.days # daynumber 1=1/1/1900
Jday =day+2415018.5+time-timezone/24 # Julian day
Jcent =(Jday-2451545)/36525 # Julian century
Manom = 357.52911+Jcent*(35999.05029-0.0001537*Jcent)
Mlong = 280.46646+Jcent*(36000.76983+Jcent*0.0003032)%360
Eccent = 0.016708634-Jcent*(0.000042037+0.0001537*Jcent)
Mobliq = 23+(26+((21.448-Jcent*(46.815+Jcent*(0.00059-Jcent*0.001813))))/60)/60
obliq = Mobliq+0.00256*cos(rad(125.04-1934.136*Jcent))
vary = tan(rad(obliq/2))*tan(rad(obliq/2))
Seqcent = sin(rad(Manom))*(1.914602-Jcent*(0.004817+0.000014*Jcent))+sin(rad(2*Manom))*(0.019993-0.000101*Jcent)+sin(rad(3*Manom))*0.000289
Struelong= Mlong+Seqcent
Sapplong = Struelong-0.00569-0.00478*sin(rad(125.04-1934.136*Jcent))
declination = deg(asin(sin(rad(obliq))*sin(rad(Sapplong))))
eqtime = 4*deg(vary*sin(2*rad(Mlong))-2*Eccent*sin(rad(Manom))+4*Eccent*vary*sin(rad(Manom))*cos(2*rad(Mlong))-0.5*vary*vary*sin(4*rad(Mlong))-1.25*Eccent*Eccent*sin(2*rad(Manom)))
hourangle= deg(acos(cos(rad(90.833))/(cos(rad(latitude))*cos(rad(declination)))-tan(rad(latitude))*tan(rad(declination))))
solarnoon =(720-4*longitude-eqtime+timezone*60)/1440
sunrise = solarnoon-hourangle*4/1440
sunset = solarnoon+hourangle*4/1440
print timefromdecimalday(sunrise)
print timefromdecimalday(sunset)
return {
'solarnoon': timefromdecimalday(solarnoon),
'sunrise': timefromdecimalday(sunrise),
'sunset': timefromdecimalday(sunset)
}
def get_euler_angles_group(self):
euler_angles = VGroup(*[
DecimalNumber(deg(angle), num_decimal_places=1)
for angle in self.camera.frame.euler_angles
]).arrange(DOWN).to_corner(UR, buff=0.5)
euler_angles.fix_in_frame()
return euler_angles
def _declination(self):
"""
"""
appLong = self._appLong()
oc = self._obliqCorr()
return deg(asin(sin(rad(oc)) * sin(rad(appLong))))
def calculate_int_waypoint(self, dec_lat, dec_lon, m_dist, dec_brng): lat = rad(dec_lat) lon = rad(dec_lon) brng = rad(dec_brng) dist = m_dist/1000 new_lat = asin(sin(lat)*cos(dist/self.ER) + cos(lat)*sin(dist/self.ER)*cos(brng)) new_lon = lon + atan2(sin(brng)*sin(dist/self.ER)*cos(lat),cos(dist/self.ER) - sin(lat)*sin(new_lat)) norm_new_lon = (new_lon + (3*pi)) % (2*pi) - pi rnd_lat = deg(new_lat) rnd_lon = deg(norm_new_lon) new_latlon = (rnd_lat, rnd_lon); return new_latlon
def table():
g = ephem.Observer()
g.name='Raleigh'
g.lat=rad(35.78) # lat/long in decimal degrees
g.long=rad(-78.64)
moon = ephem.Moon(g)
sun = ephem.Sun(g)
results = []
for h_year in range (1431,1442):
print
um = HijriDate(h_year,9,1)
Hijri_str = "%02d-%02d-%d" % (um.day, um.month, um.year)
print Hijri_str
print " Gregorian: %d/%d/%d" %(um.year_gr, um.month_gr, um.day_gr)
g.date = ephem.Date("%d/%d/%d" %(um.year_gr, um.month_gr, um.day_gr, )) # midnight UTC
g.date = g.next_transit(sun, start=g.next_rising(sun))
print ' solar transit: %s' % datetime.strftime(ephem.localtime(g.date), '%c')
sunset1 = g.next_setting(sun)
print ' sunset: %s' % datetime.strftime(ephem.localtime(sunset1), '%c')
#lunar circumstances at local sunset
g.date = sunset1
moon.compute(g)
print ' lunar alt at sunset: %.1f' % deg(moon.alt)
print 'lunar elong at sunset: %.1f' % deg(moon.elong)
print 'lunar phase at sunset: %.1f%%' % moon.phase
# https://moonsighting.com/ramadan-eid.html
# http://www.fiqhcouncil.org/node/83
if deg(moon.alt) >= 5.0 and deg(moon.elong) >= 8.0:
start = sunset1
else:
start = g.next_setting(sun)
results.append(start)
newmoon2 = ephem.next_new_moon(g.date )
g.date = newmoon2
end = g.next_setting(sun)
print ' new moon : %s UTC' % datetime.strftime(newmoon2.datetime(), '%c')
print ' Ramadan_begin_end start: %s' % datetime.strftime(ephem.localtime(start), '%c')
print ' Ramadan_begin_end end : %s' % datetime.strftime(ephem.localtime(end), '%c')
return results
def build(self, skip_collision_calc=False):
'''
build the PTG vector, by sampling phi at the specified resolution
Warning: Takes a while to complete, around 1 hour on the default configurations
To speed up testing of collision unrelated feature set skip_collision_calc to True
'''
phi_max = self.vehicle.phi_max
for phi in np.arange(-phi_max, phi_max + self.phi_resolution,
self.phi_resolution):
self.config['init_phi'] = min(deg(phi), 30.)
self.config['name'] = '{0}_init_phi = {1:0.1f}'.format(
self.name, deg(phi))
ptg = self.ptg_class(self.vehicle, self.config)
if skip_collision_calc:
ptg.build_cpoints()
else:
ptg.build()
self.ptgs.append(ptg)
def plot_trajectories(self, axes):
for cpoints_at_k in self.cpoints:
x = [cpoint.pose.x for cpoint in cpoints_at_k]
y = [cpoint.pose.y for cpoint in cpoints_at_k]
axes.plot(x,
y,
label=r'$\alpha = {0:.1f}^\circ$'.format(
deg(cpoints_at_k[0].alpha)))
axes.legend(loc='upper left', shadow=True)
def _HaSunrise(self):
"""
"""
declin = self._declination()
rtn = cos(rad(90.833)) / (cos(rad(self._lat)) * cos(rad(declin)))
rtn -= tan(rad(self._lat)) * tan(rad(declin))
return deg(acos(rtn))
def plot_ptg_cpoints(aptg: APTG, init_phi: float):
import matplotlib.pyplot as plt
fig, ax = plt.subplots()
aptg.build(skip_collision_calc=True)
ptg = aptg.ptg_at_phi(init_phi)
ax.title.set_text('Trajectories at $\phi_i = {0}^\circ$ '.format(
deg(init_phi)))
ptg.plot_trajectories(ax)
plt.show()
def get_orbital_elements(r,rdot,t_2,t_0):
rnorm = np.linalg.norm(r)
rdotnorm = np.linalg.norm(rdot)
a = get_a(rnorm,rdot)
n = sqrt(k**2 / a**3)
e = get_e(r,rdot,a)
h = k * np.cross(r,rdot)
hnorm = np.linalg.norm(h)
I = acos(h[2] / hnorm)
Omega = get_Omega(h,I)
omegaf = get_omegaf(r,rnorm,I,Omega)
f = get_f(a,e,r,rdot,rnorm)
omega = get_omega(omegaf,f)
M = get_M(a,e,rnorm,f,t_2,t_0)
I = deg(I) % 360
Omega = deg(Omega) % 360
omega = deg(omega) % 360
M = deg(M) % 360
return a,e,I,Omega,omega,M
def update(self):
while Game.event_queue:
nxt = Game.event_queue.pop()
if nxt.type == MOUSEBUTTONDOWN:
if nxt.button == 1:
self.shoot()
# TODO play sound
allkeys = key.get_pressed()
vdir = hdir = 0
if allkeys[K_w]:
vdir = 1
elif allkeys[K_s]:
vdir = -1
if allkeys[K_a]:
hdir = -1
elif allkeys[K_d]:
hdir = 1
if vdir != 0 or hdir != 0: # if you're applying gas
hdir, vdir = norm((hdir, vdir))
dirneeded = deg(atan2(vdir, hdir)) % 180
self.rot %= 180
if self.rot == dirneeded:
self.vx += hdir * self.acceleration_rate
self.vy -= vdir * self.acceleration_rate
finalspeed = sqrt(self.vx * self.vx + self.vy * self.vy)
if finalspeed > self.max_speed:
self.vx *= self.max_speed / finalspeed
self.vy *= self.max_speed / finalspeed
else:
if self.vx == 0 and self.vy == 0:
self.turn_toward(dirneeded)
else:
self.apply_brakes()
else:
self.apply_brakes()
if self.vx != 0 or self.vy != 0:
self.x += self.vx * Game.delta
self.y += self.vy * Game.delta
self.calculate_vertices()
mpos = mouse.get_pos()
self.set_gunangle(deg(atan2(mpos[1] - self.y, mpos[0] - self.x)))
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
# in hours, east is positive
latitude = self.coord[0]
longitude = self.coord[1]
# daynumber 1=1/1/1900
some_day = self.day
# Julian day
jday = some_day + 2415020.5
# Julian century
jcent = (jday - 2451545) / 36525
Manom = 357.52911 + jcent * (35999.05029 - 0.0001537 * jcent)
Mlong = 280.46646 + jcent * (36000.76983 + jcent * 0.0003032) % 360
Eccent = 0.016708634 - jcent * (0.000042037 + 0.0001537 * jcent)
Mobliq = 23 + (26 + ((21.448 - jcent * (46.815 + jcent * (0.00059 - jcent * 0.001813)))) / 60) / 60
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = sin(rad(Manom)) * (1.914602 - jcent * (0.004817 + 0.000014 * jcent)) + sin(rad(2 * Manom)) * (0.019993 - 0.000101 * jcent) + sin(rad(3 * Manom)) * 0.000289
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569 - 0.00478 * sin(rad(125.04 - (1934.136 * jcent)))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(
vary * sin(2 * rad(Mlong)) - 2 * Eccent * sin(rad(Manom)) + 4 * Eccent * vary * sin(rad(Manom)) * cos(
2 * rad(Mlong)) - 0.5 * vary * vary * sin(4 * rad(Mlong)) - 1.25 * Eccent * Eccent * sin(
2 * rad(Manom)))
hourangle = deg(acos(cos(rad(90.833)) / (cos(rad(latitude)) * cos(rad(declination))) - tan(rad(latitude)) * tan(
rad(declination))))
self.solarnoon_t = (720 - 4 * longitude - eqtime) / 1440 + some_day - 25567
self.sunrise_t = self.solarnoon_t - hourangle * 4 / 1440
self.sunset_t = self.solarnoon_t + hourangle * 4 / 1440
def modify_angle(self, angle: str, degrees: int):
if angle == 'AB':
init_AB = self.AB
self.AB += degrees
if self.AB != init_AB:
self.C = math.sqrt(self.A * 2 + self.B * 2 - 2 * self.A * self.B * cos(self.AB))
self.BC = acos((self.B * 2 + self.C * 2 - self.A ** 2) / (2 * self.B * self.C))
self.BC = deg(self.BC)
self.CA = acos((self.C * 2 + self.A * 2 - self.B ** 2) / (2 * self.C * self.A))
self.CA = deg(self.CA)
print('Angle AB is:', triangle.AB)
elif angle == 'BC':
init_BC = self.BC
self.BC += degrees
if self.BC != init_BC:
self.A = math.sqrt(self.B * 2 + self.C * 2 - 2 * self.B * self.C * cos(self.BC))
self.AB = acos((self.A * 2 + self.B * 2 - self.C ** 2) / (2 * self.A * self.B))
self.AB = deg(self.AB)
self.CA = acos((self.C * 2 + self.A * 2 - self.B ** 2) / (2 * self.C * self.A))
self.CA = deg(self.CA)
print('Angle BC is:', triangle.BC)
elif angle == 'CA':
init_CA = self.CA
self.CA += degrees
if self.CA != init_CA:
self.B = math.sqrt(self.C * 2 + self.A * 2 - 2 * self.C * self.A * cos(self.CA))
self.AB = acos((self.A * 2 + self.B * 2 - self.C ** 2) / (2 * self.A * self.B))
self.AB = deg(self.AB)
self.BC = acos((self.B * 2 + self.C * 2 - self.A ** 2) / (2 * self.B * self.C))
self.BC = deg(self.BC)
print('Angle CA is:', triangle.CA)
if self.AB + self.BC + self.CA < 0 or self.AB + self.BC + self.CA > 180:
raise Exception('Angle outside interval (0, 180)')
def from_csv(r, rdot, epoch):
# Calculates orbital momentum and eccentricity vector
h = Vector.cross(r, rdot)
ev = Vector.subtract(Vector.divide(Vector.cross(rdot, h), cbody_GM),
Vector.divide(r, Vector.norm(r)))
# Calculates true anomaly and arbitrary vector n
if Vector.dot(r, rdot) >= 0:
v = acos(Vector.dot(ev, r) / (Vector.norm(ev) * Vector.norm(r)))
else:
v = (2 * pi) - acos(
Vector.dot(ev, r) / (Vector.norm(ev) * Vector.norm(r)))
n = Vector(-h.y, h.x, 0)
# Calculates inclination eccentricity and Eccentric anomaly
i = acos(h.z / h.norm())
e = ev.norm()
E = 2 * atan((tan(v / 2)) / sqrt((1 + e) / (1 - e)))
# Calculates longitude of ascending node
if n.y >= 0:
lan = acos(n.x / n.norm())
else:
lan = (2 * pi) - acos(n.x / n.norm())
# Calculates argument of periapsis
if ev.z >= 0:
aop = acos(Vector.dot(n, ev) / (Vector.norm(n) * Vector.norm(ev)))
else:
aop = (2 * pi) - acos(
Vector.dot(n, ev) / (Vector.norm(n) * Vector.norm(ev)))
# Calculates semi-major axis and mean anomaly
a = 1 / ((2 / r.norm()) - ((rdot.norm()**2) / cbody_GM))
if E >= (e * sin(E)):
M = E - (e * sin(E))
else:
M = (2 * pi) + (E - (e * sin(E)))
return Orbit(a, e, deg(i), deg(aop), deg(lan), deg(M), epoch)
def trace_trajectory_at_phi(aptg: APTG, init_phi: float):
# same as trace_trajectory_at_phi_alpha, however all possible alpha values are considered
import matplotlib.pyplot as plt
name = 'trajectories_at_phi_{0:.0f}'.format(deg(init_phi))
grid_size = aptg.vehicle.trailer_l * 4.
aptg.build(skip_collision_calc=True)
ptg = aptg.ptg_at_phi(init_phi)
fig, ax = plt.subplots()
ax.set_xlim([-grid_size, grid_size])
ax.set_ylim([-grid_size, grid_size])
frame = -1
print('plotting trajectories, this will take a while!')
for cpoints_at_alpha in ptg.cpoints:
alpha = cpoints_at_alpha[0].alpha
for cpoint in cpoints_at_alpha:
frame += 1
aptg.vehicle.plot(ax, cpoint.pose, None)
ax.title.set_text(
r'Trajectory at $\phi_i = {0:.1f}^\circ, \alpha = {1:.1f}^\circ$ '
.format(deg(init_phi), deg(alpha)))
plt.savefig('./out/{0}_{1:04d}.png'.format(name, frame))
ax.lines = []
def _eqOfTime(self):
"""
"""
y = tan(rad(self._obliqCorr() / 2))**2
gmls = self._gmls()
gmas = self._gmas()
eeo = self._eeo()
rtn = y * sin(2 * rad(gmls)) - 2 * eeo * sin(rad(gmas))
rtn += 4 * eeo * y * sin(rad(gmas)) * cos(2 * rad(gmls))
rtn -= 0.5 * y**2 * sin(4 * rad(gmls)) - 1.25 * eeo**2 * sin(
2 * rad(gmas))
return 4 * deg(rtn)
def trace_trajectory_at_phi_alpha(aptg: APTG, init_phi: float, alpha: float):
# plot the vehicle at each cpoint of the trajectory selected by fixed init_phi and fixed alpha
import matplotlib.pyplot as plt
name = 'trajectories_at_phi_{0:.0f}_alpha_{1:.0f}'.format(
deg(init_phi), deg(alpha))
aptg.build(skip_collision_calc=True)
grid_size = aptg.vehicle.trailer_l * 4.
ptg = aptg.ptg_at_phi(init_phi)
cpoints_at_alpha = ptg.cpoints[ptg.alpha2idx(alpha)]
fig, ax = plt.subplots()
ax.set_xlim([-grid_size, grid_size])
ax.set_ylim([-grid_size, grid_size])
frame = -1
for cpoint in cpoints_at_alpha:
frame += 1
#aptg.vehicle.plot(ax, cpoint.pose, None)
aptg.vehicle.plot(ax, cpoint.pose, 'b')
ax.title.set_text(
r'Trajectory at $\phi_i = {0:.1f}^\circ, \alpha = {1:.1f}^\circ$ '.
format(deg(init_phi), deg(alpha)))
plt.savefig('./out/{0}_{1:04d}.png'.format(name, frame))
ax.lines = []
def AngSolAz(Ang_dcl, Ang_hr, Ang_zth, lat): ''' Approximate Solar Azimuth angle (0=N, 90=E, 180=S, 270=W) computed from 180 + HA From Solar engineering of thermal processes, J.A. Duffie and W.A. Beckman [8]. Yield result in radians to be converted at the end into degrees. ''' Ang_sol_az = (np.sign(Ang_hr) * abs(acos((cos(rad(Ang_zth)) * sin(rad(lat)) - sin(rad(Ang_dcl))) / (sin(rad(Ang_zth)) * cos(rad(lat)))))) return deg(Ang_sol_az)
def moon_path(self):
self.date = datetime.date.today()
result = []
for i in range(24 * 4): # compute position for every 15 minutes
self.moon.compute(self)
next_new_moon = ephem.next_new_moon(self.date)
previous_new_moon = ephem.previous_new_moon(self.date)
lunation = (self.date - previous_new_moon) / (next_new_moon -
previous_new_moon)
row = [
ephem.localtime(self.date).time(),
deg(self.moon.alt),
deg(self.moon.az),
self.date.datetime().replace(tzinfo=pytz.UTC).astimezone(
self.local_timezone).strftime("%H:%M"), self.moon.phase,
lunation
]
result.append(row)
self.date += ephem.minute * 15
return pd.DataFrame(result,
columns=[
'Time', 'Moon altitude', 'Azimuth',
'Local_time', 'Phase', 'Lunation'
])
def AngZth(Ang_dcl, lat, Ang_hr): ''' Angle of Solar Zenith, it is the complement of Solar elevation (or altitude angle, e), From D L Hartman, 1994. Z= 90 - e, cos(Z)=cos(d)cos(L)cos(H)+sin(d)sin(L) (*this last one should be L), e= solar elevation, L= Latitude, d= Declination, H= hour angle For solar altitude the arcsin would be required in stead of arccosine. Yields result in RADIANS converted to degrees. ''' Ang_zth = (acos(cos(rad(Ang_dcl)) * cos(rad(lat)) * cos(rad(Ang_hr)) + sin(rad(Ang_dcl)) * sin(rad(lat)))) return deg(Ang_zth)
def AngDay(date_time): ''' Number of the day, some sources call it julian number day From J.A. Duffie and W.A. Beckman [8]. Yield result in radians but converted to degrees at the end of the function. ''' d = pd.Series(date_time).dt.dayofyear[0] # to get the day of year year = date_time.year # Formula to identify leap years year_days = 366 if ((year % 4 == 0 and year % 100 != 0) or (year % 400 == 0 and year % 100 != 0)) else 365 Ang_dy = 2 * pi * ((d - 1) / year_days) return deg(Ang_dy)
def plot_trajectories(ptg: PTG):
fig, ax = plt.subplots()
for cpoints_at_k in ptg.cpoints:
x = [cpoint.pose.x for cpoint in cpoints_at_k]
y = [cpoint.pose.y for cpoint in cpoints_at_k]
ax.plot(x, y, 'k')
ax.title.set_text('Trajectories at $\phi_i = {0}^\circ$ '.format(
deg(ptg.cpoints[0][0].phi)))
k = ptg.alpha2idx(rad(0.))
x = ptg.cpoints[k][10].x
y = ptg.cpoints[k][10].y
ax.annotate(
r'$\alpha = {0:.1f}^\circ$'.format(0.),
xy=(x, y), # theta, radius
xytext=(x - 0.5, y - 0.5), # fraction, fraction
arrowprops=dict(facecolor='black', shrink=0.05),
horizontalalignment='left',
verticalalignment='bottom',
fontsize=20)
k = ptg.alpha2idx(rad(15.))
x = ptg.cpoints[k][-10].x
y = ptg.cpoints[k][-10].y
ax.annotate(
r'$\alpha = {0:.1f}^\circ$'.format(15),
xy=(x, y), # theta, radius
xytext=(x - 0.5, y - 0.5), # fraction, fraction
arrowprops=dict(facecolor='black', shrink=0.05),
horizontalalignment='left',
verticalalignment='bottom',
fontsize=20)
k = ptg.alpha2idx(rad(30))
x = ptg.cpoints[k][-5].x
y = ptg.cpoints[k][-5].y
ax.annotate(
r'$\alpha = {0:.1f}^\circ$'.format(30),
xy=(x, y), # theta, radius
xytext=(x - 0.5, y - 0.5), # fraction, fraction
arrowprops=dict(facecolor='black', shrink=0.05),
horizontalalignment='left',
verticalalignment='bottom',
fontsize=20)
plt.show()
def __calc(self):
"""
Perform the actual calculations for sunrise, sunset and
a number of related quantities.
The results are stored in the instance variables
sunrise_t, sunset_t and solarnoon_t
"""
when = self.when
# datetime days are numbered in the Gregorian calendar
# while the calculations from NOAA are distibuted as
# OpenOffice spreadsheets with days numbered from
# 1/1/1900. The difference are those numbers taken for
# 18/12/2010
day = when.toordinal() - (734124 - 40529)
t = when.time()
time = (t.hour + t.minute/60.0 + t.second/3600.0)/24.0
longitude = self.long # in decimal degrees, east is positive
latitude = self.lat # in decimal degrees, north is positive
Jday = day + 2415018.5 + time/24 # Julian day
Jcent = (Jday - 2451545) / 36525 # Julian century
Manom = 357.52911 + Jcent * (35999.05029 - 0.0001537 * Jcent)
Mlong = 280.46646 + Jcent * (36000.76983 + Jcent * 0.0003032) % 360
Eccent = 0.016708634 - Jcent * (0.000042037 + 0.0001537 * Jcent)
Mobliq = 23 + (26 + ((21.448 - Jcent * (46.815 + Jcent *\
(0.00059 - Jcent * 0.001813)))) / 60) / 60
obliq = Mobliq + 0.00256 * cos(rad(125.04 - 1934.136 * Jcent))
vary = tan(rad(obliq / 2)) * tan(rad(obliq / 2))
Seqcent = sin(rad(Manom)) * (1.914602 - Jcent *\
(0.004817 + 0.000014 * Jcent)) + sin(rad(2 * Manom)) *\
(0.019993 - 0.000101 * Jcent) + sin(rad(3 * Manom)) * 0.000289
Struelong = Mlong + Seqcent
Sapplong = Struelong - 0.00569-0.00478 * sin(rad(125.04 - 1934.136 * Jcent))
declination = deg(asin(sin(rad(obliq)) * sin(rad(Sapplong))))
eqtime = 4 * deg(vary * sin(2 * rad(Mlong)) - 2 * Eccent *\
sin(rad(Manom)) + 4 * Eccent * vary * sin(rad(Manom)) *\
cos(2*rad(Mlong)) - 0.5 * vary * vary * sin(4 * rad(Mlong)) -\
1.25 * Eccent * Eccent * sin(2 * rad(Manom)))
hourangle = deg(acos(cos(rad(90.833)) / (cos(rad(latitude)) *\
cos(rad(declination))) - tan(rad(latitude)) * tan(rad(declination))))
solarnoon_t = (720 - 4 * longitude-eqtime) / 1440
sunrise_t = solarnoon_t-hourangle * 4 / 1440
sunset_t = solarnoon_t+hourangle * 4 / 1440
def decimaltotime(day):
global time
hours = 24.0 * day
h = int(hours)
minutes = (hours-h) * 60
m = int(minutes)
seconds = (minutes-m) * 60
s = int(seconds)
return time(hour=h, minute=m, second=s)
self.solarnoon_t = decimaltotime(solarnoon_t)
self.sunrise_t = decimaltotime(sunrise_t)
self.sunset_t = decimaltotime(sunset_t)
def main():
# Define image coordinate of conjugate points
Lx = np.array([-4.87, 89.296, 0.256, 90.328, -4.673, 88.591])
Ly = np.array([1.992, 2.706, 84.138, 83.854, -86.815, -85.269])
Rx = np.array([-97.920, -1.485, -90.906, -1.568, -100.064, -0.973])
Ry = np.array([-2.91, -1.836, 78.980, 79.482, -95.733, -94.312])
# Define interior orientation parameters
f = 152.113
# Define initial values
XL0 = YL0 = OmegaL0 = PhiL0 = KappaL0 = 0
ZL0 = ZR0 = f
XR0 = abs(Lx - Rx).mean()
YR0 = OmegaR0 = PhiR0 = KappaR0 = 0
# Define initial coordinate for object points
# X0 = np.array(Lx)
# Y0 = np.array(Ly)
# Z0 = np.zeros(len(Lx))
X0 = XR0 * Lx / (Lx - Rx)
Y0 = XR0 * Ly / (Lx - Rx)
Z0 = f - (XR0 * f / (Lx - Rx))
# Define symbols
fs, x0s, y0s = symbols("f x0 y0")
XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs = symbols(u"XL YL ZL ωL, φL, κL".encode(LANG))
XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs = symbols(u"XR YR ZR ωR, φR, κR".encode(LANG))
xls, yls, xrs, yrs = symbols("xl yl xr yr")
XAs, YAs, ZAs = symbols("XA YA ZA")
# List observation equations
F1 = getEqns(x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs, xls, yls, XAs, YAs, ZAs)
F2 = getEqns(x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs, xrs, yrs, XAs, YAs, ZAs)
# Create lists for substitution of initial values and constants
var1 = np.array([x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs, xls, yls, XAs, YAs, ZAs])
var2 = np.array([x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs, xrs, yrs, XAs, YAs, ZAs])
# Define a symbol array for unknown parameters
l = np.array([OmegaRs, PhiRs, KappaRs, YRs, ZRs, XAs, YAs, ZAs])
# Compute coefficient matrix
JF1 = F1.jacobian(l)
JF2 = F2.jacobian(l)
# Create function objects for two parts of coefficient matrix and f matrix
B1 = lambdify(tuple(var1), JF1, modules="sympy")
B2 = lambdify(tuple(var2), JF2, modules="sympy")
F01 = lambdify(tuple(var1), -F1, modules="sympy")
F02 = lambdify(tuple(var2), -F2, modules="sympy")
X = np.ones(1) # Initial value for iteration
lc = 1 # Loop counter
while abs(X.sum()) > 10 ** -12:
B = np.matrix(np.zeros((4 * len(Lx), 5 + 3 * len(Lx))))
F0 = np.matrix(np.zeros((4 * len(Lx), 1)))
# Column index which is used to update values of B and f matrix
j = 0
for i in range(len(Lx)):
# Create lists of initial values and constants
val1 = np.array([0, 0, f, XL0, YL0, ZL0, OmegaL0, PhiL0, KappaL0, Lx[i], Ly[i], X0[i], Y0[i], Z0[i]])
val2 = np.array([0, 0, f, XR0, YR0, ZR0, OmegaR0, PhiR0, KappaR0, Rx[i], Ry[i], X0[i], Y0[i], Z0[i]])
# For coefficient matrix B
b1 = matrix2numpy(B1(*val1)).astype(np.double)
b2 = matrix2numpy(B2(*val2)).astype(np.double)
B[i * 4 : i * 4 + 2, :5] = b1[:, :5]
B[i * 4 : i * 4 + 2, 5 + j * 3 : 5 + (j + 1) * 3] = b1[:, 5:]
B[i * 4 + 2 : i * 4 + 4, :5] = b2[:, :5]
B[i * 4 + 2 : i * 4 + 4, 5 + j * 3 : 5 + (j + 1) * 3] = b2[:, 5:]
# For constant matrix f
f01 = matrix2numpy(F01(*val1)).astype(np.double)
f02 = matrix2numpy(F02(*val2)).astype(np.double)
F0[i * 4 : i * 4 + 2, :5] = f01
F0[i * 4 + 2 : i * 4 + 4, :5] = f02
j += 1
# Solve unknown parameters
N = B.T * B # Compute normal matrix
t = B.T * F0 # Compute t matrix
X = N.I * t # Compute the unknown parameters
# Update initial values
OmegaR0 += X[0, 0]
PhiR0 += X[1, 0]
KappaR0 += X[2, 0]
YR0 += X[3, 0]
ZR0 += X[4, 0]
X0 += np.array(X[5::3, 0]).flatten()
Y0 += np.array(X[6::3, 0]).flatten()
Z0 += np.array(X[7::3, 0]).flatten()
# Output messages for iteration process
print "Iteration count: %d" % lc, u"|ΔX| = %.6f".encode(LANG) % abs(X.sum())
lc += 1 # Update Loop counter
# Compute residual vector
V = F0 - B * X
# Compute error of unit weight
s0 = ((V.T * V)[0, 0] / (B.shape[0] - B.shape[1])) ** 0.5
# Compute other informations
# Sigmall = np.eye(B.shape[0])
SigmaXX = s0 ** 2 * N.I
# SigmaVV = s0**2 * (Sigmall - B * N.I * B.T)
# Sigmallhat = s0**2 * (Sigmall - SigmaVV)
param_std = np.sqrt(np.diag(SigmaXX))
pho_res = np.array(V).flatten()
# pho_res = np.sqrt(np.diag(SigmaVV))
# Output results
print "\nExterior orientation parameters:"
print ("%9s" + " %9s" * 3) % ("Parameter", "Left pho", "Right pho", "SD right")
print "%-10s %8.4f %9.4f %9.4f" % ("Omega(deg)", deg(OmegaL0), deg(OmegaR0), deg(param_std[0]))
print "%-10s %8.4f %9.4f %9.4f" % ("Phi(deg)", deg(PhiL0), deg(PhiR0), deg(param_std[1]))
print "%-10s %8.4f %9.4f %9.4f" % ("Kappa(deg)", deg(KappaL0), deg(KappaR0), deg(param_std[2]))
print "%-10s %8.4f %9.4f" % ("XL", XL0, XR0)
print "%-10s %8.4f %9.4f %9.4f" % ("YL", YL0, YR0, param_std[3])
print "%-10s %8.4f %9.4f %9.4f\n" % ("ZL", ZL0, ZR0, param_std[4])
print "Object space coordinates:"
print ("%5s" + " %9s" * 6) % ("Point", "X", "Y", "Z", "SD-X", "SD-Y", "SD-Z")
for i in range(len(X0)):
print ("%5s" + " %9.4f" * 6) % (
chr(97 + i),
X0[i],
Y0[i],
Z0[i],
param_std[3 * i + 5],
param_std[3 * i + 6],
param_std[3 * i + 7],
)
print "\nPhoto coordinate residuals:"
print ("%5s" + " %9s" * 4) % ("Point", "xl-res", "yl-res", "xr-res", "yr-res")
for i in range(len(X0)):
print ("%5s" + " %9.4f" * 4) % (
chr(97 + i),
pho_res[2 * i],
pho_res[2 * i + 1],
pho_res[2 * i + len(X0) * 2],
pho_res[2 * i + len(X0) * 2 + 1],
)
print ("\n%5s" + " %9.4f" * 4 + "\n") % (
"RMS",
np.sqrt((pho_res[0 : len(X0) * 2 : 2] ** 2).mean()),
np.sqrt((pho_res[1 : len(X0) * 2 + 1 : 2] ** 2).mean()),
np.sqrt((pho_res[len(X0) * 2 :: 2] ** 2).mean()),
np.sqrt((pho_res[len(X0) * 2 + 1 :: 2] ** 2).mean()),
)
print "Standard error of unit weight : %.4f" % s0
print "Degree of freedom: %d" % (B.shape[0] - B.shape[1])
return 0
def main(inputFileName, IOFileName, outputFileName):
# Read image coordinates from file
fin = open(inputFileName)
lines = fin.readlines()
fin.close()
data = np.array(map(lambda l: map(float, l.split()), lines))
Lx, Ly, Rx, Ry = map(lambda a: a.flatten(), np.hsplit(data, 4))
# Read interior orientation information from file
fin = open(IOFileName)
data = map(lambda x: float(x), fin.readline().split())
fin.close()
f = data[0]
# Define initial values
XL0 = YL0 = OmegaL0 = PhiL0 = KappaL0 = 0
ZL0 = ZR0 = f
XR0 = abs(Lx - Rx).mean()
YR0 = OmegaR0 = PhiR0 = KappaR0 = 0
# Define initial coordinate for object points
# X0 = np.array(Lx)
# Y0 = np.array(Ly)
# Z0 = np.zeros(len(Lx))
X0 = XR0 * Lx / (Lx - Rx)
Y0 = XR0 * Ly / (Lx - Rx)
Z0 = f - ((XR0 * f) / (Lx - Rx))
# Define symbols
fs, x0s, y0s = symbols("f x0 y0")
XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs = symbols(
u"XL YL ZL ωL, φL, κL".encode(LANG))
XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs = symbols(
u"XR YR ZR ωR, φR, κR".encode(LANG))
xls, yls, xrs, yrs = symbols("xl yl xr yr")
XAs, YAs, ZAs = symbols("XA YA ZA")
# List observation equations
F1 = getEqns(
x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs,
xls, yls, XAs, YAs, ZAs)
F2 = getEqns(
x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs,
xrs, yrs, XAs, YAs, ZAs)
# Create lists for substitution of initial values and constants
var1 = np.array([x0s, y0s, fs, XLs, YLs, ZLs, OmegaLs, PhiLs, KappaLs,
xls, yls, XAs, YAs, ZAs])
var2 = np.array([x0s, y0s, fs, XRs, YRs, ZRs, OmegaRs, PhiRs, KappaRs,
xrs, yrs, XAs, YAs, ZAs])
# Define a symbol array for unknown parameters
l = np.array([OmegaRs, PhiRs, KappaRs, YRs, ZRs, XAs, YAs, ZAs])
# Compute coefficient matrix
JF1 = F1.jacobian(l)
JF2 = F2.jacobian(l)
# Create function objects for two parts of coefficient matrix and f matrix
B1 = lambdify(tuple(var1), JF1, modules='sympy')
B2 = lambdify(tuple(var2), JF2, modules='sympy')
F01 = lambdify(tuple(var1), -F1, modules='sympy')
F02 = lambdify(tuple(var2), -F2, modules='sympy')
X = np.ones(1) # Initial value for iteration
lc = 1 # Loop counter
while abs(X.sum()) > 10**-10:
B = np.matrix(np.zeros((4 * len(Lx), 5 + 3 * len(Lx))))
F0 = np.matrix(np.zeros((4 * len(Lx), 1)))
# Column index which is used to update values of B and f matrix
j = 0
for i in range(len(Lx)):
# Create lists of initial values and constants
val1 = np.array([0, 0, f, XL0, YL0, ZL0, OmegaL0, PhiL0, KappaL0,
Lx[i], Ly[i], X0[i], Y0[i], Z0[i]])
val2 = np.array([0, 0, f, XR0, YR0, ZR0, OmegaR0, PhiR0, KappaR0,
Rx[i], Ry[i], X0[i], Y0[i], Z0[i]])
# For coefficient matrix B
b1 = matrix2numpy(B1(*val1)).astype(np.double)
b2 = matrix2numpy(B2(*val2)).astype(np.double)
B[i*4:i*4+2, :5] = b1[:, :5]
B[i*4:i*4+2, 5+j*3:5+(j+1)*3] = b1[:, 5:]
B[i*4+2:i*4+4, :5] = b2[:, :5]
B[i*4+2:i*4+4, 5+j*3:5+(j+1)*3] = b2[:, 5:]
# For constant matrix f
f01 = matrix2numpy(F01(*val1)).astype(np.double)
f02 = matrix2numpy(F02(*val2)).astype(np.double)
F0[i*4:i*4+2, :5] = f01
F0[i*4+2:i*4+4, :5] = f02
j += 1
# Solve unknown parameters
N = np.matrix(B.T * B) # Compute normal matrix
t = B.T * F0 # Compute t matrix
X = N.I * t # Compute the unknown parameters
# Update initial values
OmegaR0 += X[0, 0]
PhiR0 += X[1, 0]
KappaR0 += X[2, 0]
YR0 += X[3, 0]
ZR0 += X[4, 0]
X0 += np.array(X[5::3, 0]).flatten()
Y0 += np.array(X[6::3, 0]).flatten()
Z0 += np.array(X[7::3, 0]).flatten()
# Output messages for iteration process
print "Iteration count: %d" % lc, u"|ΔX| = %.6f".encode(LANG) \
% abs(X.sum())
lc += 1 # Update Loop counter
# Compute residual vector
V = F0 - B * X
# Compute error of unit weight
s0 = ((V.T * V)[0, 0] / (B.shape[0] - B.shape[1]))**0.5
# Compute other informations
# Sigmall = np.eye(B.shape[0])
SigmaXX = s0**2 * N.I
# SigmaVV = s0**2 * (Sigmall - B * N.I * B.T)
# Sigmallhat = s0**2 * (Sigmall - SigmaVV)
param_std = np.sqrt(np.diag(SigmaXX))
pho_res = np.array(V).flatten()
# pho_res = np.sqrt(np.diag(SigmaVV))
# Output results
print "\nExterior orientation parameters:"
print ("%9s"+" %9s"*3) % ("Parameter", "Left pho", "Right pho", "SD right")
print "%-10s %8.4f %9.4f %9.4f" % (
"Omega(deg)", deg(OmegaL0), deg(OmegaR0), deg(param_std[0]))
print "%-10s %8.4f %9.4f %9.4f" % (
"Phi(deg)", deg(PhiL0), deg(PhiR0), deg(param_std[1]))
print "%-10s %8.4f %9.4f %9.4f" % (
"Kappa(deg)", deg(KappaL0), deg(KappaR0), deg(param_std[2]))
print "%-10s %8.4f %9.4f" % (
"XL", XL0, XR0)
print "%-10s %8.4f %9.4f %9.4f" % (
"YL", YL0, YR0, param_std[3])
print "%-10s %8.4f %9.4f %9.4f\n" % (
"ZL", ZL0, ZR0, param_std[4])
print "Object space coordinates:"
print ("%5s"+" %9s"*6) % ("Point", "X", "Y", "Z", "SD-X", "SD-Y", "SD-Z")
for i in range(len(X0)):
print ("%5s"+" %9.4f"*6) % (
("p%d" % i), X0[i], Y0[i], Z0[i],
param_std[3*i+5],
param_std[3*i+6],
param_std[3*i+7])
print "\nPhoto coordinate residuals:"
print ("%5s"+" %9s"*4) % ("Point", "xl-res", "yl-res", "xr-res", "yr-res")
for i in range(len(X0)):
print ("%5s"+" %9.4f"*4) % (
("p%d" % i), pho_res[2*i], pho_res[2*i+1],
pho_res[2*i+len(X0)*2], pho_res[2*i+len(X0)*2+1])
print ("\n%5s"+" %9.4f"*4+"\n") % (
"RMS",
np.sqrt((pho_res[0:len(X0)*2:2]**2).mean()),
np.sqrt((pho_res[1:len(X0)*2+1:2]**2).mean()),
np.sqrt((pho_res[len(X0)*2::2]**2).mean()),
np.sqrt((pho_res[len(X0)*2+1::2]**2).mean()))
print "Standard error of unit weight : %.4f" % s0
print "Degree of freedom: %d" % (B.shape[0] - B.shape[1])
# Write out results
fout = open(outputFileName, "w")
fout.write("%.8f %.8f %.8f\n" % (XL0, YL0, ZL0))
fout.write("%.8f %.8f %.8f\n" % (XR0, YR0, ZR0))
for i in range(len(X0)):
fout.write("%.8f %.8f %.8f\n" % (X0[i], Y0[i], Z0[i]))
fout.close()
return 0
for i in range(nSamples):
Ip[i,:] = samples[i,:] * iCosTh[i]
Ep[i,:] = samples[i,:] * eCosTh[i]
Ip[i] = normalize(I - Ip[i,:])
Ep[i] = normalize(E - Ep[i,:])
ePhi[i] = np.arccos(np.dot(Ip[i,:], Ep[i,:]) - 1e-8)
iTh = iTheta[ok]
eTh = eTheta[ok]
ePh = ePhi[ok]
for i in range(len(iTh)):
p[1,a] += hs.eval(deg(iTh[i]), deg(eTh[i]), deg(ePh[i]))
p[1,a] *= dw / 15.0
p[0,a] = a
p[2,a] = np.sum(iTh*eTh / (iTh+eTh)) * dw
#for i in range(nSamples):
# pl.plot([samples[i,0]], [samples[i,1]],'o', c=str(ePhi[i] / math.pi))
pl.plot(p[0,:],p[1,:],'o')
pl.plot(p[0,:],p[2,:],'o')
pl.show()
def spaceResection(inputFile, outputFile, s,
useRANSAC, maxIter, sampleSize, thres, init):
"""Perform a space resection."""
# Read observables from txt file
with open(inputFile) as fin:
f = float(fin.readline()) # The focal length in mm
# Define symbols
EO = symbols("XL YL ZL Omega Phi Kappa") # Exterior orienration parameters
PT = symbols("XA YA ZA") # Object point coordinates
pt = symbols("xa ya") # Image coordinates
# Define variable for inerior orienration parameters
IO = f, 0, 0
# List and linearize observation equations
F = getEqn(IO, EO, PT, pt)
JFx = F.jacobian(EO)
JFl = F.jacobian(PT) # Jacobian matrix for observables
# Create lambda function objects
FuncJFl = lambdify((EO+PT), JFl, 'numpy')
FuncJFx = lambdify((EO+PT), JFx, 'numpy')
FuncF = lambdify((EO+PT+pt), F, 'numpy')
data = pd.read_csv(
inputFile,
delimiter=' ',
usecols=range(1, 9),
names=[str(i) for i in range(8)],
skiprows=1)
# Check data size
if useRANSAC and len(data) <= sampleSize:
print "Insufficient data for applying RANSAC method,",
print "change to normal approach"
useRANSAC = False
if useRANSAC:
bestErr = np.inf
bestIC = 0
bestParam = 0
bestN = 0
for i in range(maxIter):
print "Iteration count: %d" % (i+1)
sample = data.sample(sampleSize)
# Compute initial model with sample data
try:
X0, s0, N = estimate(
sample, f, s, (FuncJFl, FuncJFx, FuncF), init)
except np.linalg.linalg.LinAlgError:
continue
idx = getInlier(data, f, s, (FuncJFl, FuncJFx, FuncF), X0, thres)
consensusSet = data.loc[idx] # Inliers
# Update the model if the number consesus set is greater than
# current model and the error is smaller
if len(consensusSet) >= bestIC:
try:
X0, s0, N = estimate(
consensusSet, f, s, (FuncJFl, FuncJFx, FuncF), init)
except np.linalg.linalg.LinAlgError:
continue
if s0 < bestErr:
bestErr = s0
bestIC = len(consensusSet)
bestParam = X0
bestN = N
print "Found better model,",
print "inlier=%d (%.2f%%), error=%.6f" % \
(bestIC, 100.0 * bestIC / len(data), bestErr)
if bestIC == 0:
print "Cannot apply RANSAC method, change to normal approach"
bestParam, bestErr, bestN = estimate(
data, f, s, (FuncJFl, FuncJFx, FuncF), init)
else:
bestParam, bestErr, bestN = estimate(
data.sample(frac=1), f, s, (FuncJFl, FuncJFx, FuncF), init)
# Compute other informations
SigmaXX = bestErr**2 * bestN.I
paramStd = np.sqrt(np.diag(SigmaXX))
XL, YL, ZL, Omega, Phi, Kappa = np.array(bestParam).ravel()
# Output results
print "Exterior orientation parameters:"
print (" %9s %11s %11s") % ("Parameter", "Value", "Std.")
print " %-10s %11.6f %11.6f" % (
"Omega(deg)", deg(Omega) % 360, deg(paramStd[3]))
print " %-10s %11.6f %11.6f" % (
"Phi(deg)", deg(Phi) % 360, deg(paramStd[4]))
print " %-10s %11.6f %11.6f" % (
"Kappa(deg)", deg(Kappa) % 360, deg(paramStd[5]))
print " %-10s %11.6f %11.6f" % ("XL", XL, paramStd[0])
print " %-10s %11.6f %11.6f" % ("YL", YL, paramStd[1])
print " %-10s %11.6f %11.6f" % ("ZL", ZL, paramStd[2])
print "\nSigma0 : %.6f" % bestErr
with open(outputFile, 'w') as fout:
fout.write("%.6f "*3 % (XL, YL, ZL))
fout.write("%.6f "*3 %
tuple(map(lambda x: deg(x) % 360, [Omega, Phi, Kappa])))
fout.write("%.6f "*3 % tuple(paramStd[:3]))
fout.write("%.6f "*3 % tuple(map(lambda x: deg(x), paramStd[3:])))
def transProc(pointFile, controlPtFile, outputFileName):
"""A function to control 3D conformal transformation process"""
# Read 3D coordinates from file
fin = open(controlPtFile)
lines = fin.readlines()
fin.close()
data = np.array(map(lambda l: map(float, l.split()), lines))
x, y, z, X, Y, Z = map(lambda e: e.flatten(), np.hsplit(data, 6))
L = np.matrix(np.append(x, [y, z, X, Y, Z])).T
# Define symbols
Ss, Omegas, Phis, Kappas, Txs, Tys, Tzs = symbols(
u"σ ω φ κ Tx Ty Tz".encode(LANG))
dXs = np.array([Ss, Omegas, Phis, Kappas, Txs, Tys, Tzs])
# Symbole for observations
xs = np.array(symbols("x1:%d" % (len(x)+1)))
ys = np.array(symbols("y1:%d" % (len(y)+1)))
zs = np.array(symbols("z1:%d" % (len(z)+1)))
Xs = np.array(symbols("X1:%d" % (len(X)+1)))
Ys = np.array(symbols("Y1:%d" % (len(Y)+1)))
Zs = np.array(symbols("Z1:%d" % (len(Z)+1)))
Ls = np.array(np.append(xs, [ys, zs, Xs, Ys, Zs]))
# Compute initial values
S0, Omega0, Phi0, Kappa0, Tx0, Ty0, Tz0 = getInit(x, y, z, X, Y, Z)
L0 = np.matrix(np.append(x, [y, z, X, Y, Z])).T
# List observation equations
F = getEqns(
Ss, Omegas, Phis, Kappas, Txs, Tys, Tzs, xs, ys, zs, Xs, Ys, Zs)
# Create lists for substitution of initial values and constants
var = np.append([Ss, Omegas, Phis, Kappas, Txs, Tys, Tzs], Ls)
# Compute cofficient matrix
JFx = F.jacobian(dXs)
JFl = F.jacobian(Ls)
# Create function objects for two parts of cofficient matrix and f matrix
FuncB = lambdify(tuple(var), JFx, modules='sympy')
FuncA = lambdify(tuple(var), JFl, modules='sympy')
FuncF = lambdify(tuple(var), F, modules='sympy')
dX = np.ones(1) # Initial value for iteration
lc = 1 # Loop counter
while abs(dX.sum()) > 10**-10:
# Create lists of initial values and constants
val = np.append([S0, Omega0, Phi0, Kappa0, Tx0, Ty0, Tz0], L0)
# Substitute values for symbols
B = np.matrix(FuncB(*val)).astype(np.double)
A = np.matrix(FuncA(*val)).astype(np.double)
F0 = np.matrix(FuncF(*val)).astype(np.double)
F = -F0 - A * (L - L0)
Qe = A * A.T
We = Qe.I
N = (B.T * We * B) # Compute normal matrix
t = (B.T * We * F) # Compute t matrix
dX = N.I * t # Compute the unknown parameters
V = A.T * We * (F - B * dX) # Compute residual vector
# Update initial values
S0 += dX[0, 0]
Omega0 += dX[1, 0]
Phi0 += dX[2, 0]
Kappa0 += dX[3, 0]
Tx0 += dX[4, 0]
Ty0 += dX[5, 0]
Tz0 += dX[6, 0]
L0 = L + V
# Output messages for iteration process
print "Iteration count: %d" % lc, u"|ΔX| = %.6f".encode(LANG) \
% abs(dX.sum())
lc += 1 # Update Loop counter
# Compute residual vector
V = A.T * We * (F - B * dX) # Compute residual vector
# Compute error of unit weight
s0 = ((V.T * V)[0, 0] / (B.shape[0] - B.shape[1]))**0.5
# Compute other informations
SigmaXX = s0**2 * N.I
# SigmaVV = s0**2 * (A.T - A.T * We * B * N.I * B.T) \
# * (A.T * We - A.T * We * B * N.I * B.T * We).T
param_std = np.sqrt(np.diag(SigmaXX))
# Output results
print "\nResiduals:"
print ("%-8s"+" %-8s"*6) % (
"Point", "x res", "y res", "z res", "X res", "Y res", "Z res")
for i in range(0, len(V) / 6):
print ("%-8d"+" %-8.4f"*6) % (i + 1, V[i, 0], V[i + 3, 0], V[i + 6, 0],
V[i + 9, 0], V[i + 12, 0], V[i + 15, 0])
print "\n3D comformal transformation parameters:"
print ("%9s"+" %12s %12s") % ("Parameter", "Value", "Stan Err")
print "%-10s %12.4f %12.5f" % ("Scale", S0, param_std[0])
print "%-10s %12.4fd %11.5fd" % (
"Omega(deg)", deg(Omega0), deg(param_std[1]))
print "%-10s %12.4fd %11.5fd" % (
"Phi(deg)", deg(Phi0), deg(param_std[2]))
print "%-10s %12.4fd %11.5fd" % (
"Kappa(deg)", deg(Kappa0), deg(param_std[3]))
print "%-10s %12.4f %12.5f" % ("Tx", Tx0, param_std[4])
print "%-10s %12.4f %12.5f" % ("Ty", Ty0, param_std[5])
print "%-10s %12.4f %12.5f" % ("Tz", Tz0, param_std[6])
print "\nStandard error of unit weight : %.4f" % s0
print "Degree of freedom: %d" % (B.shape[0] - B.shape[1])
# Transform 3D coordinates
# Read 3D coordinates from file
fin = open(pointFile)
lines = fin.readlines()
fin.close()
data = np.array(map(lambda l: map(float, l.split()), lines))
x, y, z = map(lambda e: e.flatten(), np.hsplit(data, 3))
M = getM(Omega0, Phi0, Kappa0)
X = S0 * (M[0, 0]*x + M[1, 0]*y + M[2, 0]*z) + Tx0
Y = S0 * (M[0, 1]*x + M[1, 1]*y + M[2, 1]*z) + Ty0
Z = S0 * (M[0, 2]*x + M[1, 2]*y + M[2, 2]*z) + Tz0
# Write out results
fout = open(outputFileName, "w")
for i in range(len(X)):
fout.write("%.8f %.8f %.8f\n" % (X[i], Y[i], Z[i]))
fout.close()
def func(delta_hour):
d = d_o + dt.timedelta(hours=delta_hour)
d, T, h, ps, s, p, N = astro_params(d)
cm_rm = gracefulol([
(1, 1),
(0.054501, cos(s-p)),
(0.010025, cos(s-2*h+p)),
(0.008249, cos(2*(s-h))),
(0.002970, cos(2*(s-p)))])
lmd_m = gracefulol([
(1, s),
(deg(0.109760), sin(s-p)),
(deg(0.022236), sin(s-2*h+p)),
(deg(0.011490), sin(2*(s-h))),
(deg(0.003728), sin(2*(s-p)))], map_to_360deg)
beta_m = gracefulol([
(deg(0.089504), sin(s-N)),
(deg(0.004897), sin(2*s-p-N)),
(-deg(0.004847), sin(p-N)),
(deg(0.003024), sin(s-2*h+N))], map_to_360deg)
cs_rs = gracefulol([
(1, 1),
(0.016750, cos(h-ps)),
(0.000280, cos(2*(h-ps))),
(0.000005, cos(3*(h-ps)))])
lmd_s = gracefulol([
(1, h),
(deg(0.016750), cos(h-ps)),
(deg(0.000280), cos(2*(h-ps))),
(deg(0.000005), cos(3*(h-ps)))], map_to_360deg)
x = (d.timestamp() % 86400)*15/3600 + h + L - 180
E = gracefulol([
(23.452294, 1),
(-0.0130125, T),
(-0.0000016, T*T),
(0.0000005, T*T*T)], map_to_360deg)
cos_zm = sin(phi) * (
sin(E)*sin(lmd_m)*cos(beta_m) +
cos(E)*sin(beta_m)) + \
cos(phi) * (
cos(lmd_m)*cos(beta_m)*cos(x) +
sin(x) * (
cos(E)*sin(lmd_m)*cos(beta_m) +
-sin(E)*sin(beta_m)))
d_gm = 54.993 * cm_rm**3 * (1-3*cos_zm**2) + \
1.369 * cm_rm**4 * (3*cos_zm - 5*cos_zm**3)
cos_zs = sin(phi) * sin(lmd_s)*sin(E) + \
cos(phi) * (
cos(lmd_s)*cos(x) +
sin(lmd_s)*sin(x)*cos(E))
d_gs = 25.358 * cs_rs**3 * (1-3*cos_zs**2)
return d_gm + d_gs
