From 2c99eea38fa7185c146d28e1ddb2f288ec30b18d Mon Sep 17 00:00:00 2001 From: Benjamin Winger Date: Thu, 11 Jan 2018 19:43:24 -0500 Subject: [PATCH] Added geolocation code (adapted from Chris's PR #79) --- modules/core/include/frame.h | 7 ++ modules/core/src/camera.cpp | 11 ++- modules/core/src/frame.cpp | 4 + modules/targetanalysis/CMakeLists.txt | 2 +- modules/targetanalysis/include/gps.h | 41 ++++++++++ modules/targetanalysis/src/gps.cpp | 93 ++++++++++++++++++++++ modules/targetanalysis/test/CMakeLists.txt | 6 ++ modules/targetanalysis/test/gps_test.cpp | 77 ++++++++++++++++++ 8 files changed, 234 insertions(+), 7 deletions(-) create mode 100644 modules/targetanalysis/include/gps.h create mode 100644 modules/targetanalysis/src/gps.cpp create mode 100644 modules/targetanalysis/test/gps_test.cpp diff --git a/modules/core/include/frame.h b/modules/core/include/frame.h index 77fec432..e99b655c 100644 --- a/modules/core/include/frame.h +++ b/modules/core/include/frame.h @@ -106,6 +106,13 @@ class Frame{ */ cv::Mat* undistort(Camera &camera); + /** + * @brief Camera associated with the frame + * + * @return Camera associated with the frame + */ + Camera & get_camera(); + private: /** diff --git a/modules/core/src/camera.cpp b/modules/core/src/camera.cpp index 63fb9747..65e5771b 100644 --- a/modules/core/src/camera.cpp +++ b/modules/core/src/camera.cpp @@ -36,19 +36,18 @@ Mat * Camera::undistort(Mat & img) { Camera Camera::TestCamera() { double cameraMatrix[] = { - 2.4052826789763981e+03, 0, 2000, - 0, 2.4052826789763981e+03, 1500, + 2410, 0, 3000, + 0, 2410, 2000, 0, 0, 1 }; double distMatrix[] = { - 6.0190515380007324e-02, -1.8618345553370965e+00, 0, 0, - 2.9590336363673964e+00 + 0, 0, 0, 0, 0 }; return Camera( - Size(4000, 3000), - Size2d(120, 90), + Size(6000, 4000), + Size2d(73.74, 53.13), Mat( Size(3, 3), CV_8UC1, diff --git a/modules/core/src/frame.cpp b/modules/core/src/frame.cpp index 09a88269..79208c90 100644 --- a/modules/core/src/frame.cpp +++ b/modules/core/src/frame.cpp @@ -87,3 +87,7 @@ void Frame::save(std::string dir) { exifData["Exif.Photo.UserComment"] = ss.str(); image->writeMetadata(); } + +Camera & Frame::get_camera() { + return camera; +} diff --git a/modules/targetanalysis/CMakeLists.txt b/modules/targetanalysis/CMakeLists.txt index 467c785d..2ddab890 100644 --- a/modules/targetanalysis/CMakeLists.txt +++ b/modules/targetanalysis/CMakeLists.txt @@ -1,6 +1,6 @@ include_directories(include ../core/include) ADD_DEFINITIONS("-DBOOST_LOG_DYN_LINK") -add_library(TargetAnalysis src/area_analyzer.cpp src/target_analyzer.cpp src/target_loader.cpp) +add_library(TargetAnalysis src/area_analyzer.cpp src/target_analyzer.cpp src/target_loader.cpp src/gps.cpp) target_link_libraries(TargetAnalysis ${Boost_LIBRARIES} Core) target_compile_features(TargetAnalysis PRIVATE) add_subdirectory("test") diff --git a/modules/targetanalysis/include/gps.h b/modules/targetanalysis/include/gps.h new file mode 100644 index 00000000..54cd406c --- /dev/null +++ b/modules/targetanalysis/include/gps.h @@ -0,0 +1,41 @@ +/** + * @file gps.cpp + * @author WARG + * + * @section LICENSE + * + * Copyright (c) 2015-2017, Waterloo Aerial Robotics Group (WARG) + * All rights reserved. + * + * This software is licensed under a modified version of the BSD 3 clause license + * that should have been included with this software in a file called COPYING.txt + * Otherwise it is available at: + * https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt + */ + +#ifndef GPS_H +#define GPS_H + +#include "frame.h" + +static const double GPS_TOLERANCE = 0.0000001; + +/** + * @brief Determines GPS location of a point in an image + * @param point Location in the image (pixels) + * @param f Image + * @param returnResult Resulting location in the image (in latitude and longitude) + * + * @return true on success, false on failure + */ +int get_gps(cv::Point2d point, Frame *f, cv::Point2d *returnResult); + +/** + * @brief Calculates the distance (in metres) between two sets of GPS co-ordinates + * + * Calculates distance in metres between two sets of GPS co-ordinates. + * Inaccurate for long distances since it calculates the direct distance rather than the distance along the surface of the earth. + */ +double gps_dist(cv::Point2d p1, cv::Point2d p2); + +#endif // GPS_H diff --git a/modules/targetanalysis/src/gps.cpp b/modules/targetanalysis/src/gps.cpp new file mode 100644 index 00000000..6e0841c3 --- /dev/null +++ b/modules/targetanalysis/src/gps.cpp @@ -0,0 +1,93 @@ +/** + * @file gps.cpp + * @author WARG + * + * @section LICENSE + * + * Copyright (c) 2015-2017, Waterloo Aerial Robotics Group (WARG) + * All rights reserved. + * + * This software is licensed under a modified version of the BSD 3 clause license + * that should have been included with this software in a file called COPYING.txt + * Otherwise it is available at: + * https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt + */ + +#include +#include +#include "frame.h" + +#define RAD2DEG(rad) ((rad)*180.0/M_PI) +#define DEG2RAD(deg) ((deg)*M_PI/180.0) +#define EARTH_RADIUS 6371000 + +//Based on the GPS location of the image, calculates the +//GPS location of a certain pixel in the image. +bool get_gps(cv::Point2d point, Frame* f, cv::Point2d* returnResult){ + if (f == NULL) { + BOOST_LOG_TRIVIAL(error) << "Frame is null"; + return false; + } + BOOST_LOG_TRIVIAL(trace) << "get_gps(" << point << ", " << f->get_id() << ")"; + + const Metadata* m = f->get_metadata(); + cv::Mat img = f->get_img(); + int h = img.cols; + int w = img.rows; + + if (w <= 0 || h <= 0){ + BOOST_LOG_TRIVIAL(error) << "Invalid frame size w:" << w << " h:" << h; + return false; + } + + cv::Point2d imgCenter(w/2, h/2); + + //(0,0) is in the center of the image + cv::Point2d biasedPoint = point - imgCenter; + + double altitude = m->altitude; + double heading = m->heading; + double latitude = m->lat; + double longitude = m->lon; + + BOOST_LOG_TRIVIAL(trace) << "Camera FOV: " << f->get_camera().get_fov(); + + BOOST_LOG_TRIVIAL(trace) << "Dist from Center (pixels: " << biasedPoint; + + double cameraXEdge = altitude * tan(DEG2RAD(f->get_camera().get_fov().width/2)); //meters from center of photo to edge + double cameraYEdge = altitude * tan(DEG2RAD(f->get_camera().get_fov().height/2)); //meters from center of photo to edge + + BOOST_LOG_TRIVIAL(trace) << "X Edge: " << cameraXEdge << " Y Edge: " << cameraYEdge; + + //Rotation Matrix - Heading + //Note: The '-heading' compensates for the fact that directional heading is + //a clockwise quantity, but cos(theta) assumes theta is a counterclockwise + //quantity. + double realX = cos(DEG2RAD(-heading)) * biasedPoint.x/(w/2)*cameraXEdge - sin(DEG2RAD(-heading)) * biasedPoint.y/(h/2)*cameraYEdge; + double realY = sin(DEG2RAD(-heading)) * biasedPoint.x/(w/2)*cameraXEdge + cos(DEG2RAD(-heading)) * biasedPoint.y/(h/2)*cameraYEdge; + + BOOST_LOG_TRIVIAL(trace) << "Real X: " << realX << " Real Y: " << realY; + + double lon = RAD2DEG(realX/EARTH_RADIUS)/cos(DEG2RAD(latitude)) + longitude; + double lat = RAD2DEG(realY/EARTH_RADIUS) + latitude; + + BOOST_LOG_TRIVIAL(trace) << "Distance from centre: " << RAD2DEG(realY/EARTH_RADIUS) << " " << RAD2DEG(realX/EARTH_RADIUS)/cos(DEG2RAD(latitude)); + + BOOST_LOG_TRIVIAL(trace) << std::setprecision(10) << "Result: " << lat << " " << lon; + + *returnResult = cv::Point2d(lat,lon); + return true; +} + +double gps_dist(cv::Point2d p1, cv::Point2d p2) { + double dLat = DEG2RAD(p2.x-p1.x); + double dLon = DEG2RAD(p2.y-p1.y); + + double rLat1 = DEG2RAD(p1.x); + double rLat2 = DEG2RAD(p2.x); + + double a = sin(dLat/2) * sin(dLat/2) + + sin(dLon/2) * sin(dLon/2) * cos(rLat1) * cos(rLat2); + double c = 2 * atan2(sqrt(a), sqrt(1-a)); + return EARTH_RADIUS * c; +} diff --git a/modules/targetanalysis/test/CMakeLists.txt b/modules/targetanalysis/test/CMakeLists.txt index d597a3e4..3422188c 100644 --- a/modules/targetanalysis/test/CMakeLists.txt +++ b/modules/targetanalysis/test/CMakeLists.txt @@ -3,8 +3,14 @@ enable_testing() if(Boost_FOUND AND OpenCV_FOUND) ADD_DEFINITIONS("-DBOOST_LOG_DYN_LINK") add_executable(target_loader_test test.cpp) + add_executable(gps_test gps_test.cpp) + target_link_libraries(target_loader_test ${OpenCV_LIBS} ${Boost_LIBRARIES} Core TargetAnalysis) + target_link_libraries(gps_test ${OpenCV_LIBS} ${Boost_LIBRARIES} Core TargetAnalysis) # Tests add_test("SimpleLoad" target_loader_test "${TESTDATA_DIR}/sample.json" "Simple Load" --log_format=XML --log_sink=TEST_LOADER.xml --log_level=all --report_level=no) + + add_test("GpsTest" gps_test --log_format=XML --log_sink=TEST_GPS.xml + --log_level=all --report_level=no) endif() diff --git a/modules/targetanalysis/test/gps_test.cpp b/modules/targetanalysis/test/gps_test.cpp new file mode 100644 index 00000000..6f4086d6 --- /dev/null +++ b/modules/targetanalysis/test/gps_test.cpp @@ -0,0 +1,77 @@ +/** + * @file gps_test.cpp + * @author WARG + * + * @section LICENSE + * + * Copyright (c) 2015-2017, Waterloo Aerial Robotics Group (WARG) + * All rights reserved. + * + * This software is licensed under a modified version of the BSD 3 clause license + * that should have been included with this software in a file called COPYING.txt + * Otherwise it is available at: + * https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt + */ + +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MODULE TargetAnalysis + +#include +#include +#include +#include +#include "gps.h" + +using namespace std; +using namespace boost; + +namespace logging = boost::log; + +void gps_check(cv::Point2d point, cv::Point2d expected, Frame *f) { + cv::Point2d result; + if (get_gps(point, f, &result)) { + BOOST_TEST_MESSAGE("Result: " << result); + BOOST_TEST_MESSAGE("Expected: " << expected); + BOOST_TEST_MESSAGE("Error distance: " << gps_dist(result, expected)); + BOOST_CHECK(norm(result - expected) < GPS_TOLERANCE); + } else { + BOOST_TEST_FAIL("get_gps returned failure"); + } +} + +BOOST_AUTO_TEST_CASE(GpsTest){ + Metadata m; + m.lat = 43.47181; + m.lon = -80.54366; + m.altitude = 100; + m.heading = 0; + Camera testcamera = Camera::TestCamera(); + Frame f( + new cv::Mat(6000, 4000, CV_32F), + "test_image", + m, + testcamera + ); + + gps_check(cv::Point2d(3000, 2000), cv::Point2d(43.47181, -80.54366), &f); + + gps_check(cv::Point2d(1500, 1000), cv::Point2d(43.47158517, -80.54412471), &f); + + m.heading = 180; + Frame f2( + new cv::Mat(6000, 4000, CV_32F), + "test_image", + m, + testcamera + ); + gps_check(cv::Point2d(4500, 3000), cv::Point2d(43.47158517, -80.54412471), &f2); + + m.heading = 90; + Frame f3( + new cv::Mat(6000, 4000, CV_32F), + "test_image", + m, + testcamera + ); + gps_check(cv::Point2d(4000, 500), cv::Point2d(43.47158517, -80.54412471), &f3); +}