[tools] Added process_exif script

closes paparazzi#483

sw/tools/process_exif/README

@@ -0,0 +1,67 @@
+
+This is a script which loads GPS info into photo files (EXIF) based on the DC_SHOT messages
+in the telemetry data. The .data file contains DC_SHOT messages (possibly not all emitted by telemetry,
+but I'll get back to that later). This file together with the files in jpeg format are
+copied to a processing directory, where this script extracts the GPS position from each DC_SHOT message
+and edits the exif photo data in place, so effectively loads the GPS position into the EXIF data for
+each photo.
+
+This script allows for gaps in DC_SHOT photo numbers and has some rudimentary error checking.
+
+
+Instructions for use (on Ubuntu):
+
+1. Make sure python is installed.
+2. Install the necessary python packages:
+
+# sudo apt-get install python-gdal
+# sudo apt-get install gir1.2-gexiv2-0.4
+
+3. Create a directory for processing. The photo's EXIF data will be edited in place.
+4. Copy the .data file from the <paparazzi-dir>/var/log directory into this processing directory.
+5. Copy all photos taken during the session to the directory. 
+
+Verification:
+- Sort photos by name. Since most cameras output the photo name with a number at the end, this should
+  show a list of photo names with consecutive numbers.
+- Verify that the first photo corresponds to the first DC_SHOT message (photonr == 1).
+- Add some dummy photos at the start to make up for any missing photos that may exist (such that they get
+  sorted before the real actual one).
+
+6. Run the script:
+
+# python sw/tools/process_exif/process_exif.py /<processing-dir>
+
+Verification of processing:
+- Look at the logs!  These include the GPS positions calculated from UTM paparazzi positions. 
+  It has been tested on the southern+western hemispheres, but not yet on eastern and northern hemispheres.
+- Verify the tags in the photo:
+  exiftool -v2 <processing-dir>/<some-photo-name>.jpg
+
+----
+
+Considerations:
+
+The Ivy telemetry bus can get a little clogged up and this will result in discarded messages. This means that 
+not all DC_SHOT messages actually emitted by telemetry need to be there on the ground station, so some photos
+won't have GPS coordinates loaded. Not all processing software for orthomosaics however require GPS positions 
+for all photos, for example if they rely on recognizing corresponding features in overlapping photos.
+
+Having more GPS coordinates in photos helps to improve accuracy, as the error in measurements approximates zero
+over time if the error follows a normal distribution. 
+
+Having many GPS coordinates is not enough however to achieve correct precision down to centimeters. Atmospheric conditions
+need to be eliminated by applying 3-5 ground control points in strategic locations. These conditions can easily 
+cause the entire orthomosaic to be shifted over a meter.
+
+
+If your processing software does require the GPS coordinates per photo, you need to look into the use of a 
+telemetry logger onboard. This logger can be hooked up separately on the tx+gnd lines of telemetry and "listen in" on
+the connection.
+
+
+The timing between actually taking a picture and the pulse event being sent is also an important consideration.
+Obviously the GPS frequency is also an issue. If this is used on a fixedwing, obviously if the plane is underway
+at 12m/s, 250ms will introduce a 3m bias on the position. Another reason to rely on GPS positions only as hints
+and apply ground control points to "set" the orthomosaic to the right location.
+

sw/tools/process_exif/process_exif.py

@@ -0,0 +1,166 @@
+#!/usr/bin/python
+#
+#       <message NAME="DC_SHOT" ID="110">
+#        <field TYPE="int16" NAME="photo_nr"/>
+#        <field UNIT="cm" TYPE="int32" NAME="utm_east"/>
+#        <field UNIT="cm" TYPE="int32" NAME="utm_north"/>
+#        <field UNIT="m" TYPE="float" NAME="z"/>
+#        <field TYPE="uint8" NAME="utm_zone"/>
+#        <field UNIT="decideg" TYPE="int16" NAME="phi"/>
+#        <field UNIT="decideg" TYPE="int16" NAME="theta"/>
+#        <field UNIT="decideg" TYPE="int16" NAME="course"/>
+#        <field UNIT="cm/s" TYPE="uint16" NAME="speed"/>
+#        <field UNIT="ms" TYPE="uint32" NAME="itow"/>
+#      </message>
+#
+
+# sudo apt-get install python-gdal
+# sudo apt-get install gir1.2-gexiv2-0.4
+
+from gi.repository import GExiv2
+import glob
+import os
+import re
+import fnmatch
+import sys
+import math
+
+M_PI=3.14159265358979323846
+M_PI_2=(M_PI/2)
+M_PI_4=(M_PI/4)
+
+def RadOfDeg( deg ):
+    return (deg / M_PI) * 180.
+
+# converts UTM coords to lat/long.  Equations from USGS Bulletin 1532 
+# East Longitudes are positive, West longitudes are negative. 
+# North latitudes are positive, South latitudes are negative
+# Lat and Long are in decimal degrees. 
+# Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+# ( I had some code here to use GDAL and which looked much simpler, but couldn't get that to work )
+
+def UTMtoLL( northing, easting, utm_zone ):
+
+    k0 = 0.9996;
+    a = 6378137; # WGS-84
+    eccSquared = 0.00669438; # WGS-84
+    e1 = (1-math.sqrt(1-eccSquared))/(1+math.sqrt(1-eccSquared));
+
+    x = easting - 500000.0; # remove 500,000 meter offset for longitude
+    y = northing;
+
+    is_northern = northing < 0
+    if ( not is_northern ):
+		y -= 10000000.0 # remove 10,000,000 meter offset used for southern hemisphere
+
+    LongOrigin = (utm_zone - 1)*6 - 180 + 3;  # +3 puts origin in middle of zone
+
+    eccPrimeSquared = (eccSquared)/(1-eccSquared);
+
+    M = y / k0;
+    mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
+
+    phi1Rad = mu	+ (3*e1/2-27*e1*e1*e1/32)*math.sin(2*mu) + (21*e1*e1/16-55*e1*e1*e1*e1/32)*math.sin(4*mu) +(151*e1*e1*e1/96)*math.sin(6*mu);
+    phi1 = RadOfDeg(phi1Rad);
+
+    N1 = a/math.sqrt(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad));
+    T1 = math.tan(phi1Rad)*math.tan(phi1Rad);
+    C1 = eccPrimeSquared*math.cos(phi1Rad)*math.cos(phi1Rad);
+    R1 = a*(1-eccSquared)/math.pow(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad), 1.5);
+    D = x/(N1*k0);
+
+    Lat = phi1Rad - (N1*math.tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
+    Lat = RadOfDeg(Lat)
+
+    Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)*D*D*D*D*D/120)/math.cos(phi1Rad)
+    Long = LongOrigin + RadOfDeg(Long)
+
+    return Lat, Long
+
+
+# At least the directory must be given
+if len(sys.argv) < 2:
+    print "This script requires one argument: A directory containing photos and the paparazzi .data file"
+    sys.exit()
+
+path = str(sys.argv[ 1] )
+
+if os.path.isdir(path) == False:
+    print "The indicated path '%s' is not a directory"%(path)
+    sys.exit()
+
+# Searching for all files with .data extension in indicated directory.
+# It should only have one.
+list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))]
+files = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.data'), j, re.IGNORECASE)]
+
+if  len(files) > 1:
+    print "Too many data files found. Only one is allowed."
+    sys.exit()
+
+if len(files) == 0:
+    print "No data files in 'data'. Copy data file there."
+    sys.exit()
+
+# Now searching for all photos (extension .jpg) in directory
+list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))]
+photos = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.jpg'), j, re.IGNORECASE)]
+
+# Photos must be sorted by number
+photos.sort()
+
+# Opening the data file, iterating all lines and searching for DC_SHOT messages
+f = open( files[0], 'r' )
+for line in f:
+    line = line.rstrip()
+    line = re.sub(' +',' ',line)
+    if 'DC_SHOT' in line:
+        # 618.710 1 DC_SHOT 212 29133350 -89510400 8.5 25 -9 29 0 0 385051650
+        splitted = line.split( ' ' )
+
+        if len(splitted) < 12:
+            continue
+        try:
+            photonr = int(splitted[ 3 ])
+            utm_east = ( float(int(splitted[ 4 ])) / 100. )
+            utm_north = ( float(int(splitted[ 5 ])) / 100. )
+            alt = float(splitted[ 6 ])
+            utm_zone = int(splitted[ 7 ])
+            phi = int(splitted[ 8 ])
+            theta = int(splitted[ 9 ])
+            course = int(splitted[ 10 ])
+            speed = int(splitted[ 11 ])
+
+            lon, lat = UTMtoLL( utm_north, utm_east, utm_zone )
+
+            # Check that there as many photos and pick the indicated one.
+            # (this assumes the photos were taken correctly without a hiccup)
+            # It would never be able to check this anyway, since the camera could stall or 
+            # not interpret the pulse?  Leading to an incorrect GPS coordinate.
+            if len( photos ) < photonr:
+                print "Photo data %d found, but ran out of photos in directory"%(photonr)
+                continue                        
+
+            # I've seen log files with -1 as DC_SHOT number due to an int8 I think. This should be 
+            # fixed now, but just in case someone runs this on old data.
+            if (photonr < 0):
+                print "Negative photonr found."
+                continue
+
+            # Pick out photo, open it through exiv2, 
+            photoname = photos[ photonr - 1 ]
+            photo = GExiv2.Metadata( photoname )
+
+            photo.set_gps_info(lat, lon, alt)
+            photo.save_file()
+
+            print "Photo %s and photonr %d merged. Lat/Lon/Alt: %f, %f, %f"%(photoname, photonr, lat, lon, alt)
+
+        except ValueError as e:
+            print "Cannot read line: %s"%(line) 
+            print "Value error(%s)"%(e)
+            continue
+
+print "Finished! exiting."
+