diff --git a/src/aliceVision/numeric/CMakeLists.txt b/src/aliceVision/numeric/CMakeLists.txt index 97bc1278b2..e687699f0b 100644 --- a/src/aliceVision/numeric/CMakeLists.txt +++ b/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) diff --git a/src/aliceVision/numeric/gps.cpp b/src/aliceVision/numeric/gps.cpp new file mode 100644 index 0000000000..547c3b1fae --- /dev/null +++ b/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 +#include +#include + +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.0 - sqrEccentricity * sinLat * sinLat); + + return {(normRadius + alt) * cosLat * cosLon, + (normRadius + alt) * cosLat * sinLon, + (normRadius * (1.0 - sqrEccentricity) + alt) * sinLat}; +} + +double parseAltitudeFromString(const std::string& alt, const std::string& altRef) +{ + static const std::array 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.0 ? -altitude : altitude; +} + +//WGS84 +double parseGPSFromString(const std::string& gpsDegrees, const std::string& gpsRef) +{ + static const std::array 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 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(i)); + } + + return (gpsRef == "S" || gpsRef == "W") ? -gpsCoord : gpsCoord; +} + +} // namespace aliceVision diff --git a/src/aliceVision/numeric/gps.hpp b/src/aliceVision/numeric/gps.hpp new file mode 100644 index 0000000000..39276c0d35 --- /dev/null +++ b/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); + +} \ No newline at end of file diff --git a/src/aliceVision/numeric/gps_test.cpp b/src/aliceVision/numeric/gps_test.cpp new file mode 100644 index 0000000000..f4eb3ea2cb --- /dev/null +++ b/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 +#include +#include "aliceVision/numeric/numeric.hpp" +#include + +#define BOOST_TEST_MODULE numeric + +#include +#include +#include + +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); +} \ No newline at end of file diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index d5cf7318e8..0d89cdf46f 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/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 gpsPositions{}; + std::vector 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 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 diff --git a/src/aliceVision/sfm/utils/alignment.hpp b/src/aliceVision/sfm/utils/alignment.hpp index eb8598f150..28f5768dff 100644 --- a/src/aliceVision/sfm/utils/alignment.hpp +++ b/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 diff --git a/src/aliceVision/sfmData/SfMData.hpp b/src/aliceVision/sfmData/SfMData.hpp index 508d9f4fce..420047396c 100644 --- a/src/aliceVision/sfmData/SfMData.hpp +++ b/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()) diff --git a/src/aliceVision/sfmData/View.cpp b/src/aliceVision/sfmData/View.cpp index 20aa62b9fa..5a194db65c 100644 --- a/src/aliceVision/sfmData/View.cpp +++ b/src/aliceVision/sfmData/View.cpp @@ -15,7 +15,7 @@ #include - +#include namespace aliceVision { namespace sfmData { @@ -106,13 +106,10 @@ std::map::const_iterator View::findMetadataIterator(co bool View::hasMetadata(const std::vector& 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& names, bool isPositive) const @@ -204,5 +201,72 @@ int View::getIntMetadata(const std::vector& 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 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 diff --git a/src/aliceVision/sfmData/View.hpp b/src/aliceVision/sfmData/View.hpp index 406755f686..093534e1a4 100644 --- a/src/aliceVision/sfmData/View.hpp +++ b/src/aliceVision/sfmData/View.hpp @@ -12,6 +12,7 @@ #include #include #include +#include namespace aliceVision { namespace sfmData { @@ -32,6 +33,18 @@ enum class EEXIFOrientation , UNKNOWN = -1 }; +struct GPSExifTags +{ + static std::string latitude(); + static std::string latitudeRef(); + static std::string longitude(); + static std::string longitudeRef(); + static std::string altitude(); + static std::string altitudeRef(); + static std::vector all(); +}; + + /** * @brief A view define an image by a string and unique indexes for * the view, the camera intrinsic, the pose and the subpose if the camera is part of a rig @@ -200,7 +213,7 @@ class View */ bool isPoseIndependant() const { - return (!isPartOfRig() || _isIndependantPose); + return (!isPartOfRig() || _isPoseIndependent); } /** @@ -229,6 +242,13 @@ class View */ bool hasMetadata(const std::vector& names) const; + /** + * @brief Return true if the metadata for longitude and latitude exist. + * It checks that all the tags from GPSExifTags exists + * @return true if GPS data is available + */ + bool hasGpsMetadata() const; + /** * @brief Return true if the given metadata name exists and is a digit * @param[in] names List of possible names for the metadata @@ -359,6 +379,26 @@ class View return static_cast(orientation); } + /** + * @brief Get the gps position in the absolute cartesian reference system. + * @return The position x, y, z as a three dimensional vector. + */ + Vec3 getGpsPositionFromMetadata() const; + + /** + * @brief Get the gps position in the WGS84 reference system. + * @param[out] lat the latitude + * @param[out] lon the longitude + * @param[out] alt the altitude + */ + void getGpsPositionWGS84FromMetadata(double& lat, double& lon, double& alt) const; + + /** + * @brief Get the gps position in the WGS84 reference system as a vector. + * @return A three dimensional vector with latitude, logitude and altitude. + */ + Vec3 getGpsPositionWGS84FromMetadata() const; + const bool getApplyWhiteBalance() const { if (getIntMetadata({"AliceVision:useWhiteBalance"}) == 0) @@ -468,9 +508,9 @@ class View * @brief setIndependantPose * @param independant */ - void setIndependantPose(bool independant) + void setIndependantPose(bool independent) { - _isIndependantPose = independant; + _isPoseIndependent = independent; } /** @@ -544,8 +584,8 @@ class View IndexT _frameId = UndefinedIndexT; /// resection id IndexT _resectionId = UndefinedIndexT; - /// pose independant of other view(s) - bool _isIndependantPose = true; + /// pose independent of other view(s) + bool _isPoseIndependent = true; /// map for metadata std::map _metadata; }; diff --git a/src/aliceVision/sfmData/view_test.cpp b/src/aliceVision/sfmData/view_test.cpp index 414d57e615..5831cdd9cd 100644 --- a/src/aliceVision/sfmData/view_test.cpp +++ b/src/aliceVision/sfmData/view_test.cpp @@ -50,5 +50,18 @@ BOOST_AUTO_TEST_CASE(View_Metadata) BOOST_CHECK_EQUAL(view.getMetadataFocalLength(), 5.0); } + + { + sfmData::View view; + + BOOST_CHECK_THROW(view.getGpsPositionFromMetadata(), std::out_of_range); + + for(const auto& t : sfmData::GPSExifTags::all()) + { + BOOST_CHECK(!view.hasGpsMetadata()); + view.addMetadata(t, "100.6"); + } + BOOST_CHECK(view.hasGpsMetadata()); + } } diff --git a/src/software/utils/main_sfmTransform.cpp b/src/software/utils/main_sfmTransform.cpp index b6ee9865a2..89516dd74b 100644 --- a/src/software/utils/main_sfmTransform.cpp +++ b/src/software/utils/main_sfmTransform.cpp @@ -38,8 +38,9 @@ enum class EAlignmentMethod: unsigned char , AUTO_FROM_CAMERAS, AUTO_FROM_LANDMARKS, FROM_SINGLE_CAMERA, - FROM_CENTER_CAMERA - , FROM_MARKERS + FROM_CENTER_CAMERA, + FROM_MARKERS, + FROM_GPS }; /** @@ -58,6 +59,7 @@ std::string EAlignmentMethod_enumToString(EAlignmentMethod alignmentMethod) case EAlignmentMethod::FROM_SINGLE_CAMERA: return "from_single_camera"; case EAlignmentMethod::FROM_CENTER_CAMERA: return "from_center_camera"; case EAlignmentMethod::FROM_MARKERS: return "from_markers"; + case EAlignmentMethod::FROM_GPS: return "from_gps"; } throw std::out_of_range("Invalid EAlignmentMethod enum"); } @@ -79,6 +81,7 @@ EAlignmentMethod EAlignmentMethod_stringToEnum(const std::string& alignmentMetho if(method == "from_single_camera") return EAlignmentMethod::FROM_SINGLE_CAMERA; if(method == "from_center_camera") return EAlignmentMethod::FROM_CENTER_CAMERA; if(method == "from_markers") return EAlignmentMethod::FROM_MARKERS; + if(method == "from_gps") return EAlignmentMethod::FROM_GPS; throw std::out_of_range("Invalid SfM alignment method : " + alignmentMethod); } @@ -215,7 +218,8 @@ int aliceVision_main(int argc, char **argv) "\t- auto_from_cameras: Use cameras\n" "\t- auto_from_landmarks: Use landmarks\n" "\t- from_single_camera: Use camera specified by --tranformation\n" - "\t- from_markers: Use markers specified by --markers\n") + "\t- from_markers: Use markers specified by --markers\n" + "\t- from_gps: use gps metadata\n") ("transformation", po::value(&transform)->default_value(transform), "required only for 'transformation' and 'single camera' methods:\n" "Transformation: Align [X,Y,Z] to +Y-axis, rotate around Y by R deg, scale by S; syntax: X,Y,Z;R;S\n" @@ -392,6 +396,16 @@ int aliceVision_main(int argc, char **argv) } break; } + case EAlignmentMethod::FROM_GPS: + { + std::mt19937 randomNumberGenerator; + if(!sfm::computeNewCoordinateSystemFromGpsData(sfmData, randomNumberGenerator, S, R, t)) + { + ALICEVISION_LOG_ERROR("Failed to find a valid transformation from the GPS metadata."); + return EXIT_FAILURE; + } + break; + } } // apply user scale