adding geotagging python script

Tools/sdlog2/geotagging.py

+#!/usr/bin/env python
+#
+#       __geotagging__
+#       Tag the images recorded during a flight with geo location extracted from
+#       a PX4 log file.
+# 
+#       This file accepts *.jpg format images and reads position information
+#       from a *.px4log file
+#
+#       Example Syntax:
+#       python geotagging.py --logfile=log001.px4log --input=images/
+#                            --output=imagesWithTag/
+#
+#   Author: Andreas Bircher, Wingtra, http://wingtra.com, in 2016
+#   
+# 
+
+__author__ = "Andreas Bircher"
+
+import glob
+import os
+import pyexiv2
+import fractions
+from PIL import Image
+from PIL.ExifTags import TAGS
+import sys
+from shutil import copyfile
+from optparse import OptionParser
+from numpy import genfromtxt
+import shutil
+import csv
+
+class TriggerList( object ):
+    def __init__( self ):
+        self.CAMT_seq = []
+        self.CAMT_timestamp = []
+        self.GPOS_Lat = []
+        self.GPOS_Lon = []
+        self.GPOS_Alt = []
+
+class ImageList( object ):
+    def __init__( self ):
+        self.jpg = []
+        self.raw = []
+
+def to_degree(value, loc):
+    if value < 0:
+        loc_value = loc[0]
+    elif value > 0:
+        loc_value = loc[1]
+    else:
+        loc_value = ""
+    absolute_value = abs(value)
+    deg =  int(absolute_value)
+    t1 = (absolute_value-deg)*60
+    min = int(t1)
+    sec = round((t1 - min)* 60, 5)
+    return (deg, min, sec, loc_value)
+
+def SetGpsLocation(file_name, lat, lng):
+    """
+        Adding GPS tag
+
+    """
+    lat_deg = to_degree(lat, ["S", "N"])
+    lng_deg = to_degree(lng, ["W", "E"])
+
+    exiv_lat = (pyexiv2.Rational(lat_deg[0] * 60 + lat_deg[1], 60), pyexiv2.Rational(lat_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
+    exiv_lng = (pyexiv2.Rational(lng_deg[0] * 60 + lng_deg[1], 60), pyexiv2.Rational(lng_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
+
+    exiv_image = pyexiv2.ImageMetadata(file_name)
+    exiv_image.read()
+
+    exiv_image["Exif.GPSInfo.GPSLatitude"] = exiv_lat
+    exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
+    exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng
+    exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3]
+    exiv_image["Exif.Image.GPSTag"] = 654
+    exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
+    exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
+
+    exiv_image.write(True)
+
+def LoadPX4log(px4_log_file):
+    """
+       load px4 log file and extract trigger locations
+    """
+    os.system('python sdlog2_dump.py ' + px4_log_file + ' -f log.csv')
+    f = open('log.csv', 'rb')
+    reader = csv.reader(f)
+    headers = reader.next()
+    line = {}
+    for h in headers:
+        line[h] = []
+
+    for row in reader:
+        for h, v in zip(headers, row):
+            line[h].append(v)
+
+    trigger_list = TriggerList()
+    for seq in range(0, len(line['CAMT_seq']) - 1):
+        if line['CAMT_seq'][seq] != line['CAMT_seq'][seq + 1]:
+            trigger_list.CAMT_seq.append(line['CAMT_seq'][seq + 1])
+            trigger_list.CAMT_timestamp.append(line['CAMT_timestamp'][seq + 1])
+            trigger_list.GPOS_Lat.append(line['GPOS_Lat'][seq + 1])
+            trigger_list.GPOS_Lon.append(line['GPOS_Lon'][seq + 1])
+            trigger_list.GPOS_Alt.append(line['GPOS_Alt'][seq + 1])
+    return trigger_list
+
+def LoadImageList(input_folder):
+    """
+       load the image list
+    """
+    image_list = ImageList()
+    for jpg_image in glob.glob(input_folder + "/*.jpg"):
+        image_list.jpg.append(jpg_image)
+    for jpg_image in glob.glob(input_folder + "/*.JPG"):
+        image_list.jpg.append(jpg_image)
+    for raw_image in glob.glob(input_folder + "/*.RC"):
+        image_list.raw.append(raw_image)
+    if len(image_list.jpg) != len(image_list.raw) and len(image_list.jpg) * len(image_list.raw) != 0:
+        print("Unequal number of jpg and raw images")
+    if len(image_list.jpg) == 0 and len(image_list.raw) == 0:
+        print("No images found")
+    return image_list
+
+def FilterTrigger(trigger_list, image_list):
+    """
+       filter triggers to allow exact matching with recorded images
+    """
+    if len(image_list.jpg) != len(trigger_list.CAMT_seq) and len(image_list.raw) != len(trigger_list.CAMT_seq):
+        # filter trigger list to match the number of pics
+        print("No trigger filter implemented yet.")
+
+    return trigger_list
+
+def TagImages(trigger_list, image_list, output_folder):
+    """
+       load px4 log file and extract trigger locations
+    """
+    print len(image_list.jpg)
+    print len(trigger_list.GPOS_Lat)
+    print len(trigger_list.GPOS_Lon)
+    for image in range(len(image_list.jpg)):
+        base_path, filename = os.path.split(image_list.jpg[image])
+        copyfile(image_list.jpg[image], output_folder + "/" + filename)
+        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]))
+    for image in range(len(image_list.raw)):
+        base_path, filename = os.path.split(image_list.raw[image])
+        copyfile(image_list.raw[image], output_folder + "/" + filename)
+        SetGpsLocation(output_folder + "/" + filename, float(trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]))
+
+def main():
+    """
+        Main method
+    """
+    parser = OptionParser()
+    parser.add_option("-l", "--logfile", dest = "LogFile", 
+                    help = "PX4 log file containing recorded positions", 
+                    metavar = "string")
+    parser.add_option("-i", "--input", dest = "InputFolder",
+                    help = "Input folder containing untagged images",
+                    type = "string")
+    parser.add_option("-o", "--output", dest = "OutputFolder",
+                    help = "Output folder to contain tagged images",
+                    type = "string")
+    
+    (options, args) = parser.parse_args()
+    if not options.LogFile:
+        print "please type python " \
+              "geotagging.py --logfile=[filename] --intput=[folder] [--output=[folder]]"     
+    elif not options.InputFolder:
+        print "please type python " \
+              "geotagging.py --logfile=[filename] --intput=[folder] [--output=[folder]]s"
+    else:
+        trigger_list = LoadPX4log(options.LogFile)
+        image_list = LoadImageList(options.InputFolder)
+
+        if not options.OutputFolder:
+            options.OutputFolder = "imagesWithTag"
+        if not os.path.exists(options.OutputFolder):
+            os.makedirs(options.OutputFolder)
+
+        trigger_list = FilterTrigger(trigger_list, image_list)
+
+        TagImages(trigger_list, image_list, options.OutputFolder)
+
+if __name__ == "__main__":
+    main()