video2geo_1.py

#ffmpeg -i "G:\PHOTO\_0\Olympus\2015-05-27\DSCN9178.MOV"  -f image2 -vcodec mjpeg -qscale 1 -r 5 frames/output_%05d.jpg"

import os
import sys
import datetime
import math
from datetime import timedelta
from datetime import date

from decimal import Decimal
from decimal import getcontext

import pyexiv2
from pyexiv2.utils import make_fraction

getcontext().prec = 8

frames_dir=os.path.join(os.getcwd(),'frames')

frames_dir=r'C:\Users\trolleway\Documents\video2geo\frames'
frames_dir=os.getcwd()+'/frames/'

fps=29.97
fps=3.

offset_angle = 50

framecount=1
timediff=3600*20
ms_for_frame = int(round(1000/fps))
td = timedelta(days=0,hours=14, minutes=59, seconds=12-9.5, microseconds=0)
#trackdate=datetime.date(2015,4,26)
#td=td.replace(2015,4,26)
timecode=0
timedelta_for_frame=timedelta(microseconds=int(round(1000000/fps)))

def getFristDayOfTrack():
	return datetime.datetime(2015,5,04)

real_date_of_frame=getFristDayOfTrack()
real_date_of_frame=real_date_of_frame+td



import xml.dom.minidom

dom3 = xml.dom.minidom.parse('20150504-142721_reopen.gpx')

def getlat(trkpt):
	return trkpt.attributes["lat"].value.encode('utf-8')
def getlon(trkpt):
	return trkpt.attributes["lon"].value.encode('utf-8')
def gettime(trkpt):
	rfc3339 = trkpt.getElementsByTagName('time')[0].firstChild.data
	try:
		t = datetime.datetime.strptime(rfc3339, '%Y-%m-%dT%H:%M:%S.%fZ')
	except ValueError:
		t = datetime.datetime.strptime(rfc3339, '%Y-%m-%dT%H:%M:%SZ')
	return t

trackdata=(dom3.getElementsByTagName('gpx')[0].getElementsByTagName('trk')[0].getElementsByTagName('trkseg')[0].getElementsByTagName('trkpt'))
gpx_records=[]
for trkpt in trackdata:
	lat=getlat(trkpt)
	lon=getlon(trkpt)
	time=gettime(trkpt)
	trkpt_record=[time,lat,lon]
	gpx_records.append(trkpt_record)

	

def compute_bearing(start_lat, start_lon, end_lat, end_lon):
    '''
    Get the compass bearing from start to end.

    Formula from
    http://www.movable-type.co.uk/scripts/latlong.html
    '''
    # make sure everything is in radians
    start_lat = math.radians(start_lat)
    start_lon = math.radians(start_lon)
    end_lat = math.radians(end_lat)
    end_lon = math.radians(end_lon)

    dLong = end_lon - start_lon

    dPhi = math.log(math.tan(end_lat/2.0+math.pi/4.0)/math.tan(start_lat/2.0+math.pi/4.0))
    if abs(dLong) > math.pi:
        if dLong > 0.0:
            dLong = -(2.0 * math.pi - dLong)
        else:
            dLong = (2.0 * math.pi + dLong)

    y = math.sin(dLong)*math.cos(end_lat)
    x = math.cos(start_lat)*math.sin(end_lat) - math.sin(start_lat)*math.cos(end_lat)*math.cos(dLong)
    bearing = (math.degrees(math.atan2(y, x)) + 360.0) % 360.0


    return bearing
	
def getPercentOfTime(datetime1,datetime2,datetime3):

	td1=datetime3-datetime2
	#print "td1=",td1.total_seconds()
	td2=datetime1-datetime2
	#print "td2=",td2.total_seconds()
	per=(100/td1.total_seconds())*td2.total_seconds()
	per=per/100
	#print "per=",str(per)
	
	return per

def interpolate0(x1,y1,x2,y2,percent):
	def ratio2percent0(percent):
		d1=1+percent
		d2=percent
		ratio=round((100/d1*d2)/100 , 6)
		
		
		#print 'percent=',percent,'ratio=',ratio
		return Decimal(ratio)
		
	getcontext().prec = 8
	l=Decimal(percent)/Decimal(1-percent)
	
	
	ratio=ratio2percent(percent)
	#print ratio
	ratio=Decimal(percent)
	#print ratio
	#quit()
	
	down=1+ratio
	x3=(Decimal(x1)+(ratio*Decimal(x2))) / down
	y3=(Decimal(y1)+(ratio*Decimal(y2))) / down
	#print "x1=",x1,"x2=",x2, "percent=",percent, 'r=',ratio,'x3=',x3,'down=',down
	
	
	#quit()
	return x3,y3

def interpolate(x1,y1,x2,y2,datetime_photo, datetime_before, datetime_after):


    dt_before = (datetime_photo-datetime_before).total_seconds()
    dt_after = (datetime_after-datetime_photo).total_seconds()

    #print "x1~",type(x1),"x2~",type(x2),"dt_after~",type(dt_after),dt_after,"dt_before~",type(dt_before)
    #quit()
    # simple linear interpolation
    lat = (float(x1)*dt_after + float(x2)*dt_before) / (dt_before + dt_after)
    lon = (float(y1)*dt_after + float(y2)*dt_before) / (dt_before + dt_after)

    bearing = compute_bearing(x1, y1, float(x2), float(y2))
	
	
	#quit()
    return lat,lon, bearing
	
	
def to_deg(value, loc):
    '''
    Convert decimal position to degrees.
    '''
    if value < 0:
        loc_value = loc[0]
    elif value > 0:
        loc_value = loc[1]
    else:
        loc_value = ""
    abs_value = abs(value)
    deg =  int(abs_value)
    t1 = (abs_value-deg)*60
    mint = int(t1)
    sec = round((t1 - mint)* 60, 6)
    return (deg, mint, sec, loc_value)
	
	
def writeGeotagToJPG(filename,lat,lon,bearing,real_datetime):
    
    metadata = pyexiv2.ImageMetadata(filename)
    metadata.read()
	
    try:
        lat_deg = to_deg(lat, ["S", "N"])
        lon_deg = to_deg(lon, ["W", "E"])
		
        exiv_lat = (make_fraction(lat_deg[0],1), make_fraction(int(lat_deg[1]),1), make_fraction(int(lat_deg[2]*1000000),1000000))
        exiv_lon = (make_fraction(lon_deg[0],1), make_fraction(int(lon_deg[1]),1), make_fraction(int(lon_deg[2]*1000000),1000000))

        # convert direction into fraction
        exiv_bearing = make_fraction(int(bearing*100),100)

        # add to exif
        metadata["Exif.GPSInfo.GPSLatitude"] = exiv_lat
        metadata["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
        metadata["Exif.GPSInfo.GPSLongitude"] = exiv_lon
        metadata["Exif.GPSInfo.GPSLongitudeRef"] = lon_deg[3]
        metadata["Exif.Image.GPSTag"] = 654
        metadata["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
        metadata["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
        metadata["Exif.GPSInfo.GPSImgDirection"] = exiv_bearing
        metadata["Exif.GPSInfo.GPSImgDirectionRef"] = "T"
        metadata["Exif.Image.DateTime"] = real_datetime
        metadata["Iptc.Application2.Caption"] = ["Test. Georefrenced frame from video record by trolleway"]
        

        metadata.write()
        #print("Added geodata to: {0} ({1}, {2}".format(filename, lat, lon))
    except ValueError, e:
        print("Skipping {0}: {1}".format(filename, e))	
		
		
	
#print frames_dir
for dirpath, dnames, fnames in os.walk(frames_dir):
    for file in fnames:
		#if os.path.isfile(file):
		if file.endswith(".jpg"):

			
			real_date_of_frame=real_date_of_frame+timedelta_for_frame
			
			#print str(framecount).ljust(5)+file.rjust(20)+str(timecode).rjust(10)+str(real_date_of_frame).rjust(35)
			
			#search record in gpx before and after this frame
			
			is_prev_point_found=False
			is_next_point_found=False
			gpx_record_prev=0
			gpx_record_next=0
			
			#trackpoint[0] - is timestamp
			
			for trackpoint in gpx_records:
				if (trackpoint[0] <= real_date_of_frame):
					gpx_record_prev=trackpoint
					is_prev_point_found=True
					
				if (trackpoint[0] > real_date_of_frame) and (is_next_point_found==False):
					gpx_record_next=trackpoint
					is_next_point_found=True
					
			if is_prev_point_found==False or is_next_point_found==False:
				print file,"skipped, because outside of track"
				continue
			#print gpx_record_prev		
			#print gpx_record_next	
			
			
			percent=getPercentOfTime(real_date_of_frame,gpx_record_prev[0],gpx_record_next[0])
			
			point_lat,point_lon,bearing=interpolate(float(gpx_record_prev[1]),float(gpx_record_prev[2]), float(gpx_record_next[1]), float(gpx_record_next[2]),real_date_of_frame,gpx_record_prev[0],gpx_record_next[0])
			direction = (bearing + offset_angle + 360) % 360
			#print file.rjust(10),str(percent).ljust(10) ,str(point_lat).rjust(10),str(point_lon).rjust(10)
			#ip = LineString([(gpx_record_prev[1], gpx_record_prev[2]), (gpx_record_next[1], gpx_record_next[2])]).interpolate(kof, normalized=True)
			#print ip.wkt
			writeGeotagToJPG(os.path.join(frames_dir,file),point_lat,point_lon,direction,real_date_of_frame)
			
			#print file,real_date_of_frame.time(),gpx_record_prev[0].time(),gpx_record_next[0].time(), percent, point_lat,point_lon
			print file, point_lat,point_lon
			
			#quit()
			framecount=framecount+1
			timecode=timecode+ms_for_frame
			#print str(framecount).ljust(15), str(timecode-timediff).ljust(15), str(timecode).ljust(15)