Added geolocation code (adapted from Chris's PR #79)

Author: Benjamin Winger

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:
 
     /**

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,

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;
+}

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")

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

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 <iomanip>
+#include <boost/log/trivial.hpp>
+#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;
+}

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()

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 <boost/test/unit_test.hpp>
+#include <boost/log/core.hpp>
+#include <boost/log/trivial.hpp>
+#include <boost/log/expressions.hpp>
+#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);
+}