CV Pipeline Integration

.devcontainer/devcontainer.json

@@ -2,9 +2,6 @@
 {
 	"name": "Existing Dockerfile",
 	"image": "ghcr.io/tritonuas/obcpp:main",
-	// "build": {
-	// 	"dockerfile": "Dockerfile"
-	// },
 
 	"customizations": {
 		"vscode": {

.gitignore

@@ -13,6 +13,12 @@ _deps/
 Testing/
 imgs/
 
+models/*
+!models/.gitkeep
+
+tests/integration/images/*
+!tests/integration/images/.gitkeep
+
 .vscode/*
 !.vscode/c_cpp_properties.json
 

CMakeLists.txt

@@ -119,7 +119,7 @@ target_add_protobuf(playground)
 target_add_loguru(playground)
 
 # load_torchvision_model
-add_executable(load_torchvision_model tests/integration/load_torchvision_model.cpp)
+add_executable(load_torchvision_model "tests/integration/load_torchvision_model.cpp")
 target_add_torch(load_torchvision_model)
 target_add_torchvision(load_torchvision_model)
 
@@ -136,6 +136,40 @@ target_add_mavsdk(mavlink_client)
 target_add_matplot(mavlink_client)
 target_add_loguru(mavlink_client)
 
+# cv_pipeline 
+add_executable(cv_pipeline ${SOURCES} "tests/integration/cv_pipeline.cpp")
+target_add_json(cv_pipeline)
+target_add_matplot(cv_pipeline)
+target_add_opencv(cv_pipeline)
+target_add_loguru(cv_pipeline)
+target_add_httplib(cv_pipeline)
+target_add_protobuf(cv_pipeline)
+target_add_torch(cv_pipeline)
+target_add_torchvision(cv_pipeline)
+target_add_mavsdk(cv_pipeline)
+target_add_loguru(cv_pipeline)
+
+# cv_matching
+add_executable(cv_matching ${SOURCES} "tests/integration/cv_matching.cpp")
+target_add_json(cv_matching)
+target_add_matplot(cv_matching)
+target_add_opencv(cv_matching)
+target_add_torch(cv_matching)
+target_add_torchvision(cv_matching)
+target_add_loguru(cv_matching)
+target_add_httplib(cv_matching)
+target_add_mavsdk(cv_matching)
+
+# cv_segmentation
+add_executable(cv_segmentation ${SOURCES} "tests/integration/cv_segmentation.cpp")
+target_add_json(cv_segmentation)
+target_add_matplot(cv_segmentation)
+target_add_opencv(cv_segmentation)
+target_add_torch(cv_segmentation)
+target_add_torchvision(cv_segmentation)
+target_add_loguru(cv_segmentation)
+target_add_httplib(cv_segmentation)
+target_add_mavsdk(cv_segmentation)
 # path_plotting 
 add_executable(path_plotting ${SOURCES} tests/integration/path_plotting.cpp)
 target_add_torch(path_plotting)
@@ -163,6 +197,33 @@ add_subdirectory(tests/unit)
 add_subdirectory(${DEPS_DIRECTORY}/google-test)
 # =============================
 
+# =============================
+# Pull models
+add_custom_target(pull_models 
+  DEPENDS pull_saliency pull_matching pull_segmentation
+)
+
+# Saliency model
+add_custom_target(pull_saliency
+  COMMAND gdown https://drive.google.com/uc?id=1S1IfXlGs_pCH49DwZmbD-tZA5YH0A1gx -O ${CMAKE_BINARY_DIR}/../models/torchscript_19.pth
+)
+
+# Matching model 
+add_custom_target(pull_matching
+  COMMAND gdown https://drive.google.com/file/d/1NeFiAfSSLXAZWlehfd0ox7p_jFF4YdrO -O ${CMAKE_BINARY_DIR}/../models/target_siamese_1.pt
+)
+
+# Segmentation model
+add_custom_target(pull_segmentation
+  COMMAND gdown https://drive.google.com/file/d/1U2EbfJFzcjVnjTuD6ud-bIf8YOiEassf -O ${CMAKE_BINARY_DIR}/../models/fcn-model_20-epochs_06-01-2023T21-16-02.pth
+)
+# =============================
+
+# =============================
+# Pull testing images
+# 
+# =============================
+
 # =============================
 # Linting 
 
@@ -172,6 +233,7 @@ if(CPPLINT)
   # define lint target
   add_custom_target(lint
     COMMAND cpplint
+    # Do not require licenses, TODO assignment, Google versions of C++ libs
     --filter=-legal,-readability/todo,-build/c++11
     --linelength=100
     --recursive

include/camera/interface.hpp

@@ -3,10 +3,26 @@
 
 #include <string>
 #include <vector>
+#include <memory>
 
 #include <nlohmann/json.hpp>
 #include <opencv2/opencv.hpp>
 
+// class to contain all telemetry that should be tagged with an image.
+// In the future this could be in a mavlink file.
+class ImageTelemetry {
+ public:
+    ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double yaw,
+                   double pitch, double roll);
+    const double latitude;
+    const double longitude;
+    const double altitude;
+    const double airspeed;
+    const double yaw;
+    const double pitch;
+    const double roll;
+};
+
 /*
  * FYI: this is class that will standardize image data but
  * if all of our cameras have a uniform image output type
@@ -19,14 +35,17 @@
 class ImageData {
  private:
     const std::string NAME;
-    const std::string PATHS;
+    const std::string PATH;
     const cv::Mat DATA;
+    const ImageTelemetry TELEMETRY;
 
  public:
-    ImageData(std::string NAME, std::string PATH, cv::Mat DATA);
-    std::string getName();
-    std::string getPath();
-    cv::Mat getData();
+    ImageData(std::string NAME, std::string PATH, cv::Mat DATA, ImageTelemetry TELEMETRY);
+    ImageData(const ImageData&) = default;
+    std::string getName() const;
+    std::string getPath() const;
+    cv::Mat getData() const;
+    ImageTelemetry getTelemetry() const;
 };
 
 // ? possibly convert most common / important json fields to
@@ -40,7 +59,7 @@ class CameraConfiguration {
 
     void updateConfig(nlohmann::json newSetting);
 
-    void updateConfigField(std::string key, T value);
+    // void updateConfigField(std::string key, T value);
 
     nlohmann::json getConfig();
 
@@ -50,8 +69,8 @@ class CameraConfiguration {
 class CameraInterface {
  private:
     CameraConfiguration config;
-    ImageData recentPicture;  // might need to move it to public
-    bool doneTakingPicture;      // overengineering time
+    std::unique_ptr<ImageData> recentPicture;  // might need to move it to public
+    bool doneTakingPicture;                    // overengineering time
     std::string uploadPath;
     // Interpreter interp
     // TODO: SERVER CONNECTION HERE ?
@@ -61,19 +80,21 @@ class CameraInterface {
  public:
     explicit CameraInterface(CameraConfiguration config);
 
-    void connect();
+    virtual ~CameraInterface() = default;
+
+    virtual void connect() = 0;
 
-    bool verifyConnection();
+    virtual bool verifyConnection() = 0;
 
-    void takePicture();
+    virtual void takePicture() = 0;
 
-    ImageData getLastPicture();
+    virtual ImageData getLastPicture() = 0;
 
-    bool takePictureForSeconds(int sec);
+    virtual bool takePictureForSeconds(int sec) = 0;
 
-    void startTakingPictures(double intervalSec);
+    virtual void startTakingPictures(double intervalSec) = 0;
 
-    bool isDoneTakingPictures();
+    virtual bool isDoneTakingPictures() = 0;
 
     CameraConfiguration getConfig();
 

include/camera/mock.hpp

@@ -0,0 +1,24 @@
+#ifndef INCLUDE_CAMERA_MOCK_HPP_
+#define INCLUDE_CAMERA_MOCK_HPP_
+
+#include <memory>
+
+#include "camera/interface.hpp"
+
+class MockCamera : public CameraInterface {
+ public:
+    explicit MockCamera(CameraConfiguration config);
+    ~MockCamera() = default;
+    void connect() override;
+    bool verifyConnection() override;
+    void takePicture() override;
+    ImageData getLastPicture() override;
+    bool takePictureForSeconds(int sec) override;
+    void startTakingPictures(double intervalSec) override;
+    bool isDoneTakingPictures() override;
+
+ private:
+    std::unique_ptr<ImageData> lastPicture;
+};
+
+#endif  // INCLUDE_CAMERA_MOCK_HPP_

include/cv/classification.hpp

@@ -0,0 +1,55 @@
+#ifndef INCLUDE_CV_CLASSIFICATION_HPP_
+#define INCLUDE_CV_CLASSIFICATION_HPP_
+
+#include <string>
+#include <opencv2/opencv.hpp>
+
+struct ClassificationResults {
+    // TODO: replace with protobuf structs instead of strings
+    std::string shape;
+    std::string shapeColor;
+    std::string character;
+    std::string characterColor;
+};
+
+// Classification is responsible for predicting characteristics about
+// ground comptition targets. These characterisitcs include shape type,
+// alphanumeric character type, shape color, and alphanumeric character color.
+// Currently, the shape and character classifiers are implemented using a
+// Convolutional Neural Network (CNN).
+// The color classifier is implented using a K-nearest neigbors model (KNN)
+// The implementation of the models themselves can be found here:
+// https://github.com/tritonuas/taxonomy-101
+// In this class we will take the pretrained models and use them to make
+// inferences.
+class Classification {
+ public:
+    // classify takes a cropped image of the target (saliency output) and
+    // two binary masks to represent which region of pixels correspond to
+    // shape and character respectivel (output of segmentation). Using this
+    // data, the shape type, character type, shape color and character color
+    // will be predicted.
+    ClassificationResults classify(cv::Mat croppedImage, cv::Mat shapeMask, cv::Mat characterMask);
+
+ private:
+    // classifyShape takes a cropped image of the target (output of saliency)
+    // and a binary mask (output of segmentation). The binary mask should
+    // represent which region of pixels correspond to the shape region of
+    // the target.
+    std::string classifyShape(cv::Mat croppedImage, cv::Mat shapeMask);
+
+    // classifyShape takes a cropped image of the target (output of saliency)
+    // and a binary mask (output of segmentation). The binary mask should
+    // represent which region of pixels correspond to the character region of
+    // the target.
+    std::string classifyCharacter(cv::Mat croppedImage, cv::Mat characterMask);
+
+    // classify the primary color of a region described by a binary mask.
+    // This can be used for finding both shape and character color since
+    // we will use the same algorithm to detect the primary color in
+    // whatever region the mask describes. All that changes is the mask
+    // that's passed in.
+    std::string classifyColor(cv::Mat croppedImage, cv::Mat mask);
+};
+
+#endif  // INCLUDE_CV_CLASSIFICATION_HPP_

include/cv/localization.hpp

@@ -0,0 +1,86 @@
+#ifndef INCLUDE_CV_LOCALIZATION_HPP_
+#define INCLUDE_CV_LOCALIZATION_HPP_
+
+#include "cv/utilities.hpp"
+
+#include "camera/interface.hpp"
+#include "utilities/datatypes.hpp"
+
+// TODO: these should be constants in the config file
+// (or maybe queried by camera)
+#define PIXEL_SIZE_MM 0.0024
+#define FOCAL_LENGTH_MM 50
+#define IMG_WIDTH_PX 5472
+#define IMG_HEIGHT_PX 3648
+
+// Localization is responsible for calculating the real world latitude/longitude
+// of competition targets.
+// See our Python implementation here: https://github.com/tritonuas/localization
+class Localization {
+ public:
+    // localize is responsible for transforming the position of a target
+    // within a full resolution image (image coordinates) to it's position
+    // in the real world (latitude/longitude coords). We are given the
+    // pixel coordinates of the target from saliency and the GPS position of
+    // the plane at the time of image capture.
+    //
+    // TODO: also need to pass in camera/lens information such as sensor width,
+    // focal length, and image width/height
+    virtual GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) = 0;
+
+ protected:
+    struct CameraIntrinsics {
+        double pixelSize;   //mm
+        double focalLength; //mm
+        double resolutionX; //Pixels
+        double resolutionY; //Pixels
+    };
+
+    CameraIntrinsics camera{
+        .pixelSize = PIXEL_SIZE_MM,
+        .focalLength = FOCAL_LENGTH_MM,
+        .resolutionX = IMG_WIDTH_PX,
+        .resolutionY = IMG_HEIGHT_PX,
+    };
+};
+
+// Localization by converting via ECEF coordinates (Earth Centered, Earth Fixed)
+class ECEFLocalization : Localization {
+ public:
+    GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) override;
+ private:
+    // ECEF - Earth Centered, Earth Fixed coordinate system. 0,0,0 is the center of the Earth.
+    struct ECEFCoordinates {
+        double x; //Meters in the plane of the equator in the direction of the prime meridian
+        double y; //Meters in the plane of the equator in the direction of 90 degrees East
+        double z; //Meters in the direction of the North pole
+    };
+
+    // ENU - East, North, Up coordinate system. Used to show an offset from a certain location on the Earth.
+    struct ENUCoordinates {
+        double e; //Meters East from reference location
+        double n; //Meters North from reference location
+        double u; //Meters Up from reference location
+    };
+
+
+    struct CameraVector {
+        double roll;    //Radians
+        double pitch;   //Radians
+        double heading; //Radians
+    };
+
+    ECEFCoordinates GPStoECEF(GPSCoord gps);
+    ECEFCoordinates ENUtoECEF(ENUCoordinates offset, GPSCoord originGPS);
+    GPSCoord ECEFtoGPS(ECEFCoordinates ecef);
+    CameraVector PixelsToAngle(CameraIntrinsics camera, CameraVector state, double targetX, double targetY);
+    ENUCoordinates AngleToENU(CameraVector target, GPSCoord aircraft, double terrainHeight);
+};
+
+// Localization using GSD (ground sample distance) ratio 
+class GSDLocalization : Localization {
+ public:
+    GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) override;
+};
+
+#endif  // INCLUDE_CV_LOCALIZATION_HPP_