GeoTagController.cc 13.6 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1 2 3 4 5 6 7 8 9 10
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "GeoTagController.h"
Andreas Bircher's avatar
Andreas Bircher committed
11
#include "ExifParser.h"
12 13
#include "QGCFileDialog.h"
#include "QGCLoggingCategory.h"
14
#include "MainWindow.h"
15
#include <math.h>
Andreas Bircher's avatar
Andreas Bircher committed
16
#include <QtEndian>
17 18
#include <QMessageBox>
#include <QDebug>
Andreas Bircher's avatar
Andreas Bircher committed
19
#include <cfloat>
Don Gagne's avatar
Don Gagne committed
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37

GeoTagController::GeoTagController(void)
    : _progress(0)
    , _inProgress(false)
{
    connect(&_worker, &GeoTagWorker::progressChanged,   this, &GeoTagController::_workerProgressChanged);
    connect(&_worker, &GeoTagWorker::error,             this, &GeoTagController::_workerError);
    connect(&_worker, &GeoTagWorker::started,           this, &GeoTagController::inProgressChanged);
    connect(&_worker, &GeoTagWorker::finished,          this, &GeoTagController::inProgressChanged);
}

GeoTagController::~GeoTagController()
{

}

void GeoTagController::pickLogFile(void)
{
38
    QString filename = QGCFileDialog::getOpenFileName(MainWindow::instance(), "Select log file load", QString(), "PX4 log file (*.px4log);;All Files (*.*)");
Don Gagne's avatar
Don Gagne committed
39 40 41 42 43 44 45 46
    if (!filename.isEmpty()) {
        _worker.setLogFile(filename);
        emit logFileChanged(filename);
    }
}

void GeoTagController::pickImageDirectory(void)
{
47
    QString dir = QGCFileDialog::getExistingDirectory(MainWindow::instance(), "Select image directory");
Don Gagne's avatar
Don Gagne committed
48 49 50 51 52 53
    if (!dir.isEmpty()) {
        _worker.setImageDirectory(dir);
        emit imageDirectoryChanged(dir);
    }
}

54 55
void GeoTagController::pickSaveDirectory(void)
{
56
    QString dir = QGCFileDialog::getExistingDirectory(MainWindow::instance(), "Select save directory");
57 58 59 60 61 62
    if (!dir.isEmpty()) {
        _worker.setSaveDirectory(dir);
        emit saveDirectoryChanged(dir);
    }
}

Don Gagne's avatar
Don Gagne committed
63 64 65 66
void GeoTagController::startTagging(void)
{
    _errorMessage.clear();
    emit errorMessageChanged(_errorMessage);
67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128

    QDir imageDirectory = QDir(_worker.imageDirectory());
    if(!imageDirectory.exists()) {
        _errorMessage = tr("Cannot find the image directory");
        emit errorMessageChanged(_errorMessage);
        return;
    }
    if(_worker.saveDirectory() == "") {
        if(!imageDirectory.mkdir(_worker.imageDirectory() + "/TAGGED")) {
            QMessageBox msgBox(QMessageBox::Question,
                               tr("Images have alreay been tagged."),
                               tr("The images have already been tagged. Do you want to replace the previously tagged images?"),
                               QMessageBox::Cancel);
            msgBox.setWindowModality(Qt::ApplicationModal);
            msgBox.addButton(tr("Replace"), QMessageBox::ActionRole);
            if (msgBox.exec() == QMessageBox::Cancel) {
                _errorMessage = tr("Images have already been tagged");
                emit errorMessageChanged(_errorMessage);
                return;
            }
            QDir oldTaggedFolder = QDir(_worker.imageDirectory() + "/TAGGED");
            oldTaggedFolder.removeRecursively();
            if(!imageDirectory.mkdir(_worker.imageDirectory() + "/TAGGED")) {
                _errorMessage = tr("Couldn't replace the previously tagged images");
                emit errorMessageChanged(_errorMessage);
                return;
            }
        }
    } else {
        QDir saveDirectory = QDir(_worker.saveDirectory());
        if(!saveDirectory.exists()) {
            _errorMessage = tr("Cannot find the save directory");
            emit errorMessageChanged(_errorMessage);
            return;
        }
        saveDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable);
        QStringList nameFilters;
        nameFilters << "*.jpg" << "*.JPG";
        saveDirectory.setNameFilters(nameFilters);
        QStringList imageList = saveDirectory.entryList();
        if(!imageList.isEmpty()) {
            QMessageBox msgBox(QMessageBox::Question,
                               tr("Save folder not empty."),
                               tr("The save folder already contains images. Do you want to replace them?"),
                               QMessageBox::Cancel);
            msgBox.setWindowModality(Qt::ApplicationModal);
            msgBox.addButton(tr("Replace"), QMessageBox::ActionRole);
            if (msgBox.exec() == QMessageBox::Cancel) {
                _errorMessage = tr("Save folder not empty");
                emit errorMessageChanged(_errorMessage);
                return;
            }
            foreach(QString dirFile, imageList)
            {
                if(!saveDirectory.remove(dirFile)) {
                    _errorMessage = tr("Couldn't replace the existing images");
                    emit errorMessageChanged(_errorMessage);
                    return;
                }
            }
        }
    }
Don Gagne's avatar
Don Gagne committed
129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
    _worker.start();
}

void GeoTagController::_workerProgressChanged(double progress)
{
    _progress = progress;
    emit progressChanged(progress);
}

void GeoTagController::_workerError(QString errorMessage)
{
    _errorMessage = errorMessage;
    emit errorMessageChanged(errorMessage);
}

GeoTagWorker::GeoTagWorker(void)
    : _cancel(false)
146 147 148
    , _logFile("")
    , _imageDirectory("")
    , _saveDirectory("")
Don Gagne's avatar
Don Gagne committed
149 150 151 152 153 154 155
{

}

void GeoTagWorker::run(void)
{
    _cancel = false;
156 157
    emit progressChanged(1);
    double nSteps = 5;
Andreas Bircher's avatar
Andreas Bircher committed
158

159 160
    // Load Images
    _imageList.clear();
Andreas Bircher's avatar
Andreas Bircher committed
161 162 163 164 165 166
    QDir imageDirectory = QDir(_imageDirectory);
    imageDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable);
    imageDirectory.setSorting(QDir::Name);
    QStringList nameFilters;
    nameFilters << "*.jpg" << "*.JPG";
    imageDirectory.setNameFilters(nameFilters);
167 168
    _imageList = imageDirectory.entryInfoList();
    if(_imageList.isEmpty()) {
Andreas Bircher's avatar
Andreas Bircher committed
169 170 171
        emit error(tr("The image directory doesn't contain images, make sure your images are of the JPG format"));
        return;
    }
172
    emit progressChanged((100/nSteps));
Andreas Bircher's avatar
Andreas Bircher committed
173

174 175 176 177 178
    // Parse EXIF
    ExifParser exifParser;
    _tagTime.clear();
    for (int i = 0; i < _imageList.size(); ++i) {
        QFile file(_imageList.at(i).absoluteFilePath());
Andreas Bircher's avatar
Andreas Bircher committed
179
        if (!file.open(QIODevice::ReadOnly)) {
180 181
            emit error(tr("Geotagging failed. Couldn't open an image."));
            return;
Andreas Bircher's avatar
Andreas Bircher committed
182
        }
183
        QByteArray imageBuffer = file.readAll();
Andreas Bircher's avatar
Andreas Bircher committed
184 185
        file.close();

186
        _tagTime.append(exifParser.readTime(imageBuffer));
Andreas Bircher's avatar
Andreas Bircher committed
187

188
        emit progressChanged((100/nSteps) + ((100/nSteps) / _imageList.size())*i);
Andreas Bircher's avatar
Andreas Bircher committed
189

190 191 192 193 194
        if (_cancel) {
            qCDebug(GeotaggingLog) << "Tagging cancelled";
            emit error(tr("Tagging cancelled"));
            return;
        }
Andreas Bircher's avatar
Andreas Bircher committed
195 196
    }

197
    // Load PX4 log
Andreas Bircher's avatar
Andreas Bircher committed
198 199 200
    _geoRef.clear();
    _triggerTime.clear();
    if (!parsePX4Log()) {
201 202 203 204 205 206 207 208 209
        if (_cancel) {
            qCDebug(GeotaggingLog) << "Tagging cancelled";
            emit error(tr("Tagging cancelled"));
            return;
        } else {
            qCDebug(GeotaggingLog) << "Log parsing failed";
            emit error(tr("Log parsing failed - tagging cancelled"));
            return;
        }
Andreas Bircher's avatar
Andreas Bircher committed
210
    }
211
    emit progressChanged(3*(100/nSteps));
Andreas Bircher's avatar
Andreas Bircher committed
212

213
    qCDebug(GeotaggingLog) << "Found " << _geoRef.count() << " trigger logs.";
Andreas Bircher's avatar
Andreas Bircher committed
214

215 216 217 218 219
    if (_cancel) {
        qCDebug(GeotaggingLog) << "Tagging cancelled";
        emit error(tr("Tagging cancelled"));
        return;
    }
Andreas Bircher's avatar
Andreas Bircher committed
220

221
    // Filter Trigger
Andreas Bircher's avatar
Andreas Bircher committed
222
    if (!triggerFiltering()) {
223 224
        qCDebug(GeotaggingLog) << "Geotagging failed in trigger filtering";
        emit error(tr("Geotagging failed in trigger filtering"));
Andreas Bircher's avatar
Andreas Bircher committed
225 226
        return;
    }
227
    emit progressChanged(4*(100/nSteps));
Andreas Bircher's avatar
Andreas Bircher committed
228

229 230 231 232 233
    if (_cancel) {
        qCDebug(GeotaggingLog) << "Tagging cancelled";
        emit error(tr("Tagging cancelled"));
        return;
    }
Andreas Bircher's avatar
Andreas Bircher committed
234

235 236 237 238 239 240 241 242 243 244 245
    // Tag images
    int maxIndex = std::min(_imageIndices.count(), _triggerIndices.count());
    maxIndex = std::min(maxIndex, _imageList.count());
    for(int i = 0; i < maxIndex; i++) {
        QFile fileRead(_imageList.at(_imageIndices[i]).absoluteFilePath());
        if (!fileRead.open(QIODevice::ReadOnly)) {
            emit error(tr("Geotagging failed. Couldn't open an image."));
            return;
        }
        QByteArray imageBuffer = fileRead.readAll();
        fileRead.close();
Andreas Bircher's avatar
Andreas Bircher committed
246

247 248 249
        if (!exifParser.write(imageBuffer, _geoRef[_triggerIndices[i]])) {
            emit error(tr("Geotagging failed. Couldn't write to image."));
            return;
Andreas Bircher's avatar
Andreas Bircher committed
250
        } else {
251 252 253 254 255 256 257 258 259
            QFile fileWrite;
            if(_saveDirectory == "") {
                fileWrite.setFileName(_imageDirectory + "/TAGGED/" + _imageList.at(_imageIndices[i]).fileName());
            } else {
                fileWrite.setFileName(_saveDirectory + "/" + _imageList.at(_imageIndices[i]).fileName());
            }
            if (!fileWrite.open(QFile::WriteOnly)) {
                emit error(tr("Geotagging failed. Couldn't write to an image."));
                return;
Andreas Bircher's avatar
Andreas Bircher committed
260
            }
261 262
            fileWrite.write(imageBuffer);
            fileWrite.close();
Andreas Bircher's avatar
Andreas Bircher committed
263
        }
264
        emit progressChanged(4*(100/nSteps) + ((100/nSteps) / maxIndex)*i);
Andreas Bircher's avatar
Andreas Bircher committed
265

Don Gagne's avatar
Don Gagne committed
266
        if (_cancel) {
267
            qCDebug(GeotaggingLog) << "Tagging cancelled";
Don Gagne's avatar
Don Gagne committed
268 269 270 271 272
            emit error(tr("Tagging cancelled"));
            return;
        }
    }

273 274 275 276 277 278
    if (_cancel) {
        qCDebug(GeotaggingLog) << "Tagging cancelled";
        emit error(tr("Tagging cancelled"));
        return;
    }

Don Gagne's avatar
Don Gagne committed
279 280
    emit progressChanged(100);
}
Andreas Bircher's avatar
Andreas Bircher committed
281 282 283 284

bool GeoTagWorker::parsePX4Log()
{
    // general message header
285
    char header[] = {(char)0xA3, (char)0x95, (char)0x00};
286 287 288
    // header for GPOS message header
    char gposHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x10, (char)0x00};
    int gposHeaderOffset;
Andreas Bircher's avatar
Andreas Bircher committed
289
    // header for GPOS message
290
    char gposHeader[] = {(char)0xA3, (char)0x95, (char)0x10, (char)0x00};
Andreas Bircher's avatar
Andreas Bircher committed
291 292
    int gposOffsets[3] = {3, 7, 11};
    int gposLengths[3] = {4, 4, 4};
293 294 295
    // header for trigger message header
    char triggerHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x37, (char)0x00};
    int triggerHeaderOffset;
Andreas Bircher's avatar
Andreas Bircher committed
296
    // header for trigger message
297
    char triggerHeader[] = {(char)0xA3, (char)0x95, (char)0x37, (char)0x00};
Andreas Bircher's avatar
Andreas Bircher committed
298 299
    int triggerOffsets[2] = {3, 11};
    int triggerLengths[2] = {8, 4};
300

Andreas Bircher's avatar
Andreas Bircher committed
301 302 303
    // load log
    QFile file(_logFile);
    if (!file.open(QIODevice::ReadOnly)) {
304
        qCDebug(GeotaggingLog) << "Could not open log file " << _logFile;
Andreas Bircher's avatar
Andreas Bircher committed
305 306 307 308 309
        return false;
    }
    QByteArray log = file.readAll();
    file.close();

310 311 312 313 314 315
    // extract header information: message lengths
    uint8_t* iptr = reinterpret_cast<uint8_t*>(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data());
    gposHeaderOffset = static_cast<int>(qFromLittleEndian(*iptr));
    iptr = reinterpret_cast<uint8_t*>(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data());
    triggerHeaderOffset = static_cast<int>(qFromLittleEndian(*iptr));

Andreas Bircher's avatar
Andreas Bircher committed
316 317 318 319 320
    // extract trigger data
    int index = 1;
    int sequence = -1;
    QGeoCoordinate lastCoordinate;
    while(index < log.count() - 1) {
321 322 323 324 325 326

        if (_cancel) {
            return false;
        }

        // first extract trigger
327
        index = log.indexOf(triggerHeader, index + 1);
Andreas Bircher's avatar
Andreas Bircher committed
328
        // check for whether last entry has been passed
329
        if (index < 0) {
Andreas Bircher's avatar
Andreas Bircher committed
330 331
            break;
        }
332 333 334 335 336 337 338

        if (log.indexOf(header, index + 1) != index + triggerHeaderOffset) {
            continue;
        }
        uint64_t* time = reinterpret_cast<uint64_t*>(log.mid(index + triggerOffsets[0], triggerLengths[0]).data());
        double timeDouble = static_cast<double>(qFromLittleEndian(*time)) / 1.0e6;
        uint32_t* seq = reinterpret_cast<uint32_t*>(log.mid(index + triggerOffsets[1], triggerLengths[1]).data());
339
        int seqInt = static_cast<int>(qFromLittleEndian(*seq));
340 341
        if (sequence >= seqInt || sequence + 20 < seqInt) { // assume that logging has not skipped more than 20 triggers. this prevents wrong header detection
            continue;
342
        }
343 344
        _triggerTime.append(timeDouble);
        sequence = seqInt;
345 346 347 348 349 350 351 352 353 354 355 356 357 358

        // second extract position
        bool lookForGpos = true;
        while (lookForGpos) {

            if (_cancel) {
                return false;
            }

            int gposIndex = log.indexOf(gposHeader, index + 1);
            if (gposIndex < 0) {
                _geoRef.append(lastCoordinate);
                break;
            }
Andreas Bircher's avatar
Andreas Bircher committed
359
            index = gposIndex;
360 361
            // verify that at an offset of gposHeaderOffset the next log message starts
            if (gposIndex + gposHeaderOffset == log.indexOf(header, gposIndex + 1)) {
362 363 364 365 366 367 368 369 370 371 372
                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));
                _geoRef.append(lastCoordinate);
                break;
Andreas Bircher's avatar
Andreas Bircher committed
373 374 375 376 377 378 379 380
            }
        }
    }
    return true;
}

bool GeoTagWorker::triggerFiltering()
{
381 382 383 384
    _imageIndices.clear();
    _triggerIndices.clear();
    for(int i = 0; i < _tagTime.count() && i < _triggerTime.count(); i++) {
        _imageIndices.append(i);
Andreas Bircher's avatar
Andreas Bircher committed
385 386
        _triggerIndices.append(i);
    }
387

Andreas Bircher's avatar
Andreas Bircher committed
388 389
    return true;
}