geotagging backend

qgroundcontrol.pro

@@ -340,7 +340,8 @@ HEADERS += \
     src/AutoPilotPlugins/APM/APMAirframeLoader.h \
     src/QmlControls/QGCImageProvider.h \
     src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h \
-    src/PositionManager/PositionManager.h
+    src/PositionManager/PositionManager.h \
+    src/AnalyzeView/ExifParser.h
 
 AndroidBuild {
 HEADERS += \
@@ -506,7 +507,8 @@ SOURCES += \
     src/QmlControls/QGCImageProvider.cc \
     src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc \
     src/PositionManager/SimulatedPosition.cc \
-    src/PositionManager/PositionManager.cpp
+    src/PositionManager/PositionManager.cpp \
+    src/AnalyzeView/ExifParser.cpp
 
 DebugBuild {
 SOURCES += \

src/AnalyzeView/ExifParser.cpp

+#include "ExifParser.h"
+#include <QtEndian>
+#include <QDateTime>
+
+ExifParser::ExifParser()
+{
+
+}
+
+ExifParser::~ExifParser()
+{
+
+}
+
+double ExifParser::readTime(QByteArray& buf)
+{
+    char tiffHeader[] = {0x49,0x49,0x2A,0x00};
+    char createDateHeader[] = {0x04,0x90,0x02,0x00};
+
+    // find header position
+    uint32_t tiffHeaderIndex = buf.indexOf(tiffHeader);
+
+    // find creation date header index
+    uint32_t createDateHeaderIndex = buf.indexOf(createDateHeader);
+
+    // extract size of date-time string, -1 accounting for null-termination
+    uint32_t* sizeString = reinterpret_cast<uint32_t*>(buf.mid(createDateHeaderIndex + 4, 4).data());
+    uint32_t createDateStringSize = qFromLittleEndian(*sizeString) - 1;
+
+    // extract location of date-time string
+    uint32_t* dataIndex = reinterpret_cast<uint32_t*>(buf.mid(createDateHeaderIndex + 8, 4).data());
+    uint32_t createDateStringDataIndex = qFromLittleEndian(*dataIndex) + tiffHeaderIndex;
+
+    // read out data of create date-time field
+    QString createDate = buf.mid(createDateStringDataIndex, createDateStringSize);
+
+    QStringList createDateList = createDate.split(' ');
+    if (createDateList.count() < 2) {
+        qWarning() << "Could not decode creation time and date: " << createDateList;
+        return -1.0;
+    }
+    QStringList dateList = createDateList[0].split(':');
+    if (dateList.count() < 3) {
+        qWarning() << "Could not decode creation date: " << dateList;
+        return -1.0;
+    }
+    QStringList timeList = createDateList[1].split(':');
+    if (timeList.count() < 3) {
+        qWarning() << "Could not decode creation time: " << timeList;
+        return -1.0;
+    }
+    QDate date(dateList[0].toInt(), dateList[1].toInt(), dateList[2].toInt());
+    QTime time(timeList[0].toInt(), timeList[1].toInt(), timeList[2].toInt());
+    QDateTime tagTime(date, time);
+    return tagTime.toMSecsSinceEpoch()/1000.0;
+}
+
+bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate)
+{
+    char app1Header[2] = {0xff, 0xe1};
+    uint32_t app1HeaderInd = buf.indexOf(app1Header);
+    uint16_t *conversionPointer = reinterpret_cast<uint16_t *>(buf.mid(app1HeaderInd + 2, 2).data());
+    uint16_t app1Size = *conversionPointer;
+    uint16_t app1SizeEndian = qFromBigEndian(app1Size) + 0xa5;  // change wrong endian
+    char tiffHeader[4] = {0x49, 0x49, 0x2A, 0x00};
+    uint32_t tiffHeaderInd = buf.indexOf(tiffHeader);
+    conversionPointer = reinterpret_cast<uint16_t *>(buf.mid(tiffHeaderInd + 8, 2).data());
+    uint16_t numberOfTiffFields  = *conversionPointer;
+    uint32_t nextIfdOffsetInd = tiffHeaderInd + 10 + 12 * (numberOfTiffFields);
+    conversionPointer = reinterpret_cast<uint16_t *>(buf.mid(nextIfdOffsetInd, 2).data());
+    uint16_t nextIfdOffset = *conversionPointer;
+
+    // Definition of usefull unions and structs
+    union char2uint32_u {
+        char c[4];
+        uint32_t i;
+    };
+    union char2uint16_u {
+        char c[2];
+        uint16_t i;
+    };
+    // This struct describes a standart field used in exif files
+    struct field_s {
+        uint16_t tagID;  // Describes which information is added here, e.g. GPS Lat
+        uint16_t type;  // Describes the data type, e.g. string, uint8_t,...
+        uint32_t size;  // Describes the size
+        uint32_t content;  // Either contains the information, or the offset to the exif header where the information is stored (if 32 bits is not enough)
+    };
+    // This struct contains all the fields that we want to add to the image
+    struct fields_s {
+        field_s gpsVersion;
+        field_s gpsLatRef;
+        field_s gpsLat;
+        field_s gpsLonRef;
+        field_s gpsLon;
+        field_s gpsAltRef;
+        field_s gpsAlt;
+        field_s gpsMapDatum;
+        uint32_t finishedDataField;
+    };
+    // These are the additional information that can not be put into a single uin32_t
+    struct extended_s {
+        uint32_t gpsLat[6];
+        uint32_t gpsLon[6];
+        uint32_t gpsAlt[2];
+        char mapDatum[7];// = {0x57,0x47,0x53,0x2D,0x38,0x34,0x00};
+    };
+    // This struct contains all the information we want to add to the image
+    struct readable_s {
+        fields_s fields;
+        extended_s extendedData;
+    };
+
+    // This union is used because for writing the information we have to use a char array, but we still want the information to be available in a more descriptive way
+    union {
+        char c[0xa3];
+        readable_s readable;
+    } gpsData;
+
+
+    char2uint32_u gpsIFDInd;
+    gpsIFDInd.i = nextIfdOffset;
+
+    // this will stay constant
+    char gpsInfo[12] = {0x25, 0x88, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, gpsIFDInd.c[0], gpsIFDInd.c[1], gpsIFDInd.c[2], gpsIFDInd.c[3]};
+
+    // filling values to gpsData
+    uint32_t gpsDataExtInd = gpsIFDInd.i + 2 + sizeof(fields_s);
+
+    // Filling up the fields with the corresponding values
+    gpsData.readable.fields.gpsVersion.tagID = 0;
+    gpsData.readable.fields.gpsVersion.type = 1;
+    gpsData.readable.fields.gpsVersion.size = 4;
+    gpsData.readable.fields.gpsVersion.content = 2;
+
+    gpsData.readable.fields.gpsLatRef.tagID = 1;
+    gpsData.readable.fields.gpsLatRef.type = 2;
+    gpsData.readable.fields.gpsLatRef.size = 2;
+    gpsData.readable.fields.gpsLatRef.content = coordinate.latitude() > 0 ? 'N' : 'S';
+
+    gpsData.readable.fields.gpsLat.tagID = 2;
+    gpsData.readable.fields.gpsLat.type = 5;
+    gpsData.readable.fields.gpsLat.size = 3;
+    gpsData.readable.fields.gpsLat.content = gpsDataExtInd;
+
+    gpsData.readable.fields.gpsLonRef.tagID = 3;
+    gpsData.readable.fields.gpsLonRef.type = 2;
+    gpsData.readable.fields.gpsLonRef.size = 2;
+    gpsData.readable.fields.gpsLonRef.content = coordinate.longitude() > 0 ? 'E' : 'W';
+
+    gpsData.readable.fields.gpsLon.tagID = 4;
+    gpsData.readable.fields.gpsLon.type = 5;
+    gpsData.readable.fields.gpsLon.size = 3;
+    gpsData.readable.fields.gpsLon.content = gpsDataExtInd + 6 * 4;
+
+    gpsData.readable.fields.gpsAltRef.tagID = 5;
+    gpsData.readable.fields.gpsAltRef.type = 2;
+    gpsData.readable.fields.gpsAltRef.size = 2;
+    gpsData.readable.fields.gpsAltRef.content = 0x00;
+
+    gpsData.readable.fields.gpsAlt.tagID = 6;
+    gpsData.readable.fields.gpsAlt.type = 5;
+    gpsData.readable.fields.gpsAlt.size = 1;
+    gpsData.readable.fields.gpsAlt.content = gpsDataExtInd + 6 * 4 * 2;
+
+    gpsData.readable.fields.gpsMapDatum.tagID = 18;
+    gpsData.readable.fields.gpsMapDatum.type = 2;
+    gpsData.readable.fields.gpsMapDatum.size = 7;
+    gpsData.readable.fields.gpsMapDatum.content = gpsDataExtInd + 6 * 4 * 2 + 2 * 4;
+
+    gpsData.readable.fields.finishedDataField = 0;
+
+    // Filling up the additional information that does not fit into the fields
+    gpsData.readable.extendedData.gpsLat[0] = abs(static_cast<int>(coordinate.latitude()));
+    gpsData.readable.extendedData.gpsLat[1] = 1;
+    gpsData.readable.extendedData.gpsLat[2] = static_cast<int>((fabs(coordinate.latitude()) - std::floor(fabs(coordinate.latitude()))) * 60000.0);
+    gpsData.readable.extendedData.gpsLat[3] = 1000;
+    gpsData.readable.extendedData.gpsLat[4] = 0;
+    gpsData.readable.extendedData.gpsLat[5] = 1;
+
+    gpsData.readable.extendedData.gpsLon[0] = abs(static_cast<int>(coordinate.longitude()));
+    gpsData.readable.extendedData.gpsLon[1] = 1;
+    gpsData.readable.extendedData.gpsLon[2] = static_cast<int>((fabs(coordinate.longitude()) - std::floor(fabs(coordinate.longitude()))) * 60000.0);
+    gpsData.readable.extendedData.gpsLon[3] = 1000;
+    gpsData.readable.extendedData.gpsLon[4] = 0;
+    gpsData.readable.extendedData.gpsLon[5] = 1;
+
+    gpsData.readable.extendedData.gpsAlt[0] = coordinate.altitude() * 100;
+    gpsData.readable.extendedData.gpsAlt[1] = 100;
+    gpsData.readable.extendedData.mapDatum[0] = 'W';
+    gpsData.readable.extendedData.mapDatum[1] = 'G';
+    gpsData.readable.extendedData.mapDatum[2] = 'S';
+    gpsData.readable.extendedData.mapDatum[3] = '-';
+    gpsData.readable.extendedData.mapDatum[4] = '8';
+    gpsData.readable.extendedData.mapDatum[5] = '4';
+    gpsData.readable.extendedData.mapDatum[6] = 0x00;
+
+    // remove 12 spaces from image description, as otherwise we need to loop through every field and correct the new address values
+    buf.remove(nextIfdOffsetInd + 4, 12);
+    // TODO correct size in image description
+    // insert Gps Info to image file
+    buf.insert(nextIfdOffsetInd, gpsInfo, 12);
+    char numberOfFields[2] = {0x08, 0x00};
+    // insert number of gps specific fields that we want to add
+    buf.insert(gpsIFDInd.i + tiffHeaderInd, numberOfFields, 2);
+    // insert the gps data
+    buf.insert(gpsIFDInd.i + 2 + tiffHeaderInd, gpsData.c, 0xa3);
+
+    // update the new file size and exif offsets
+    char2uint16_u converter;
+    converter.i = qToBigEndian(app1SizeEndian);
+    buf.replace(app1HeaderInd + 2, 2, converter.c, 2);
+    converter.i = nextIfdOffset + 12 + 0xa5;
+    buf.replace(nextIfdOffsetInd + 12, 2, converter.c, 2);
+
+    converter.i = (numberOfTiffFields) + 1;
+    buf.replace(tiffHeaderInd + 8, 2, converter.c, 2);
+    return true;
+}

src/AnalyzeView/ExifParser.h

+#ifndef EXIFPARSER_H
+#define EXIFPARSER_H
+
+#include <QGeoCoordinate>
+#include <QDebug>
+
+class ExifParser
+{
+public:
+    ExifParser();
+    ~ExifParser();
+    double readTime(QByteArray& buf);
+    bool write(QByteArray& data, QGeoCoordinate coordinate);
+};
+
+#endif // EXIFPARSER_H

src/AnalyzeView/GeoTagController.cc

@@ -9,6 +9,9 @@
 
 #include "GeoTagController.h"
 #include "QGCFileDialog.h"
+#include "ExifParser.h"
+#include <QtEndian>
+#include <cfloat>
 
 GeoTagController::GeoTagController(void)
     : _progress(0)
@@ -73,15 +76,191 @@ void GeoTagWorker::run(void)
     _cancel = false;
     emit progressChanged(0);
 
+    //used to time operations to get a feel for how much to progress the progressBar
+    QElapsedTimer timerTotal;
+    QElapsedTimer timerLoadImages;
+    QElapsedTimer timerParseExif;
+    QElapsedTimer timerFilter;
+    QElapsedTimer timerLoadLogFile;
+    QElapsedTimer timerGeotag;
+
+    timerTotal.start();
+
+
+    //////////// Load Images
+    timerLoadImages.start();
+
+    QDir imageDirectory = QDir(_imageDirectory);
+    if(!imageDirectory.exists()) {
+        emit error(tr("Cannot find the image directory"));
+        return;
+    }
+    if(!imageDirectory.mkdir(_imageDirectory + "/TAGGED")) {
+        emit error(tr("Images have already been tagged"));
+        return;
+    }
+
+    imageDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable);
+    imageDirectory.setSorting(QDir::Name);
+    QStringList nameFilters;
+    nameFilters << "*.jpg" << "*.JPG";
+    imageDirectory.setNameFilters(nameFilters);
+
+    QFileInfoList imageList = imageDirectory.entryInfoList();
+    if(imageList.isEmpty()) {
+        emit error(tr("The image directory doesn't contain images, make sure your images are of the JPG format"));
+        return;
+    }
+
+    _imageBuffers.clear();
+    for (int i = 0; i < imageList.size(); ++i) {
+        QFile file(imageList.at(i).absoluteFilePath());
+        if (!file.open(QIODevice::ReadOnly)) {
+            continue;
+        }
+        _imageBuffers.append(QByteArray(file.readAll()));
+        file.close();
+    }
+
+    qWarning() << "Image loading time elapsed: " << timerLoadImages.elapsed() << " milliseconds";
+
+    ////////// Parse exif data
+    timerParseExif.start();
+
+    // Parse EXIF
+    ExifParser exifParser;
+    _tagTime.clear();
+    for (int i = 0; i < _imageBuffers.count(); i++) {
+         _tagTime.append(exifParser.readTime(_imageBuffers[i]));
+    }
+
+    qWarning() << "Parse exif data time elapsed: " << timerParseExif.elapsed() << " milliseconds";
+
+    ////////// Load PX4 log
+    timerLoadLogFile.start();
+
+    _geoRef.clear();
+    _triggerTime.clear();
+    if (!parsePX4Log()) {
+        qWarning() << "Geotagging failed";
+        return;
+    }
+    qWarning() << "Found " << _geoRef.count() << " trigger logs.";
+
+    qWarning() << "Log loading time elapsed: " << timerLoadLogFile.elapsed() << " milliseconds";
+
+    ////////// Filter Trigger
+    timerFilter.start();
+
+    if (!triggerFiltering()) {
+        qWarning() << "Geotagging failed";
+        return;
+    }
+
+    qWarning() << "Filter time elapsed: " << timerFilter.elapsed() << " milliseconds";
+
+    //////////// Tag images
+    timerGeotag.start();
+
+    for(int i = 0; i < _imageIndices.count() && i < _triggerIndices.count() && i < imageList.count(); i++) {
+        if (!exifParser.write(_imageBuffers[_imageIndices[i]], _geoRef[_triggerIndices[i]])) {
+            _cancel = true;
+            break;
+        } else {
+            QFile file(_imageDirectory + "/TAGGED/" + imageList[_imageIndices[i]].fileName());
+            if (file.open( QFile::WriteOnly)) {
+                file.write(_imageBuffers[_imageIndices[i]]);
+                file.close();
+            }
+        }
+    }
+
+    qWarning() << "Tagging images time elapsed: " << timerGeotag.elapsed() << " milliseconds";
+
     for (int i=0; i<10;i++) {
         if (_cancel) {
             emit error(tr("Tagging cancelled"));
             return;
         }
         emit progressChanged(i*10);
-        sleep(1);
+        //sleep(1);
     }
 
+    qWarning() << "Total time elapsed: " << timerTotal.elapsed() << " milliseconds";
     emit progressChanged(100);
     emit taggingComplete();
 }
+
+bool GeoTagWorker::parsePX4Log()
+{
+    // general message header
+    // char header[] = {0xA3, 0x95, 0x00};
+    // header for GPOS message
+    char gposHeader[] = {0xA3, 0x95, 0x10, 0x00};
+    int gposOffsets[3] = {3, 7, 11};
+    int gposLengths[3] = {4, 4, 4};
+    // header for trigger message
+    char triggerHeader[] = {0xA3, 0x95, 0x37, 0x00};
+    int triggerOffsets[2] = {3, 11};
+    int triggerLengths[2] = {8, 4};
+    // load log
+    QFile file(_logFile);
+    if (!file.open(QIODevice::ReadOnly)) {
+        qWarning() << "Could not open log file";
+        return false;
+    }
+    QByteArray log = file.readAll();
+    file.close();
+
+    // extract trigger data
+    int index = 1;
+    int sequence = -1;
+    QGeoCoordinate lastCoordinate;
+    while(index < log.count() - 1) {
+        int gposIndex = log.indexOf(gposHeader, index + 1);
+        int triggerIndex = log.indexOf(triggerHeader, index + 1);
+        // check for whether last entry has been passed
+        if ((gposIndex < 0 && triggerIndex < 0) || (gposIndex >= log.count() - 1 && triggerIndex >= log.count() - 1)) {
+            break;
+        } else if (gposIndex < 0) {
+            gposIndex = triggerIndex + 1;
+        } else if (triggerIndex < 0) {
+            triggerIndex = gposIndex + 1;
+        }
+        // extract next entry, gpos or trigger
+        if (gposIndex < triggerIndex) {
+            // TODO: somehow verify that the gposIndex is really the header of a gpos message
+            int32_t* lat = reinterpret_cast<int32_t*>(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data());
+            double latitude = static_cast<double>(qFromLittleEndian(*lat))/1.0e7;
+            lastCoordinate.setLatitude(latitude);
+            int32_t* lon = reinterpret_cast<int32_t*>(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data());
+            double longitude = static_cast<double>(qFromLittleEndian(*lon))/1.0e7;
+            longitude = fmod(180.0 + longitude, 360.0) - 180.0;
+            lastCoordinate.setLongitude(longitude);
+            float* alt = reinterpret_cast<float*>(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data());
+            lastCoordinate.setAltitude(qFromLittleEndian(*alt));
+            index = gposIndex;
+        } else {
+            uint64_t* time = reinterpret_cast<uint64_t*>(log.mid(triggerIndex + triggerOffsets[0], triggerLengths[0]).data());
+            double timeDouble = static_cast<double>(qFromLittleEndian(*time));
+            uint32_t* seq = reinterpret_cast<uint32_t*>(log.mid(triggerIndex + triggerOffsets[1], triggerLengths[1]).data());
+            int seqInt = static_cast<int>(qFromLittleEndian(*seq));
+            if (sequence < seqInt && sequence + 20 > seqInt) { // assume that logging has not skipped more than 20 triggers. this prevents wrong header detection
+               _geoRef.append(lastCoordinate);
+               _triggerTime.append(timeDouble/1000000.0);
+               sequence = seqInt;
+            }
+            index = triggerIndex;
+        }
+    }
+    return true;
+}
+
+bool GeoTagWorker::triggerFiltering()
+{
+    for (int i = 0; i < _triggerTime.count() && i < _tagTime.count(); i++) {
+        _triggerIndices.append(i);
+        _imagesIndices.append(i);
+    }
+    return true;
+}

src/AnalyzeView/GeoTagController.h

@@ -13,6 +13,9 @@
 #include <QObject>
 #include <QString>
 #include <QThread>
+#include <QElapsedTimer>
+#include <QDebug>
+#include <QGeoCoordinate>
 
 class GeoTagWorker : public QThread
 {
@@ -27,7 +30,7 @@ public:
     void setLogFile(const QString& logFile) { _logFile = logFile; }
     void setImageDirectory(const QString& imageDirectory) { _imageDirectory = imageDirectory; }
 
-    void cancellTagging(void) { _cancel = true; }
+    void cancelTagging(void) { _cancel = true; }
 
 protected:
     void run(void) final;
@@ -38,9 +41,19 @@ signals:
     void progressChanged(double progress);
 
 private:
+    bool parsePX4Log();
+    bool triggerFiltering();
+
+
     bool    _cancel;
     QString _logFile;
     QString _imageDirectory;
+    QList<QByteArray> _imageBuffers;
+    QList<double> _tagTime;
+    QList<QGeoCoordinate> _geoRef;
+    QList<double> _triggerTime;
+    QList<int> _imageIndices;
+    QList<int> _triggerIndices;
 };
 
 /// Controller for GeoTagPage.qml. Supports geotagging images based on logfile camera tags.
@@ -67,7 +80,7 @@ public:
     Q_INVOKABLE void pickLogFile(void);
     Q_INVOKABLE void pickImageDirectory(void);
     Q_INVOKABLE void startTagging(void);
-    Q_INVOKABLE void cancelTagging(void) { _worker.cancellTagging(); }
+    Q_INVOKABLE void cancelTagging(void) { _worker.cancelTagging(); }
 
     QString logFile         (void) const { return _worker.logFile(); }
     QString imageDirectory  (void) const { return _worker.imageDirectory(); }