GPS alignment from exif metadata

src/aliceVision/numeric/CMakeLists.txt

@@ -2,12 +2,14 @@
 set(numeric_files_headers
   numeric.hpp
   projection.hpp
+  gps.hpp
 )
 
 # Sources
 set(numeric_files_sources
   numeric.cpp
   projection.cpp
+  gps.cpp
 )
 
 alicevision_add_library(aliceVision_numeric
@@ -26,3 +28,4 @@ alicevision_add_test(numeric_test.cpp    NAME "numeric"            LINKS aliceVi
 alicevision_add_test(polynomial_test.cpp NAME "numeric_polynomial" LINKS aliceVision_numeric)
 alicevision_add_test(lmFunctor_test.cpp  NAME "numeric_lmFunctor"  LINKS aliceVision_numeric)
 alicevision_add_test(projection_test.cpp NAME "numeric_projection" LINKS aliceVision_numeric)
+alicevision_add_test(gps_test.cpp NAME "numeric_gps" LINKS aliceVision_numeric)

src/aliceVision/numeric/gps.cpp

@@ -0,0 +1,81 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2021 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#include "gps.hpp"
+#include <cmath>
+#include <boost/algorithm/string.hpp>
+#include <aliceVision/system/Logger.hpp>
+
+namespace aliceVision
+{
+
+Vec3 WGS84ToCartesian(const Vec3& gps)
+{
+
+    // equatorial radius WGS84 major axis
+    const static double equRadius = 6378137.0;
+    const static double flattening = 1.0 / 298.257222101;
+
+    const static double sqrEccentricity = flattening * (2.0 - flattening);
+
+    const double lat = degreeToRadian(gps(0));
+    const double lon = degreeToRadian(gps(1));
+    const double alt = gps(2);
+
+    const double sinLat = std::sin(lat);
+    const double cosLat = std::cos(lat);
+    const double sinLon = std::sin(lon);
+    const double cosLon = std::cos(lon);
+
+    // Normalized radius
+    const double normRadius = equRadius / std::sqrt(1 - sqrEccentricity * sinLat * sinLat);
+
+    return {(normRadius + alt) * cosLat * cosLon,
+            (normRadius + alt) * cosLat * sinLon,
+            (normRadius * (1 - sqrEccentricity) + alt) * sinLat};
+}
+
+double parseAltitudeFromString(const std::string& alt, const std::string& altRef)
+{
+    static const std::array<std::string, 2> allowedRef = {"0", "1"};
+    if(!std::any_of(allowedRef.cbegin(), allowedRef.cend(), [altRef](const std::string& s){return altRef == s;}))
+    {
+        const auto message = "Unexpected value for gps reference: " + altRef;
+        ALICEVISION_LOG_ERROR(message);
+        throw std::invalid_argument(message);
+    }
+    const double ref = std::stod(altRef);
+    const double altitude = std::stod(alt);
+
+    return ref > 0 ? -altitude : altitude;
+}
+
+//WGS84
+double parseGPSFromString(const std::string& gpsDegrees, const std::string& gpsRef)
+{
+    static const std::array<std::string, 4> allowedRef = {"W", "E", "N", "S"};
+    if(!std::any_of(allowedRef.cbegin(), allowedRef.cend(), [gpsRef](const std::string& s){return gpsRef == s;}))
+    {
+        const auto message = "Unexpected value for gps reference: " + gpsRef;
+        ALICEVISION_LOG_ERROR(message);
+        throw std::invalid_argument(message);
+    }
+    std::vector<std::string> result;
+    // parse the comma separated values
+    boost::split(result, gpsDegrees, boost::is_any_of(","));
+
+    double gpsCoord{0};
+
+    for(std::size_t i = 0; i < result.size(); ++i)
+    {
+        const auto d = std::stod(result[i]);
+        gpsCoord += d * std::pow(60., -static_cast<int>(i));
+    }
+
+    return (gpsRef == "S" || gpsRef == "W") ? -gpsCoord : gpsCoord;
+}
+
+} // namespace aliceVision

src/aliceVision/numeric/gps.hpp

@@ -0,0 +1,42 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2021 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include "numeric.hpp"
+
+namespace aliceVision {
+
+/**
+ * @brief Convert the position expressed in WGS84 reference frame (latitude, longitude, altitude) to the cartesian
+ * coordinates x, y, z.
+ * @param[in] gps A three dimensional vector containing the WGS84 coordinates latitude, longitude, altitude
+ * @return A three dimensional vector containing the x, y and z coordinates.
+ */
+Vec3 WGS84ToCartesian(const Vec3& gps);
+
+/**
+ * @brief Convert the string representation of the altitude in gps representation to a numeric value.
+ * @param[in] alt String containing the altitude.
+ * @param[in] altRef String containing the reference for the altitude, if 1 the altitude will be interpreted as below
+ * the sea level.
+ * (https://www.awaresystems.be/imaging/tiff/tifftags/privateifd/gps/gpsaltituderef.html)
+ * @return the altitude in meters over the sea level.
+ * @throws std::invalid_argument() if the data is not valid.
+ */
+double parseAltitudeFromString(const std::string& alt, const std::string& altRef);
+
+/**
+ * @brief Convert the string representation of the gps position (latitude or longitude) to a numeric value.
+ * @param[in] gpsDegrees The string representation of the latitude or longitude in degrees.
+ * @param[in] gpsRef A string giving the reference for the latitude or longitude (i.e. "N" or "S" for latitude,
+ * "E" or "W" for longitude). https://www.awaresystems.be/imaging/tiff/tifftags/privateifd/gps/gpslatituderef.html
+ * @return The decimal degree representation of the position.
+ * @throws std::invalid_argument() if the data is not valid.
+ */
+double parseGPSFromString(const std::string& gpsDegrees, const std::string& gpsRef);
+
+}

src/aliceVision/numeric/gps_test.cpp

@@ -0,0 +1,51 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2021 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#include <iostream>
+#include <aliceVision/system/Logger.hpp>
+#include "aliceVision/numeric/numeric.hpp"
+#include <aliceVision/numeric/gps.hpp>
+
+#define BOOST_TEST_MODULE numeric
+
+#include <boost/test/unit_test.hpp>
+#include <boost/test/tools/floating_point_comparison.hpp>
+#include <aliceVision/unitTest.hpp>
+
+using namespace aliceVision;
+using namespace std;
+
+
+BOOST_AUTO_TEST_CASE ( conversionTest )
+{
+    const Vec3 gps{43.597824, 1.4548992, 150};
+    const Vec3 expected{4625021.304, 117467.403, 4375942.605};
+    const auto res = WGS84ToCartesian(gps);
+
+    EXPECT_MATRIX_CLOSE_FRACTION(res, expected, 1e-5);
+}
+
+BOOST_AUTO_TEST_CASE ( parseGPSTest )
+{
+    const std::string gps{"1, 27, 19.0008"};
+    const double expected{1.45528};
+    const auto res = parseGPSFromString(gps, "E");
+
+    BOOST_CHECK_CLOSE_FRACTION(res, expected, 1e-5);
+
+    BOOST_CHECK_THROW(parseGPSFromString(gps, "G"), std::invalid_argument);
+}
+
+BOOST_AUTO_TEST_CASE ( parseAltTest )
+{
+    const std::string gps{"10785.65"};
+    const double expected{10785.65};
+    const auto res = parseAltitudeFromString(gps, "0");
+    BOOST_CHECK_CLOSE_FRACTION(res, expected, 1e-5);
+    BOOST_CHECK(parseAltitudeFromString(gps, "1") < 0);
+
+    BOOST_CHECK_THROW(parseAltitudeFromString(gps, "N"), std::invalid_argument);
+}

src/aliceVision/sfm/utils/alignment.cpp

@@ -787,5 +787,52 @@ bool computeNewCoordinateSystemFromSpecificMarkers(const sfmData::SfMData& sfmDa
     return geometry::decomposeRTS(RTS, out_S, out_t, out_R);
 }
 
+
+
+bool computeNewCoordinateSystemFromGpsData(const sfmData::SfMData& sfmData, std::mt19937 &randomNumberGenerator, double& out_S, Mat3& out_R, Vec3& out_t)
+{
+    std::vector<Vec3> gpsPositions{};
+    std::vector<Vec3> centers{};
+    gpsPositions.reserve(sfmData.getPoses().size());
+    centers.reserve(sfmData.getPoses().size());
+
+    // for each reconstructed view
+    for(const auto& v : sfmData.getViews())
+    {
+        const auto viewID = v.first;
+        const auto& view = v.second;
+        // skip no pose
+        if(!(sfmData.isPoseAndIntrinsicDefined(viewID) && view->hasGpsMetadata()))
+        {
+            ALICEVISION_LOG_TRACE("Skipping view " << viewID << " because pose " << sfmData.isPoseAndIntrinsicDefined(viewID) << " and gps " << view->hasGpsMetadata());
+            continue;
+        }
+        // extract the gps position
+        gpsPositions.push_back(view->getGpsPositionFromMetadata());
+        // get the center
+        centers.push_back(sfmData.getPose(*view.get()).getTransform().center());
+    }
+
+    // if enough data try to find the transformation
+    if(gpsPositions.size() < 4)
+    {
+        ALICEVISION_LOG_INFO("Not enough points to estimate the rototranslation to align the gps");
+        return false;
+    }
+
+    Mat x1(3, centers.size());
+    Mat x2(3, gpsPositions.size());
+
+    for(Mat::Index i = 0; i < gpsPositions.size(); ++i)
+    {
+        x1.col(i) = centers[i];
+        x2.col(i) = gpsPositions[i];
+    }
+
+    std::vector<std::size_t> inliers;
+    const bool refine{true};
+    return aliceVision::geometry::ACRansac_FindRTS(x1, x2, randomNumberGenerator, out_S, out_t, out_R, inliers, refine);
+}
+
 } // namespace sfm
 } // namespace aliceVision

src/aliceVision/sfm/utils/alignment.hpp

@@ -163,7 +163,7 @@ inline void applyTransform(sfmData::SfMData& sfmData,
       sfmData.setPose(view, sfmData::CameraPose(pose));
     }
   }
-  
+
   for(auto& landmark: sfmData.structure)
   {
     landmark.second.X = S * R * landmark.second.X + t;
@@ -215,6 +215,21 @@ void computeNewCoordinateSystemFromLandmarks(const sfmData::SfMData& sfmData,
                                              Mat3& out_R,
                                              Vec3& out_t);
 
+/**
+ * @brief Compute a new coordinate system using the GPS data available in the metadata. The transformation will bring the
+ * model in the cartesian metric reference system.
+ * @param[in] sfmData The sfmdata containing the scene.
+ * @param[in,out] randomNumberGenerator The random number generator.
+ * @param[out] out_S the scale factor.
+ * @param[out] out_R the rotation.
+ * @param[out] out_t the translation.
+ * @return false if no reliable transformation can be computed or the sfmdata does not contain gps metadata, true otherwise.
+ */
+bool computeNewCoordinateSystemFromGpsData(const sfmData::SfMData& sfmData, std::mt19937 &randomNumberGenerator,
+                                             double& out_S,
+                                             Mat3& out_R,
+                                             Vec3& out_t);
+
 /**
  * @brief Retrieve the View Id from a string expression (integer with the view id or filename of the input image).
  * @param[in] sfmData: sfm scene

src/aliceVision/sfmData/SfMData.hpp

@@ -301,7 +301,7 @@ class SfMData
    * @warning: This function returns a CameraPose (a temporary object and not a reference),
    *           because in the RIG context, this pose is the composition of the rig pose and the sub-pose.
    */
-  const CameraPose getPose(const View& view) const
+  CameraPose getPose(const View& view) const
   {
     // check the view has valid pose / rig etc
     if(!view.isPartOfRig() || view.isPoseIndependant())

src/aliceVision/sfmData/View.cpp

@@ -15,7 +15,7 @@
 
 
 #include <iostream>
-
+#include <aliceVision/numeric/gps.hpp>
 
 namespace aliceVision {
 namespace sfmData {
@@ -106,13 +106,10 @@ std::map<std::string, std::string>::const_iterator View::findMetadataIterator(co
 
 bool View::hasMetadata(const std::vector<std::string>& names) const
 {
-    for(const std::string& name : names)
-    {
-        const auto it = findMetadataIterator(name);
-        if(it != _metadata.end())
-            return true;
-    }
-    return false;
+    return std::any_of(names.cbegin(), names.cend(),
+                       [this](const std::string& name){
+                           return (findMetadataIterator(name) != _metadata.end());
+                       });
 }
 
 bool View::hasDigitMetadata(const std::vector<std::string>& names, bool isPositive) const
@@ -204,5 +201,72 @@ int View::getIntMetadata(const std::vector<std::string>& names) const
     }
 }
 
+bool View::hasGpsMetadata() const
+{
+    const auto tags = GPSExifTags::all();
+    return std::all_of(tags.cbegin(), tags.cend(), [this](const std::string& t){ return hasMetadata({t}); });
+}
+
+Vec3 View::getGpsPositionFromMetadata() const
+{
+    const auto gps = getGpsPositionWGS84FromMetadata();
+    return WGS84ToCartesian(gps);
+}
+
+void View::getGpsPositionWGS84FromMetadata(double& lat, double& lon, double& alt) const
+{
+    const auto& meta = getMetadata();
+    const auto gpsLat = meta.at(GPSExifTags::latitude());
+    const auto gpsLatRef = meta.at(GPSExifTags::latitudeRef());
+    lat = parseGPSFromString(gpsLat, gpsLatRef);
+
+    const auto gpsLon = meta.at(GPSExifTags::longitude());
+    const auto gpsLonRef = meta.at(GPSExifTags::longitudeRef());
+    lon = parseGPSFromString(gpsLon, gpsLonRef);
+
+    const auto gpsAlt = meta.at(GPSExifTags::altitude());
+    const auto gpsAltRef = meta.at(GPSExifTags::altitudeRef());
+    alt = parseAltitudeFromString(gpsAlt, gpsAltRef);
+}
+
+Vec3 View::getGpsPositionWGS84FromMetadata() const
+{
+    double lat{0};
+    double lon{0};
+    double alt{0};
+    getGpsPositionWGS84FromMetadata(lat, lon, alt);
+
+    return {lat, lon, alt};
+}
+
+std::string GPSExifTags::latitude()
+{
+    return "GPS:Latitude";
+}
+std::string GPSExifTags::latitudeRef()
+{
+    return "GPS:LatitudeRef";
+}
+std::string GPSExifTags::longitude()
+{
+    return "GPS:Longitude";
+}
+std::string GPSExifTags::longitudeRef()
+{
+    return "GPS:LongitudeRef";
+}
+std::vector<std::string> GPSExifTags::all()
+{
+    return {GPSExifTags::latitude(), GPSExifTags::latitudeRef(), GPSExifTags::longitude(), GPSExifTags::longitudeRef(), GPSExifTags::altitude(), GPSExifTags::altitudeRef()};
+}
+std::string GPSExifTags::altitude()
+{
+    return "GPS:Altitude";
+}
+std::string GPSExifTags::altitudeRef()
+{
+    return "GPS:AltitudeRef";
+}
+
 } // namespace sfmData
 } // namespace aliceVision