From 5643010067855cf6d770caf760f5d109d28568c3 Mon Sep 17 00:00:00 2001 From: Charles Ellison Date: Tue, 1 Nov 2022 09:52:30 -0500 Subject: [PATCH 1/3] add gps info to exported images --- CMakeLists.txt | 3 + guilib/include/rtabmap/gui/DatabaseViewer.h | 40 +++++++++ guilib/src/DatabaseViewer.cpp | 91 +++++++++++++++++++-- 3 files changed, 127 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b58dd88c33..61bc43bd0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -224,6 +224,9 @@ set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5) FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) +FIND_PACKAGE(exiv2 REQUIRED CONFIG NAMES exiv2) +link_libraries(exiv2lib) + IF(WITH_QT) FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) ELSE() diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 8360575925..ddb682eea5 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -45,6 +45,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + class Ui_DatabaseViewer; class QGraphicsScene; class QGraphicsView; @@ -197,6 +199,44 @@ private Q_SLOTS: void exportPoses(int format); void exportGPS(int format); + void writeExiv2Data(Exiv2::ExifData &exifData, std::string keyStr, std::string str); + + std::string toExifTimeStamp(std::string& t) + { + char result[200]; + const char* arg = t.c_str(); + int HH = 0; + int mm = 0; + int SS1 = 0; + if (strstr(arg, ":") || strstr(arg, "-")) { + int YY = 0, MM = 0, DD = 0; + char a = 0, b = 0, c = 0, d = 0, e = 0; + sscanf(arg, "%d%c%d%c%d%c%d%c%d%c%d", &YY, &a, &MM, &b, &DD, &c, &HH, &d, &mm, &e, &SS1); + } + snprintf(result, sizeof(result), "%d/1 %d/1 %d/1", HH, mm, SS1); + return result; + } + + std::string toExifString(double d) + { + char result[200]; + d *= 100; + snprintf(result, sizeof(result), "%d/100", abs(static_cast(d))); + return result; + } + + std::string toExifLatLonString(double l) + { + if(l < 0) l = -l; + int degrees = floor(l); + double minTmp = (l - degrees) * 60; + int min = floor(minTmp); + int secM = floor((minTmp - min) * 60 * 1000); + std::stringstream ss; + ss << degrees << "/1 " << min << "/1 " << secM << "/1000"; + return ss.str(); + } + private: Ui_DatabaseViewer * ui_; CloudViewer * constraintsViewer_; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 07597ddcac..b8b93cddee 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -25,6 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include + #include "rtabmap/gui/DatabaseViewer.h" #include "rtabmap/gui/CloudViewer.h" #include "ui_DatabaseViewer.h" @@ -1482,6 +1486,7 @@ void DatabaseViewer::extractImages() SensorData data; dbDriver_->getNodeData(ids_.at(i), data); data.uncompressData(); + Exiv2::ExifData exifData; if(!directoriesCreated) { @@ -1507,7 +1512,7 @@ void DatabaseViewer::extractImages() } } - if(!data.imageRaw().empty() && useStamp) + if(!data.imageRaw().empty()) { Transform p,gt; int m,w; @@ -1517,26 +1522,68 @@ void DatabaseViewer::extractImages() GPS gps; EnvSensors s; dbDriver_->getNodeInfo(ids_.at(i), p, m, w, l, stamp, gt, v, gps, s); - if(stamp == 0.0) + if(useStamp && stamp == 0.0) { UWARN("Node %d has null timestamp! Using id instead!", ids_.at(i)); } - else + else if(useStamp) { id = QString::number(stamp, 'f'); } + + //fill out image metadata + std::time_t time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::time_point(std::chrono::duration_cast(std::chrono::duration(gps.stamp())))); + std::tm timestamp = *std::localtime(&time); + char datebuf[20] = { 0 }; + std::strftime(datebuf, sizeof(datebuf), "%Y:%m:%d:%H:%M:%S", ×tamp); + std::string dateString(datebuf); + + double latitude = gps.latitude(); + double longitude = gps.longitude(); + double altitude = gps.altitude(); + exifData["Exif.Image.ProcessingSoftware"] = "RTABMAP"; + + writeExiv2Data(exifData, "EExif.GPSInfo.GPSMapDatum", "WGS-84"); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSDateStamp", dateString); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSTimeStamp", toExifTimeStamp(dateString)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitude", toExifLatLonString(latitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitude", toExifLatLonString(longitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitude", toExifString(altitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitudeRef", (latitude<0 ? "S" : "N")); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitudeRef", (longitude<0 ? "W" : "E")); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitudeRef", (altitude < 0.0 ? "1" : "0")); + + writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirectionRef", "T"); + + float x,y,z,roll,pitch,yaw; + p.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); + float bearing = (yaw/3.1416*180.0) + 180.0; + if (bearing > 90) bearing -= 90.0; + else bearing += 270.0; + writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirection", toExifString(bearing)); + } if(!data.imageRaw().empty()) { if(!data.rightRaw().empty()) { - if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw())) + std::string filename_left = QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(); + if(!cv::imwrite(filename_left, data.imageRaw())) UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str()); - if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw())) + std::string filename_right = QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(); + if(!cv::imwrite(filename_right, data.rightRaw())) UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str()); UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str()); + Exiv2::Image::AutoPtr exiv_left = Exiv2::ImageFactory::open(filename_left); + exiv_left->setExifData(exifData); + exiv_left->writeMetadata(); + + Exiv2::Image::AutoPtr exiv_right = Exiv2::ImageFactory::open(filename_right); + exiv_right->setExifData(exifData); + exiv_right->writeMetadata(); + if(databaseFileName_.empty()) { UERROR("Cannot save calibration file, database name is empty!"); @@ -1582,18 +1629,28 @@ void DatabaseViewer::extractImages() { if(!data.depthRaw().empty()) { - if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw())) + std::string filename = QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(); + if(!cv::imwrite(filename, data.imageRaw())) UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str()); if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw())) UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str()); UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str()); + + Exiv2::Image::AutoPtr exiv_image = Exiv2::ImageFactory::open(filename); + exiv_image->setExifData(exifData); + exiv_image->writeMetadata(); } else { - if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw())) + std::string filename = QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(); + if(!cv::imwrite(filename, data.imageRaw())) UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str()); else UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str()); + + Exiv2::Image::AutoPtr exiv_image = Exiv2::ImageFactory::open(filename); + exiv_image->setExifData(exifData); + exiv_image->writeMetadata(); } if(databaseFileName_.empty()) @@ -8795,4 +8852,24 @@ void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChang this->configModified(); } +void DatabaseViewer::writeExiv2Data(Exiv2::ExifData &exifData, std::string keyStr, std::string str) +{ + try{ + Exiv2::ExifKey key(keyStr); + Exiv2::ExifData::iterator pos = exifData.findKey(key); + if(pos != exifData.end()){ + exifData[keyStr].setValue(str); + } + else + { + exifData[keyStr].setValue(str); + } + } + catch (Exiv2::Error& e) { + ;//std::cerr << "Caught Exiv2 exception '" << e.what() << "'\n"; + } + + +} + } // namespace rtabmap From f6d468461e8bce308c62e2f35620c14cfcd12c20 Mon Sep 17 00:00:00 2001 From: Charles Ellison Date: Tue, 1 Nov 2022 16:58:32 -0500 Subject: [PATCH 2/3] export image with posed based gps. Previously it used raw gps info. --- guilib/include/rtabmap/gui/DatabaseViewer.h | 2 + guilib/src/DatabaseViewer.cpp | 198 ++++++++++++-------- 2 files changed, 117 insertions(+), 83 deletions(-) diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index ddb682eea5..ce07d946e3 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -106,6 +106,7 @@ private Q_SLOTS: void updateOptimizedMesh(); void exportDatabase(); void extractImages(); + std::string selectGraph(); void exportPosesRaw(); void exportPosesRGBDSLAMMotionCapture(); void exportPosesRGBDSLAM(); @@ -199,6 +200,7 @@ private Q_SLOTS: void exportPoses(int format); void exportGPS(int format); + std::map graphToGPS(std::string graphSource); void writeExiv2Data(Exiv2::ExifData &exifData, std::string keyStr, std::string str); std::string toExifTimeStamp(std::string& t) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index b8b93cddee..2f49fa1435 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1478,6 +1478,10 @@ void DatabaseViewer::extractImages() progressDialog->appendText(tr("Saving %1 images to %2...").arg(ids_.size()).arg(path)); progressDialog->show(); + // Determine source of gps info for images + std::string graphSource = DatabaseViewer::selectGraph(); + std::map gpsValues = graphToGPS(graphSource); + int imagesExported = 0; for(int i=0; i(std::chrono::duration(gps.stamp())))); std::tm timestamp = *std::localtime(&time); @@ -2431,125 +2437,151 @@ void DatabaseViewer::exportPosesKML() exportPoses(5); } -void DatabaseViewer::exportPoses(int format) +std::string DatabaseViewer::selectGraph() { - QStringList types; + QStringList types; types.push_back("Map's graph (see Graph View)"); types.push_back("Odometry"); if(!groundTruthPoses_.empty()) { types.push_back("Ground Truth"); } + if(!gpsPoses_.empty()) + { + types.push_back("GPS"); + } bool ok; QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok); if(!ok) { - return; + return "Cancel"; } - bool odometry = type.compare("Odometry") == 0; - bool groundTruth = type.compare("Ground Truth") == 0; - - if(groundTruth && groundTruthPoses_.empty()) + + if(type == "Ground Truth" && groundTruthPoses_.empty()) { QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!")); - return; + return "None"; } - else if(!odometry && graphes_.empty()) + else if(type == "Odometry" && odomPoses_.empty()) + { + QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!")); + return "None"; + } + else if(type == "GPS" && gpsPoses_.empty()) + { + QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!")); + return "None"; + } + else if(graphes_.empty()) { this->updateGraphView(); if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1) { QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!")); - return; + return "None"; } } - else if(odometry && odomPoses_.empty()) + + return type.toStdString(); +} + +std::map DatabaseViewer::graphToGPS(std::string graphSource) +{ + std::map graph; + if(graphSource == "Ground Truth") { - QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!")); - return; + graph = groundTruthPoses_; + } + else if(graphSource == "Odometry") + { + graph = odomPoses_; + } + else if(graphSource == "GPS") + { + graph = gpsPoses_; + } + else + { + graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); } - if(format == 5) + //align with ground truth for more meaningful results + pcl::PointCloud cloud1, cloud2; + cloud1.resize(graph.size()); + cloud2.resize(graph.size()); + int oi = 0; + int idFirst = 0; + for(std::map::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter) { - if(gpsValues_.empty() || gpsPoses_.empty()) - { - QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!")); - } - else + std::map::iterator iter2 = graph.find(iter->first); + if(iter2!=graph.end()) { - std::map graph; - if(groundTruth) + if(oi==0) { - graph = groundTruthPoses_; - } - else if(odometry) - { - graph = odomPoses_; - } - else - { - graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + idFirst = iter->first; } + cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); + cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z()); + } + } + Transform t = Transform::getIdentity(); + if(oi>5) + { + cloud1.resize(oi); + cloud2.resize(oi); - //align with ground truth for more meaningful results - pcl::PointCloud cloud1, cloud2; - cloud1.resize(graph.size()); - cloud2.resize(graph.size()); - int oi = 0; - int idFirst = 0; - for(std::map::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter) - { - std::map::iterator iter2 = graph.find(iter->first); - if(iter2!=graph.end()) - { - if(oi==0) - { - idFirst = iter->first; - } - cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); - cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z()); - } - } + t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1); + } + else if(idFirst) + { + t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse(); + } - Transform t = Transform::getIdentity(); - if(oi>5) - { - cloud1.resize(oi); - cloud2.resize(oi); + std::map values; + GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords(); + for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) + { + iter->second = t * iter->second; - t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1); - } - else if(idFirst) - { - t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse(); - } + GeodeticCoords coord; + coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin); + double bearing = -(iter->second.theta()*180.0/M_PI-90.0); + if(bearing < 0) + { + bearing += 360; + } - std::map values; - GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords(); - for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) - { - iter->second = t * iter->second; + Transform p, g; + int w; + std::string l; + double stamp=0.0; + int mapId; + std::vector v; + GPS gps; + EnvSensors sensors; + dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors); + values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0))); + } - GeodeticCoords coord; - coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin); - double bearing = -(iter->second.theta()*180.0/M_PI-90.0); - if(bearing < 0) - { - bearing += 360; - } + return values; +} - Transform p, g; - int w; - std::string l; - double stamp=0.0; - int mapId; - std::vector v; - GPS gps; - EnvSensors sensors; - dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors); - values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0))); - } +void DatabaseViewer::exportPoses(int format) +{ + std::string graphSource = DatabaseViewer::selectGraph(); + bool groundTruth = (graphSource == "Ground Truth"); + bool odometry = (graphSource == "Odometry"); + + if(format == 5) + { + if(gpsValues_.empty() || gpsPoses_.empty()) + { + QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!")); + } + else + { + std::map values = graphToGPS(graphSource); QString output = pathDatabase_ + QDir::separator() + "poses.kml"; QString path = QFileDialog::getSaveFileName( From 2060e578760e9ee44d53a98f8f6111885a248736 Mon Sep 17 00:00:00 2001 From: Charles Ellison Date: Tue, 1 Nov 2022 17:14:01 -0500 Subject: [PATCH 3/3] act better without gps data --- guilib/src/DatabaseViewer.cpp | 73 +++++++++++++++++++---------------- 1 file changed, 40 insertions(+), 33 deletions(-) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 2f49fa1435..51057a0ffa 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1479,8 +1479,13 @@ void DatabaseViewer::extractImages() progressDialog->show(); // Determine source of gps info for images - std::string graphSource = DatabaseViewer::selectGraph(); - std::map gpsValues = graphToGPS(graphSource); + std::string gpsSource = "None"; + std::map gpsValues; + if(!gpsPoses_.empty()) + { + gpsSource = DatabaseViewer::selectGraph(); + gpsValues = graphToGPS(gpsSource); + } int imagesExported = 0; for(int i=0; i(std::chrono::duration(gps.stamp())))); - std::tm timestamp = *std::localtime(&time); - char datebuf[20] = { 0 }; - std::strftime(datebuf, sizeof(datebuf), "%Y:%m:%d:%H:%M:%S", ×tamp); - std::string dateString(datebuf); - - double latitude = gps.latitude(); - double longitude = gps.longitude(); - double altitude = gps.altitude(); exifData["Exif.Image.ProcessingSoftware"] = "RTABMAP"; - - writeExiv2Data(exifData, "EExif.GPSInfo.GPSMapDatum", "WGS-84"); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSDateStamp", dateString); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSTimeStamp", toExifTimeStamp(dateString)); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitude", toExifLatLonString(latitude)); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitude", toExifLatLonString(longitude)); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitude", toExifString(altitude)); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitudeRef", (latitude<0 ? "S" : "N")); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitudeRef", (longitude<0 ? "W" : "E")); - writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitudeRef", (altitude < 0.0 ? "1" : "0")); - - writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirectionRef", "T"); - - float x,y,z,roll,pitch,yaw; - p.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); - float bearing = (yaw/3.1416*180.0) + 180.0; - if (bearing > 90) bearing -= 90.0; - else bearing += 270.0; - writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirection", toExifString(bearing)); - + if(!(gpsSource == "None")) + { + gps = gpsValues[ids_.at(i)]; + + std::time_t time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::time_point(std::chrono::duration_cast(std::chrono::duration(gps.stamp())))); + std::tm timestamp = *std::localtime(&time); + char datebuf[20] = { 0 }; + std::strftime(datebuf, sizeof(datebuf), "%Y:%m:%d:%H:%M:%S", ×tamp); + std::string dateString(datebuf); + + double latitude = gps.latitude(); + double longitude = gps.longitude(); + double altitude = gps.altitude(); + + writeExiv2Data(exifData, "EExif.GPSInfo.GPSMapDatum", "WGS-84"); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSDateStamp", dateString); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSTimeStamp", toExifTimeStamp(dateString)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitude", toExifLatLonString(latitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitude", toExifLatLonString(longitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitude", toExifString(altitude)); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLatitudeRef", (latitude<0 ? "S" : "N")); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSLongitudeRef", (longitude<0 ? "W" : "E")); + writeExiv2Data(exifData, "Exif.GPSInfo.GPSAltitudeRef", (altitude < 0.0 ? "1" : "0")); + + writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirectionRef", "T"); + + float x,y,z,roll,pitch,yaw; + p.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); + float bearing = (yaw/3.1416*180.0) + 180.0; + if (bearing > 90) bearing -= 90.0; + else bearing += 270.0; + writeExiv2Data(exifData, "Exif.GPSInfo.GPSImgDirection", toExifString(bearing)); + } } if(!data.imageRaw().empty())