From a82877ee0e15dc38c63614605fe3c9eb78f050b4 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 14 Oct 2016 16:54:38 +0200 Subject: [PATCH] geotagging backend --- qgroundcontrol.pro | 6 +- src/AnalyzeView/ExifParser.cpp | 219 ++++++++++++++++++++++++++++ src/AnalyzeView/ExifParser.h | 16 ++ src/AnalyzeView/GeoTagController.cc | 181 ++++++++++++++++++++++- src/AnalyzeView/GeoTagController.h | 17 ++- 5 files changed, 434 insertions(+), 5 deletions(-) create mode 100644 src/AnalyzeView/ExifParser.cpp create mode 100644 src/AnalyzeView/ExifParser.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bf438801a..c919ffda6 100644 --- a/qgroundcontrol.pro +++ b/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 += \ diff --git a/src/AnalyzeView/ExifParser.cpp b/src/AnalyzeView/ExifParser.cpp new file mode 100644 index 000000000..083aff242 --- /dev/null +++ b/src/AnalyzeView/ExifParser.cpp @@ -0,0 +1,219 @@ +#include "ExifParser.h" +#include +#include + +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(buf.mid(createDateHeaderIndex + 4, 4).data()); + uint32_t createDateStringSize = qFromLittleEndian(*sizeString) - 1; + + // extract location of date-time string + uint32_t* dataIndex = reinterpret_cast(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(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(buf.mid(tiffHeaderInd + 8, 2).data()); + uint16_t numberOfTiffFields = *conversionPointer; + uint32_t nextIfdOffsetInd = tiffHeaderInd + 10 + 12 * (numberOfTiffFields); + conversionPointer = reinterpret_cast(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(coordinate.latitude())); + gpsData.readable.extendedData.gpsLat[1] = 1; + gpsData.readable.extendedData.gpsLat[2] = static_cast((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(coordinate.longitude())); + gpsData.readable.extendedData.gpsLon[1] = 1; + gpsData.readable.extendedData.gpsLon[2] = static_cast((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; +} diff --git a/src/AnalyzeView/ExifParser.h b/src/AnalyzeView/ExifParser.h new file mode 100644 index 000000000..9a29da380 --- /dev/null +++ b/src/AnalyzeView/ExifParser.h @@ -0,0 +1,16 @@ +#ifndef EXIFPARSER_H +#define EXIFPARSER_H + +#include +#include + +class ExifParser +{ +public: + ExifParser(); + ~ExifParser(); + double readTime(QByteArray& buf); + bool write(QByteArray& data, QGeoCoordinate coordinate); +}; + +#endif // EXIFPARSER_H diff --git a/src/AnalyzeView/GeoTagController.cc b/src/AnalyzeView/GeoTagController.cc index f9314c31e..820be6b3d 100644 --- a/src/AnalyzeView/GeoTagController.cc +++ b/src/AnalyzeView/GeoTagController.cc @@ -9,6 +9,9 @@ #include "GeoTagController.h" #include "QGCFileDialog.h" +#include "ExifParser.h" +#include +#include 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(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data()); + double latitude = static_cast(qFromLittleEndian(*lat))/1.0e7; + lastCoordinate.setLatitude(latitude); + int32_t* lon = reinterpret_cast(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data()); + double longitude = static_cast(qFromLittleEndian(*lon))/1.0e7; + longitude = fmod(180.0 + longitude, 360.0) - 180.0; + lastCoordinate.setLongitude(longitude); + float* alt = reinterpret_cast(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data()); + lastCoordinate.setAltitude(qFromLittleEndian(*alt)); + index = gposIndex; + } else { + uint64_t* time = reinterpret_cast(log.mid(triggerIndex + triggerOffsets[0], triggerLengths[0]).data()); + double timeDouble = static_cast(qFromLittleEndian(*time)); + uint32_t* seq = reinterpret_cast(log.mid(triggerIndex + triggerOffsets[1], triggerLengths[1]).data()); + int seqInt = static_cast(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; +} diff --git a/src/AnalyzeView/GeoTagController.h b/src/AnalyzeView/GeoTagController.h index a2820fa35..ef870b174 100644 --- a/src/AnalyzeView/GeoTagController.h +++ b/src/AnalyzeView/GeoTagController.h @@ -13,6 +13,9 @@ #include #include #include +#include +#include +#include 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 _imageBuffers; + QList _tagTime; + QList _geoRef; + QList _triggerTime; + QList _imageIndices; + QList _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(); } -- 2.22.0