Merged Conflicts

Author: Chris Hajduk

main.cpp

@@ -54,6 +54,7 @@
 #include "metadata_input.h"
 #include "metadata_reader.h"
 #include "target.h"
+#include "camera.h"
 
 using namespace std;
 using namespace boost;
@@ -87,6 +88,72 @@ MetadataInput *logReader = new MetadataInput();
 double aveFrameTime = 1000;
 int frameCount = 0;
 
+double fisheyeMatrix[] = {
+    2.4052826789763981e+03, 0, 2000,
+    0, 2.4052826789763981e+03, 1500,
+    0, 0, 1
+};
+
+double fisheyeDistortion[] = {
+    -0.392, 0.146, 0, 0, -0.023
+};
+
+double newFisheyeMatrix[] = {
+    2000, 0, 2000,
+    0, 2000, 1500,
+    0, 0, 1
+};
+
+double rectMatrix[] = {
+    2410, 0, 960,
+    0, 2410, 540,
+    0, 0, 1
+};
+
+double rectDistortion[] = {
+    0, 0, 0, 0, 0
+};
+
+Camera goProFisheye(
+    Size(4000, 3000),
+    Size2d(120, 90),
+    Mat(
+        Size(3, 3),
+        CV_64F,
+        fisheyeMatrix
+    ),
+    Mat(
+        Size(5, 1),
+        CV_64F,
+        fisheyeDistortion
+    ),
+    Mat(
+        Size(3, 3),
+        CV_64F,
+        newFisheyeMatrix
+    )
+);
+
+Camera goProRect(
+    Size(1920, 1080),
+    Size2d(90, 67.5),
+    Mat(
+        Size(3, 3),
+        CV_64F,
+        rectMatrix
+    ),
+    Mat(
+        Size(5, 1),
+        CV_64F,
+        rectDistortion
+    ),
+    Mat(
+        Size(3, 3),
+        CV_64F,
+        rectMatrix
+    )
+);
+
 struct State {
     bool hasImageSource;
     bool hasMetadataSource;
@@ -149,7 +216,6 @@ void output() {
         }
         boost::this_thread::sleep(boost::posix_time::milliseconds(30));
     }
-    ioService.stop();
 }
 
 void init() {
@@ -171,7 +237,7 @@ int main(int argc, char** argv) {
     processors = boost::thread::hardware_concurrency();
 
     while (!cin.eof()) handle_input();
-
+    ioService.stop();
     threadpool.join_all();
     delete logReader;
     return 0;
@@ -246,7 +312,7 @@ vector<Command> commands = {
         if (logReader->num_sources() == 0) {
             BOOST_LOG_TRIVIAL(error) << "Cannot add image source until a metadata source has been specified";
         } else {
-            importer.add_source(new PictureImport(args[0], logReader), stol(args[1]));
+            importer.add_source(new PictureImport(args[0], logReader, goProFisheye), stol(args[1]));
             newState.hasImageSource = true;
         }
     }),
@@ -255,7 +321,7 @@ vector<Command> commands = {
         if (logReader->num_sources() == 0) {
             BOOST_LOG_TRIVIAL(error) << "Cannot add image source until a metadata source has been specified";
         } else {
-            importer.add_source(new DeckLinkImport(logReader), stol(args[0]));
+            importer.add_source(new DeckLinkImport(logReader, goProRect), stol(args[0]));
             newState.hasImageSource = true;
         }
     }),
@@ -272,7 +338,9 @@ vector<Command> commands = {
 void handle_state_change(State &newState) {
     if (!currentState.readingImages && newState.readingImages) {
         if (newState.hasImageSource && newState.hasMetadataSource) {
+            currentState.readingImages = true;
             queue_work(assign_workers);
+            queue_work(output);
         } else {
             BOOST_LOG_TRIVIAL(error) << "Trying to read images without both image and metadata source is not supported";
         }
@@ -340,25 +408,29 @@ int handle_args(int argc, char** argv) {
 
         if (vm.count("telemetry")) {
             logReader->add_source(new MetadataReader(*logReader, vm["telemetry"].as<string>()));
-            currentState.hasMetadataSource = true;
+            newState.hasMetadataSource = true;
+            cout << "Adding Telemetry source..." << endl;
         }
 
         if (vm.count("addr") && vm.count("port")) {
             logReader->add_source(new MetadataReader(*logReader, vm["addr"].as<string>(), vm["port"].as<string>()));
             newState.hasMetadataSource = true;
+            cout << "Adding Telemetry source..." << endl;
         }
 
 #ifdef HAS_DECKLINK
         if (vm.count("decklink")) {
-            importer.add_source(new DeckLinkImport(logReader), 500);
+            importer.add_source(new DeckLinkImport(logReader, goProRect), 500);
             newState.hasImageSource = true;
+            cout << "Adding video source..." << endl;
         }
 #endif // HAS_DECKLINK
 
         if (vm.count("images")) {
             string path = vm["images"].as<string>();
-            importer.add_source(new PictureImport(path, logReader), 0);
+            importer.add_source(new PictureImport(path, logReader, goProFisheye), 0);
             newState.hasImageSource = true;
+            cout << "Adding picture source..." << endl;
         }
 
         if (vm.count("output")) {

modules/core/CMakeLists.txt

@@ -1,3 +1,3 @@
 include_directories(include ${EXIV2_INCLUDE_DIR})
-add_library(Core  src/target.cpp src/object.cpp src/pixel_object.cpp src/frame.cpp src/benchmark.cpp)
+add_library(Core  src/target.cpp src/object.cpp src/pixel_object.cpp src/frame.cpp src/benchmark.cpp src/camera.cpp)
 target_link_libraries(Core ${EXIV2_LIBRARIES} ${OpenCV_LIBS})

modules/core/include/camera.h

@@ -0,0 +1,64 @@
+/**
+ * @file camera.h
+ * @author WARG
+ *
+ * @section LICENSE
+ *
+ *  Copyright (c) 2015, 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 CAMERA_H_INCLUDED
+#define CAMERA_H_INCLUDED
+
+/**
+ * @class Camera
+ *
+ * @brief Container class for storing information about
+ *     the camera used to capture an image *
+ */
+
+
+class Camera {
+public:
+    /**
+     * @brief creates a Camera representing the specs for a given lens and sensor
+     *
+     * @param imageSize Size of the camera image
+     * @param fov The field of view of the camera
+     * @param cameraMatrix OpenCV camera calibration matrix giving focal length and principal point
+     * @param distortionCoeffs OpenCV distortion coefficients used for undistorting images
+     */
+    Camera(cv::Size imageSize, cv::Size2d fov, cv::Mat cameraMatrix, cv::Mat distortionCoeffs, cv::Mat newCameraMatrix);
+
+    /**
+     * @brief Returns the field of view of the camera in degrees for both horizontal and vertical dimensions
+     */
+    cv::Size2d get_fov();
+
+    /**
+     * @brief Undistorts image
+     * @param img mat to be undistorted
+     * @return An orthorectified version of the given mat
+     */
+    cv::Mat * undistort(cv::Mat &img);
+
+    /**
+     * @brief Example Camera for testing purposes
+     */
+    static Camera TestCamera();
+
+private:
+    cv::Size2d fov;
+    cv::Mat cameraMatrix;
+    cv::Mat distortionCoeffs;
+    cv::Mat newCameraMatrix;
+};
+
+#endif //CAMERA_H_INCLUDED

modules/core/include/frame.h

@@ -24,13 +24,15 @@
 #include <opencv2/core/core.hpp>
 #include "metadata.h"
 #include <vector>
+#include "camera.h"
 
 class PixelObject;
 
 class Frame{
 public:
-    Frame(cv::Mat * img, std::string id, Metadata m);
-    
+    Frame(cv::Mat * img, std::string id, Metadata m, Camera &camera);
+
+    ~Frame();
     /**
      * @brief 
      *
@@ -43,7 +45,13 @@ class Frame{
      *
      * @return image associated with the frame
      */
-    cv::Mat & get_img(); 
+    cv::Mat & get_img();
+
+    /**
+     * @brief Original Image
+     * @return original image before any calibration
+     */
+    cv::Mat & orig_img();
 
     /**
      * @brief Adds given object to the list of objects in the frame
@@ -97,13 +105,26 @@ class Frame{
      */
     void set_pixel_distance(double x, double y); 
 
+    /*
+     * @brief returns a new undistorted image using the given camera
+     * Intended for testing purposes, Does not modify the Frame.
+     * @param camera the camera to use for undistortion
+     * @return A new image that is the original image contained in this frame undistorted with the camera
+     */
+    cv::Mat* undistort(Camera &camera);
+
 private:
 
     /**
      * @brief image associated with the Frame 
      */
     cv::Mat * img;
 
+    /**
+     * @brief original image without undistortion
+     */
+    cv::Mat * origImg;
+
     /**
      * @brief Timestamp for the frame
      */
@@ -119,11 +140,18 @@ class Frame{
      */
     std::vector<PixelObject *> objects;
 
+<<<<<<< HEAD
     /**
      * @brief Distance of each pixel covers in the x and y direction with
      * componesation for altitude, camera lens, etc.
      */
     cv::Point2d pixel_distance; 
+=======
+    /*
+     * @brief Camera used to capture the frame
+     */
+    Camera &camera;
+>>>>>>> 7757b8909d05a2df51e4b260f0cf9b37010ecbba
 };
 
 

modules/core/src/camera.cpp

@@ -0,0 +1,63 @@
+/**
+ * @file camera.h
+ * @author WARG
+ *
+ * @section LICENSE
+ *
+ *  Copyright (c) 2015, 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 <opencv2/calib3d.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/core/core.hpp>
+#include "camera.h"
+
+using namespace cv;
+
+Camera::Camera(Size imageSize, Size2d fov, Mat cameraMatrix, Mat distortionCoeffs, Mat newCameraMatrix = Mat())
+        : cameraMatrix(cameraMatrix), distortionCoeffs(distortionCoeffs), newCameraMatrix(newCameraMatrix), fov(fov) {
+}
+
+Mat * Camera::undistort(Mat & img) {
+    Mat *tmp = new Mat();
+    cv::undistort(img, *tmp, cameraMatrix, distortionCoeffs, newCameraMatrix);
+    return tmp;
+}
+
+Camera Camera::TestCamera() {
+    double cameraMatrix[] = {
+        2.4052826789763981e+03, 0, 2000,
+        0, 2.4052826789763981e+03, 1500,
+        0, 0, 1
+    };
+
+    double distMatrix[] = {
+        6.0190515380007324e-02, -1.8618345553370965e+00, 0, 0,
+        2.9590336363673964e+00
+    };
+
+    return Camera(
+        Size(4000, 3000),
+        Size2d(120, 90),
+        Mat(
+            Size(3, 3),
+            CV_8UC1,
+            cameraMatrix
+        ),
+        Mat(
+            Size(5, 1),
+            CV_8UC1,
+            distMatrix
+        )
+    );
+}
+
+Size2d Camera::get_fov() {
+    return fov;
+}