From 5f7b06311c650363b4f7855ff9f9aef17d2e8fd8 Mon Sep 17 00:00:00 2001 From: imisaacwu Date: Thu, 10 Oct 2024 19:23:50 -0700 Subject: [PATCH 01/10] Initial Camera re-work, new .yml configs, gstreamer conversion --- camera-config/HandCameraConfig.yml | 11 +++++ src/Constants.cpp | 24 ++++++---- src/Constants.h | 10 ++-- src/camera/Camera.cpp | 52 +++++++++++++++------ src/camera/Camera.h | 6 ++- src/world_interface/data.cpp | 2 +- src/world_interface/data.h | 2 +- src/world_interface/simulator_interface.cpp | 17 +++---- 8 files changed, 82 insertions(+), 42 deletions(-) create mode 100644 camera-config/HandCameraConfig.yml diff --git a/camera-config/HandCameraConfig.yml b/camera-config/HandCameraConfig.yml new file mode 100644 index 000000000..ea7eeec6c --- /dev/null +++ b/camera-config/HandCameraConfig.yml @@ -0,0 +1,11 @@ +%YAML:1.0 +--- +name: "hand" +description: "Camera located on the hand of the rover." +# Make sure that this camera_id matches the one defined in the udev rules. +camera_id: 20 + +format: "jpeg" +width: 640 +height: 480 +framerate: 30 \ No newline at end of file diff --git a/src/Constants.cpp b/src/Constants.cpp index e9d67ad22..941356d7c 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -47,15 +47,21 @@ const double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_SEC; const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE) .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2); -// TODO: We need to recalibrate the camera, since we replaced it with a different one. -const char* MAST_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml"; -const robot::types::CameraID MAST_CAMERA_ID = "mast"; - -const char* FOREARM_CAMERA_CONFIG_PATH = "../camera-config/WristCameraCalibration.yml"; -const robot::types::CameraID FOREARM_CAMERA_ID = "wrist"; - -const char* HAND_CAMERA_CONFIG_PATH = "../camera-config/HandCameraCalibration.yml"; -const robot::types::CameraID HAND_CAMERA_ID = "hand"; +const robot::types::CameraID MAST_CAMERA_ID = 40; +const robot::types::CameraID WRIST_CAMERA_ID = 30; +const robot::types::CameraID HAND_CAMERA_ID = 20; + +const std::unordered_map CAMERA_CONFIG_PATHS = { + {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, + {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, + {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"} +}; + +const std::unordered_map CAMERA_NAME_TO_ID = { + {"mast", MAST_CAMERA_ID}, + {"wrist", WRIST_CAMERA_ID}, + {"hand", HAND_CAMERA_ID} +}; /** @deprecated No need for this constant once we fully switch over the Mission Control PlanViz diff --git a/src/Constants.h b/src/Constants.h index 01c3e9530..2e853c2a8 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -53,15 +53,13 @@ extern const double MAX_DTHETA; // TODO: We need to recalibrate the camera, since we replaced it with a different one. // TODO: rename cameras (in MC as well) as appropriate -extern const char* MAST_CAMERA_CONFIG_PATH; extern const robot::types::CameraID MAST_CAMERA_ID; - -extern const char* FOREARM_CAMERA_CONFIG_PATH; -extern const robot::types::CameraID FOREARM_CAMERA_ID; - -extern const char* HAND_CAMERA_CONFIG_PATH; +extern const robot::types::CameraID WRIST_CAMERA_ID; extern const robot::types::CameraID HAND_CAMERA_ID; +extern const std::unordered_map CAMERA_CONFIG_PATHS; +extern const std::unordered_map CAMERA_NAME_TO_ID; + extern const uint16_t WS_SERVER_PORT; /** diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 84d06aa73..63dada22a 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -1,9 +1,12 @@ #include "Camera.h" +#include "../Constants.h" #include "CameraConfig.h" #include +#include + using cv::Mat; using cv::Size; using std::string; @@ -14,7 +17,7 @@ namespace cam { Camera::Camera() : _frame(std::make_shared()), _frame_num(std::make_shared(0)), _frame_time(std::make_shared()), - _capture(std::make_shared()), + _capture(), _frame_lock(std::make_shared()), _capture_lock(std::make_shared()), _running(std::make_shared(false)) {} @@ -23,7 +26,9 @@ bool Camera::open(int camera_id, CameraParams intrinsic_params, Mat extrinsic_pa return false; } _capture_lock->lock(); - bool result = this->_capture->open(camera_id); + std::stringstream gstr_ss = GStreamerFromFile(camera_id); + this->_capture = std::make_shared(gstr_ss.str(), cv::CAP_GSTREAMER); + bool result = this->_capture->isOpened(); _capture_lock->unlock(); LOG_F(INFO, "Opening camera %d... %s", camera_id, result ? "success" : "failed"); this->_intrinsic_params = intrinsic_params; @@ -31,18 +36,6 @@ bool Camera::open(int camera_id, CameraParams intrinsic_params, Mat extrinsic_pa return result; } -bool Camera::open(string filename, CameraParams intrinsic_params, Mat extrinsic_params) { - if (*_running) { - return false; - } - _capture_lock->lock(); - bool result = this->_capture->open(filename); - _capture_lock->unlock(); - this->_intrinsic_params = intrinsic_params; - init(extrinsic_params); - return result; -} - Camera::Camera(string filename, string name, string description, CameraParams intrinsic_params, Mat extrinsic_params) : _frame(std::make_shared()), _frame_num(std::make_shared(0)), @@ -103,7 +96,7 @@ bool Camera::openFromConfigFile(std::string filename) { } if (std::holds_alternative(cfg.filenameOrID)) { - return this->open(std::get(cfg.filenameOrID), intrinsics, extrinsics); + // return this->open(std::get(cfg.filenameOrID), intrinsics, extrinsics); } else if (std::holds_alternative(cfg.filenameOrID)) { return this->open(std::get(cfg.filenameOrID), intrinsics, extrinsics); } else { @@ -113,6 +106,35 @@ bool Camera::openFromConfigFile(std::string filename) { } } +std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { + cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id), cv::FileStorage::READ); + if (!fs.isOpened()) { + throw std::invalid_argument("Configuration file of camera ID" + std::to_string(camera_id) + " does not exist"); + } + + std::stringstream gstr_ss; + gstr_ss << "v4l2src device=/dev/video" << camera_id << " ! "; + + const std::string KEY_NAME = "name"; + const std::string KEY_DESCRIPTION = "description"; + const std::string KEY_CAMERA_ID = "camera_id"; + const std::string KEY_FORMAT = "format"; + const std::string KEY_WIDTH = "width"; + const std::string KEY_HEIGHT = "height"; + const std::string KEY_FRAMERATE = "framerate"; + + gstr_ss << "image/" << fs[KEY_FORMAT].operator std::string(); + gstr_ss << ",width=" << fs[KEY_WIDTH].operator std::string(); + gstr_ss << ",height=" << fs[KEY_HEIGHT].operator std::string(); + gstr_ss << ",framerate=" << fs[KEY_FRAMERATE].operator std::string() << "/1"; + gstr_ss << " ! " << fs[KEY_FORMAT].operator std::string() << "dec ! videoconvert"; + + gstr_ss << " ! appsink"; + LOG_F(INFO, "GSTR: %s", gstr_ss.str().c_str()); + + return gstr_ss; +} + void Camera::captureLoop() { loguru::set_thread_name(_name.c_str()); cv::Size image_size(640, 480); diff --git a/src/camera/Camera.h b/src/camera/Camera.h index 9d21c30a5..80cfc6c33 100644 --- a/src/camera/Camera.h +++ b/src/camera/Camera.h @@ -95,8 +95,8 @@ class Camera { @returns true if opening the camera succeeds, and false if not. */ - bool open(std::string filename, CameraParams intrinsic_params = CameraParams(), - cv::Mat extrinsic_params = cv::Mat()); + // bool open(std::string filename, CameraParams intrinsic_params = CameraParams(), + // cv::Mat extrinsic_params = cv::Mat()); /** @brief Opens a Camera that is not already open. @@ -164,6 +164,8 @@ class Camera { */ bool openFromConfigFile(std::string filename); + std::stringstream GStreamerFromFile(robot::types::CameraID camera_id); + /** @brief Returns true if the camera is open. */ diff --git a/src/world_interface/data.cpp b/src/world_interface/data.cpp index 5a741e772..49535e828 100644 --- a/src/world_interface/data.cpp +++ b/src/world_interface/data.cpp @@ -57,7 +57,7 @@ std::string to_string(robot::types::jointid_t joint) { } std::string to_string(const robot::types::CameraID& id) { - return id; + return std::to_string(id); } std::string to_string(robot::types::mountedperipheral_t peripheral) { diff --git a/src/world_interface/data.h b/src/world_interface/data.h index 047b927c9..f3b1eb301 100644 --- a/src/world_interface/data.h +++ b/src/world_interface/data.h @@ -43,7 +43,7 @@ using landmarks_t = std::vector>; /** @brief A pair of a camera frame and its corresponding frame number */ using CameraFrame = std::pair; /** @brief The type of a camera id */ -using CameraID = std::string; +using CameraID = int; /** @brief An indication enum, used to command the LED to flash different signals */ enum class indication_t { diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index f8348baf5..727319d15 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -112,11 +112,11 @@ static void openCamera(CameraID cam, std::optional> list1d = } void initCameras() { - auto cfg = cam::readConfigFromFile(Constants::MAST_CAMERA_CONFIG_PATH); - cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg; - openCamera(Constants::HAND_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); - openCamera(Constants::FOREARM_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); - openCamera(Constants::MAST_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); + // auto cfg = cam::readConfigFromFile(Constants::MAST_CAMERA_CONFIG_PATH); + // cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg; + // openCamera(Constants::HAND_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); + // openCamera(Constants::FOREARM_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); + // openCamera(Constants::MAST_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); } void initMotors() { @@ -148,12 +148,13 @@ void handleIMU(json msg) { void handleCamFrame(json msg) { std::string cam = msg["camera"]; + robot::types::CameraID id = Constants::CAMERA_NAME_TO_ID.at(cam); std::string b64 = msg["data"]; cv::Mat mat = base64::decodeMat(b64); // acquire exclusive lock std::lock_guard lock(cameraFrameMapMutex); - auto entry = cameraLastFrameIdxMap.find(cam); + auto entry = cameraLastFrameIdxMap.find(id); uint32_t idx = 0; if (entry != cameraLastFrameIdxMap.end()) { idx = entry->second + 1; @@ -161,8 +162,8 @@ void handleCamFrame(json msg) { CameraFrame cf = {mat, idx}; DataPoint df(cf); - cameraFrameMap[cam] = df; - cameraLastFrameIdxMap[cam] = idx; + cameraFrameMap[id] = df; + cameraLastFrameIdxMap[id] = idx; } void handleMotorStatus(json msg) { From 438e62b7e334424114f0e4e95fa5bcbd74c24e06 Mon Sep 17 00:00:00 2001 From: imisaacwu Date: Mon, 21 Oct 2024 19:34:19 -0700 Subject: [PATCH 02/10] Removed CameraParams/Configs, refactoring WIP --- src/CMakeLists.txt | 6 +- src/ar/ARTester.cpp | 45 ++-------- src/ar/CMakeLists.txt | 2 - src/ar/Detector.cpp | 5 +- src/ar/Detector.h | 4 +- src/camera/Camera.cpp | 137 +++++++++++-------------------- src/camera/Camera.h | 104 ++++++----------------- src/camera/CameraConfig.cpp | 62 -------------- src/camera/CameraConfig.h | 129 ----------------------------- src/camera/CameraConfig.md | 115 -------------------------- src/camera/CameraParams.cpp | 126 ---------------------------- src/camera/CameraParams.h | 159 ------------------------------------ 12 files changed, 81 insertions(+), 813 deletions(-) delete mode 100644 src/camera/CameraConfig.cpp delete mode 100644 src/camera/CameraConfig.h delete mode 100644 src/camera/CameraConfig.md delete mode 100644 src/camera/CameraParams.cpp delete mode 100644 src/camera/CameraParams.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b55de633b..03cbfaeb9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -132,8 +132,6 @@ endif() add_library(camera SHARED camera/Camera.cpp - camera/CameraParams.cpp - camera/CameraConfig.cpp ) target_link_libraries(camera PUBLIC ${OpenCV_LIBS}) @@ -309,8 +307,8 @@ if(WITH_TESTS) add_executable(tests Tests.cpp # AR Detection tests - ar/DetectorTests.cpp - ar/MarkerSetTests.cpp + # ar/DetectorTests.cpp + # ar/MarkerSetTests.cpp # Camera tests ../tests/camera/CameraParamsTests.cpp # GPS tests diff --git a/src/ar/ARTester.cpp b/src/ar/ARTester.cpp index 866682dd9..bd1dfbb2f 100644 --- a/src/ar/ARTester.cpp +++ b/src/ar/ARTester.cpp @@ -1,6 +1,4 @@ #include "../camera/Camera.h" -#include "../camera/CameraConfig.h" -#include "../camera/CameraParams.h" #include "Detector.h" #include @@ -36,7 +34,6 @@ const std::string keys = // TODO: come up with a better solution for examining video files // cam::Camera cap; cv::VideoCapture cap; -cam::CameraParams PARAMS; std::shared_ptr MARKER_SET; std::vector projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec); @@ -56,8 +53,8 @@ int main(int argc, char* argv[]) { return EXIT_SUCCESS; } - if (!parser.has("c") || parser.get("c").empty()) { - std::cerr << "Error: camera configuration file is required." << std::endl; + if (!parser.has("co")) { + std:cerr << "Error: camera ID required." << std::endl; parser.printMessage(); return EXIT_FAILURE; } @@ -78,35 +75,6 @@ int main(int argc, char* argv[]) { } int cam_id = parser.get("co", -1); - std::string cam_file = parser.get("fo"); - cv::FileStorage cam_config(parser.get("c"), cv::FileStorage::READ); - if (!cam_config.isOpened()) { - std::cerr << "Error: given camera configuration file " << parser.get("c") - << " does not exist!" << std::endl; - cam_config.release(); - return EXIT_FAILURE; - } - if (cam_config[cam::KEY_INTRINSIC_PARAMS].empty()) { - std::cerr << "Error: Intrinsic parameters are required for AR tag detection!" - << std::endl; - cam_config.release(); - return EXIT_FAILURE; - } - cam_config[cam::KEY_INTRINSIC_PARAMS] >> PARAMS; - // read filename or camera ID, and open camera. - if (!cam_config[cam::KEY_FILENAME].empty() && !parser.has("fo")) { - cam_file = (std::string)cam_config[cam::KEY_FILENAME]; - } else if (!cam_config[cam::KEY_CAMERA_ID].empty() && !parser.has("co")) { - cam_id = (int)cam_config[cam::KEY_CAMERA_ID]; - } else if (!parser.has("fo") && !parser.has("co")) { - std::cerr << "Error: no file or camera_id was provided in the configuration file or " - "on the command line!" - << std::endl; - std::cerr << "Usage:" << std::endl; - parser.printMessage(); - return EXIT_FAILURE; - } - cam_config.release(); cv::Mat frame; [[maybe_unused]] uint32_t fnum = 0; @@ -115,8 +83,9 @@ int main(int argc, char* argv[]) { bool open_success = false; if (cam_id > -1) { open_success = cap.open(cam_id); - } else if (!cam_file.empty()) { - open_success = cap.open(cam_file); + } else { + std::cerr << "Error: invalid camera ID" << std::endl; + return EXIT_FAILURE; } if (!open_success) { @@ -124,8 +93,8 @@ int main(int argc, char* argv[]) { return EXIT_FAILURE; } - const int w = PARAMS.getImageSize().width; - const int h = PARAMS.getImageSize().height; + const int w = cap.getWidth(); + const int h = cap.getHeight(); cap.set(cv::CAP_PROP_FRAME_WIDTH, w); cap.set(cv::CAP_PROP_FRAME_HEIGHT, h); std::cout << "Set image dimensions to " << w << " x " << h << std::endl; diff --git a/src/ar/CMakeLists.txt b/src/ar/CMakeLists.txt index 2cf907468..615071c3a 100644 --- a/src/ar/CMakeLists.txt +++ b/src/ar/CMakeLists.txt @@ -3,9 +3,7 @@ find_package(Threads REQUIRED) add_executable(ar_tester ARTester.cpp - ../camera/CameraParams.cpp ../camera/Camera.cpp - ../camera/CameraConfig.cpp Detector.cpp MarkerSet.cpp MarkerPattern.cpp diff --git a/src/ar/Detector.cpp b/src/ar/Detector.cpp index 6d8e4840e..52c2c0590 100644 --- a/src/ar/Detector.cpp +++ b/src/ar/Detector.cpp @@ -1,6 +1,5 @@ #include "Detector.h" -#include "../camera/CameraParams.h" #include "Tag.h" #include @@ -17,9 +16,9 @@ namespace AR { Detector::Detector() { } -Detector::Detector(std::shared_ptr marker_set, cam::CameraParams camera_params, +Detector::Detector(std::shared_ptr marker_set, cv::Ptr detector_params) - : marker_set_(marker_set), camera_params_(camera_params), + : marker_set_(marker_set), detector_params_(detector_params) { if (!this->empty()) { this->detector_params_->markerBorderBits = marker_set->getBorderSize(); diff --git a/src/ar/Detector.h b/src/ar/Detector.h index 3e8534e83..2b1955954 100644 --- a/src/ar/Detector.h +++ b/src/ar/Detector.h @@ -1,6 +1,5 @@ #pragma once -#include "../camera/CameraParams.h" #include "MarkerSet.h" #include "Tag.h" @@ -19,7 +18,6 @@ namespace AR { class Detector { private: std::shared_ptr marker_set_; - cam::CameraParams camera_params_; cv::Ptr detector_params_; cv::Mat map1_, map2_; std::vector _detectTagsImpl(const cv::Mat& input, @@ -28,7 +26,7 @@ class Detector { public: Detector(); - Detector(std::shared_ptr marker_set, cam::CameraParams camera_params, + Detector(std::shared_ptr marker_set, cv::Ptr detector_params = cv::aruco::DetectorParameters::create()); std::vector detectTags(const cv::Mat& input, diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 63dada22a..e7f33144b 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -1,8 +1,6 @@ #include "Camera.h" #include "../Constants.h" -#include "CameraConfig.h" - #include #include @@ -21,7 +19,23 @@ Camera::Camera() _frame_lock(std::make_shared()), _capture_lock(std::make_shared()), _running(std::make_shared(false)) {} -bool Camera::open(int camera_id, CameraParams intrinsic_params, Mat extrinsic_params) { +Camera::Camera(robot::types::CameraID camera_id) + : _frame(std::make_shared()), _frame_num(std::make_shared(0)), + _frame_time(std::make_shared()), + _capture(std::make_shared(camera_id)), + _frame_lock(std::make_shared()), + _capture_lock(std::make_shared()), + _running(std::make_shared(false)) { + open(camera_id); +} + +Camera::Camera(const Camera& other) + : _frame(other._frame), _frame_num(other._frame_num), _frame_time(other._frame_time), + _capture(other._capture), _name(other._name), _description(other._description), + _frame_lock(other._frame_lock), _capture_lock(other._capture_lock), + _thread(other._thread), _running(other._running) {} + +bool Camera::open(robot::types::CameraID camera_id) { if (*_running) { return false; } @@ -31,38 +45,11 @@ bool Camera::open(int camera_id, CameraParams intrinsic_params, Mat extrinsic_pa bool result = this->_capture->isOpened(); _capture_lock->unlock(); LOG_F(INFO, "Opening camera %d... %s", camera_id, result ? "success" : "failed"); - this->_intrinsic_params = intrinsic_params; - init(extrinsic_params); + init(); return result; } -Camera::Camera(string filename, string name, string description, CameraParams intrinsic_params, - Mat extrinsic_params) - : _frame(std::make_shared()), _frame_num(std::make_shared(0)), - _frame_time(std::make_shared()), - _capture(std::make_shared(filename)), _name(name), - _description(description), _frame_lock(std::make_shared()), - _capture_lock(std::make_shared()), _intrinsic_params(intrinsic_params), - _running(std::make_shared(false)) { - init(extrinsic_params); -} - -Camera::Camera(int camera_id, string name, string description, CameraParams intrinsic_params, - Mat extrinsic_params) - : _frame(std::make_shared()), _frame_num(std::make_shared(0)), - _frame_time(std::make_shared()), - _capture(std::make_shared(camera_id)), _name(name), - _description(description), _frame_lock(std::make_shared()), - _capture_lock(std::make_shared()), _intrinsic_params(intrinsic_params), - _running(std::make_shared(false)) { - init(extrinsic_params); -} - -void Camera::init(const Mat& extrinsic_params) { - if (!extrinsic_params.empty() && extrinsic_params.size() != Size(4, 4)) { - throw std::invalid_argument("extrinsic_params must be 4x4 if given"); - } - extrinsic_params.copyTo(this->_extrinsic_params); +void Camera::init() { *(this->_running) = true; this->_thread = std::shared_ptr( new std::thread(&Camera::captureLoop, this), [this](std::thread* p) { @@ -75,37 +62,6 @@ void Camera::init(const Mat& extrinsic_params) { }); } -Camera::Camera(const Camera& other) - : _frame(other._frame), _frame_num(other._frame_num), _frame_time(other._frame_time), - _capture(other._capture), _name(other._name), _description(other._description), - _frame_lock(other._frame_lock), _capture_lock(other._capture_lock), - _thread(other._thread), _intrinsic_params(other._intrinsic_params), - _extrinsic_params(other._extrinsic_params), _running(other._running) {} - -bool Camera::openFromConfigFile(std::string filename) { - CameraConfig cfg = readConfigFromFile(filename); - - cv::Mat extrinsics; - if (cfg.extrinsicParams) { - extrinsics = cfg.extrinsicParams.value(); - } - - CameraParams intrinsics; - if (cfg.intrinsicParams) { - intrinsics = cfg.intrinsicParams.value(); - } - - if (std::holds_alternative(cfg.filenameOrID)) { - // return this->open(std::get(cfg.filenameOrID), intrinsics, extrinsics); - } else if (std::holds_alternative(cfg.filenameOrID)) { - return this->open(std::get(cfg.filenameOrID), intrinsics, extrinsics); - } else { - // this should never happen - throw invalid_camera_config("One of " + KEY_FILENAME + " or " + KEY_CAMERA_ID + - " must be present"); - } -} - std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id), cv::FileStorage::READ); if (!fs.isOpened()) { @@ -123,9 +79,14 @@ std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { const std::string KEY_HEIGHT = "height"; const std::string KEY_FRAMERATE = "framerate"; + _name = fs[KEY_NAME].operator std::string(); + _description = fs[KEY_DESCRIPTION].operator std::string(); + _width = fs[KEY_WIDTH].operator int(); + _height = fs[KEY_HEIGHT].operator int(); + gstr_ss << "image/" << fs[KEY_FORMAT].operator std::string(); - gstr_ss << ",width=" << fs[KEY_WIDTH].operator std::string(); - gstr_ss << ",height=" << fs[KEY_HEIGHT].operator std::string(); + gstr_ss << ",width=" << _width; + gstr_ss << ",height=" << _height; gstr_ss << ",framerate=" << fs[KEY_FRAMERATE].operator std::string() << "/1"; gstr_ss << " ! " << fs[KEY_FORMAT].operator std::string() << "dec ! videoconvert"; @@ -137,23 +98,25 @@ std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { void Camera::captureLoop() { loguru::set_thread_name(_name.c_str()); - cv::Size image_size(640, 480); - if (!_intrinsic_params.empty()) { - image_size = _intrinsic_params.getImageSize(); - } - _capture_lock->lock(); - _capture->set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); - _capture->set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); - _capture->set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height); - _capture_lock->unlock(); cv::Mat frame; while (*_running) { _capture_lock->lock(); bool success = _capture->read(frame); _capture_lock->unlock(); if (success && !frame.empty()) { + // Aruco detection here: + cv::Ptr dictionary = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + std::vector> markerCorners; + cv::Mat frameCopy; + std::vector markerIds; + cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds); + frame.copyTo(frameCopy); + if (!markerIds.empty()) { + cv::aruco::drawDetectedMarkers(frameCopy, markerCorners, markerIds); + } _frame_lock->lock(); - frame.copyTo(*(this->_frame)); + frameCopy.copyTo(*(this->_frame)); (*_frame_num)++; *_frame_time = dataclock::now(); _frame_lock->unlock(); @@ -196,22 +159,6 @@ bool Camera::next(cv::Mat& frame, uint32_t& frame_num, datatime_t& frame_time) c return true; } -bool Camera::hasIntrinsicParams() const { - return !(_intrinsic_params.empty()); -} - -bool Camera::hasExtrinsicParams() const { - return !(_extrinsic_params.empty()); -} - -CameraParams Camera::getIntrinsicParams() const { - return _intrinsic_params; -} - -cv::Mat Camera::getExtrinsicParams() const { - return _extrinsic_params.clone(); -} - std::string Camera::getName() const { return _name; } @@ -220,6 +167,14 @@ std::string Camera::getDescription() const { return _description; } +int Camera::getWidth() const { + return _width; +} + +int Camera::getHeight() const { + return _height; +} + void Camera::setName(std::string new_name) { this->_name = new_name; } diff --git a/src/camera/Camera.h b/src/camera/Camera.h index 80cfc6c33..c72c41ff7 100644 --- a/src/camera/Camera.h +++ b/src/camera/Camera.h @@ -1,7 +1,6 @@ #pragma once #include "../world_interface/data.h" -#include "CameraParams.h" #include #include @@ -68,14 +67,14 @@ class Camera { std::shared_ptr _capture; std::string _name; std::string _description; + int _width; + int _height; std::shared_ptr _frame_lock; std::shared_ptr _capture_lock; std::shared_ptr _thread; - CameraParams _intrinsic_params; - cv::Mat _extrinsic_params; std::shared_ptr _running; void captureLoop(); - void init(const cv::Mat& extrinsic_params); + void init(); public: /** @@ -87,60 +86,13 @@ class Camera { manually opened with the Camera::open() method. */ Camera(); - /** - @brief Opens a Camera that is not already open. - - Will open the given file for input and use the given intrinsic and - extrinsic parameters, similar to the constructor. - - @returns true if opening the camera succeeds, and false if not. - */ - // bool open(std::string filename, CameraParams intrinsic_params = CameraParams(), - // cv::Mat extrinsic_params = cv::Mat()); - /** - @brief Opens a Camera that is not already open. - - Will open the camera at the given camera ID and use the given intrinsic - and extrinsic parameters, similar to the constructor. - - @returns true if opening the camera succeeds, and false if not. - */ - bool open(int camera_id, CameraParams intrinsic_params = CameraParams(), - cv::Mat extrinsic_params = cv::Mat()); - /** - @brief Constructs a Camera that will open the given file for input and - have the given name and description. - - @param filename The file to open and read a video feed from. This may be - the name of a video file, or a URI of a video stream. This will be passed - to the underlying OpenCV VideoCapture object, so anything supported by - VideoCapture is supported here. - - @param name The name of the camera. This should ideally be unique, but - isn't enforced at this time. - - @param description An optional description of the camera. - */ - Camera(std::string filename, std::string name, std::string description = "", - CameraParams intrinsic_params = CameraParams(), - cv::Mat extrinsic_params = cv::Mat()); /** @brief Constructs a Camera that will open the camera with the given ID and have the given name and description. - @param filename The file to open and read a video feed from. This may be - the name of a video file, or a URI of a video stream. This will be passed - to the underlying OpenCV VideoCapture object, so anything supported by - VideoCapture is supported here. - - @param name The name of the camera. This should ideally be unique, but - isn't enforced at this time. - - @param description An optional description of the camera. + @param camera_id The ID of the Camera to open and read a video feed from. */ - Camera(int camera_id, std::string name, std::string description = "", - CameraParams intrinsic_params = CameraParams(), - cv::Mat extrinsic_params = cv::Mat()); + Camera(robot::types::CameraID camera_id); /** @brief Copy constructor. @@ -149,23 +101,22 @@ class Camera { objects sharing access are destroyed, the camera will be closed. */ Camera(const Camera& other); - /** - @brief Opens the camera using the given configuration file. - - @param filename The path to the configuration file to open and - read. Configuration file should be formatted as described in @ref - cameraconfig. + @brief Opens a Camera that is not already open. - @throws invalid_camera_config If the configuration is invalid for any - reason. + Will open the camera at the given camera ID, similar to the constructor. - @throws std::invalid_argument If the configuration file does not exist. + @returns true if opening the camera succeeds, and false if not. */ - bool openFromConfigFile(std::string filename); + bool open(robot::types::CameraID camera_id); + /** + @brief Get camera configs for the specified camera. - std::stringstream GStreamerFromFile(robot::types::CameraID camera_id); + @param camera_id The ID of the Camera to get the configs for. + @returns The Gstream with configs. + */ + std::stringstream GStreamerFromFile(robot::types::CameraID camera_id); /** @brief Returns true if the camera is open. */ @@ -206,22 +157,9 @@ class Camera { */ bool next(cv::Mat& frame, uint32_t& frame_num, robot::types::datatime_t& frame_time) const; /** - @brief Returns true if this camera has associated intrinsic parameters. - */ - bool hasIntrinsicParams() const; - /** - @brief Returns true if this camera has associated extrinsic parameters. - */ - bool hasExtrinsicParams() const; - /** - @brief Returns the associated intrinsic parameters as a CameraParams - object. - */ - CameraParams getIntrinsicParams() const; - /** - @brief Returns the associated extrinsic parameters as an OpenCV matrix. + @brief Returns the name of the camera. */ - cv::Mat getExtrinsicParams() const; + std::string getName() const; /** @brief Returns the description of the camera. @@ -230,9 +168,13 @@ class Camera { */ std::string getDescription() const; /** - @brief Returns the name of the camera. + @brief Returns the width of the camera feed. */ - std::string getName() const; + int getWidth() const; + /** + @brief Returns the height of the camera feed. + */ + int getHeight() const; /** @brief Updates the name of the camera to the given name. @param new_name The new name for the camera. diff --git a/src/camera/CameraConfig.cpp b/src/camera/CameraConfig.cpp deleted file mode 100644 index acb1b97e0..000000000 --- a/src/camera/CameraConfig.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include "CameraConfig.h" -#include -#include -namespace cam { - -invalid_camera_config::invalid_camera_config() : _msg("Invalid camera configuration") {} - -invalid_camera_config::invalid_camera_config(const std::string& msg) - : _msg("Invalid camera configuration: " + msg) {} - -const char* invalid_camera_config::what() const noexcept { - return _msg.c_str(); -} - -CameraConfig readConfigFromFile(const std::string& filename) { - cv::FileStorage fs(filename, cv::FileStorage::READ); - if (!fs.isOpened()) { - throw std::invalid_argument("Configuration file " + filename + " does not exist"); - } - - CameraConfig cfg{}; - - // read intrinsic parameters - if (!fs[KEY_INTRINSIC_PARAMS].empty()) { - CameraParams intrinsics; - std::vector intrinsic_list1d; - fs[KEY_INTRINSIC_PARAMS] >> intrinsics; - cfg.intrinsicParams = intrinsics; - } - - // read extrinsic parameters - if (!fs[KEY_EXTRINSIC_PARAMS].empty()) { - cv::Mat extrinsics; - fs[KEY_EXTRINSIC_PARAMS] >> extrinsics; - cfg.extrinsicParams = extrinsics; - } - - // read name - if (fs[KEY_NAME].empty()) { - throw invalid_camera_config(KEY_NAME + " must be present"); - } - cfg.name = static_cast(fs[KEY_NAME]); - - // read description - if (!fs[KEY_DESCRIPTION].empty()) { - cfg.description = static_cast(fs[KEY_DESCRIPTION]); - } - - // read filename or camera ID, and open camera. - if (!fs[KEY_FILENAME].empty()) { - cfg.filenameOrID = static_cast(fs[KEY_FILENAME]); - } else if (!fs[KEY_CAMERA_ID].empty()) { - cfg.filenameOrID = static_cast(fs[KEY_CAMERA_ID]); - } else { - throw invalid_camera_config("One of " + KEY_FILENAME + " or " + KEY_CAMERA_ID + - " must be present"); - } - - return cfg; -} - -} // namespace cam diff --git a/src/camera/CameraConfig.h b/src/camera/CameraConfig.h deleted file mode 100644 index cc8e68558..000000000 --- a/src/camera/CameraConfig.h +++ /dev/null @@ -1,129 +0,0 @@ -#pragma once - -#include "CameraParams.h" - -#include -#include -#include - -#include - -namespace cam { - -/** - @addtogroup camera - @{ - */ - -/** - @name Configuration File Keys - - The following are constants for the keys used in the camera configuration - files. See @ref cameraconfig for more details. -*/ -/**@{*/ -/** - Config file key for camera filename. - */ -const std::string KEY_FILENAME = "filename"; -/** - Config file key for camera id. - */ -const std::string KEY_CAMERA_ID = "camera_id"; -/** - Config file key for intrinsic parameters. - */ -const std::string KEY_INTRINSIC_PARAMS = "intrinsic_params"; -/** - Config file key for extrinsic parameters. - */ -const std::string KEY_EXTRINSIC_PARAMS = "extrinsic_params"; -/** - Config file key for calibration information. - */ -const std::string KEY_CALIB_INFO = "calib_info"; -/** - Config file key for camera name. - */ -const std::string KEY_NAME = "name"; -/** - Config file key for camera description. - */ -const std::string KEY_DESCRIPTION = "description"; -/**@}*/ - -/** -Exception for errors in the camera configuration. -*/ -class invalid_camera_config : public std::exception { -public: - /** - Constructs an invalid_camera_config exception with the default message "Invalid camera - configuration". - */ - invalid_camera_config(); - /** - Constructs an invalid_camera_config exception with the given message appended to - "Invalid camera configuration:". - @param msg The message to use for the exception. - */ - invalid_camera_config(const std::string& msg); - /** - Returns the exception message as a C string. - */ - virtual const char* what() const noexcept; - -private: - std::string _msg; -}; - -/** - * @brief A struct that represents the information outlined in @ref cameraconfig. - */ -struct CameraConfig { - /** - * @brief The name of the camera. - */ - std::string name; - - /** - * @brief If specified, gives the intrinsic parameter matrix. - */ - std::optional intrinsicParams; - - /** - * @brief If specified, gives the extrinsic parameter matrix. - */ - std::optional extrinsicParams; - - /** - * @brief Either the camera ID or the filename to stream from. - * - * The camera ID specifies the ID of the camera device to use. - * If the filename is specified, this camera will stream from a video file. - */ - std::variant filenameOrID; - - /** - * @brief If specified, gives the text description of the camera. - */ - std::optional description; -}; - -/** - * @brief Read the camera config from the specified file. - * - * @param filename The path to the configuration file to open and read. Configuration file - * should be formatted as described in @ref cameraconfig. - * - * @return CameraConfig The parsed camera config object. - * - * @throws invalid_camera_config If the configuration is invalid for any reason. - * - * @throws invalid_argument If the file does not exist. - */ -CameraConfig readConfigFromFile(const std::string& filename); - -/** @} */ - -} // namespace cam diff --git a/src/camera/CameraConfig.md b/src/camera/CameraConfig.md deleted file mode 100644 index 2dc01a1de..000000000 --- a/src/camera/CameraConfig.md +++ /dev/null @@ -1,115 +0,0 @@ -# Camera Configuration {#cameraconfig} - - - -Cameras are configured using the following configuration file format. The -specification given is in general terms, and the [example](@ref example) is -given in YAML; however, XML and JSON are also supported. Please see the OpenCV -documentation on [persistent file -storage](https://docs.opencv.org/4.2.0/d4/da4/group__core__xml.html) for more -details. - -### Top-Level Format {#toplevel} - -The top level of the configuration file should be a key-value mapping, with the -following top-level keys: - * `name`: The name of the camera, e.g. "mast" or "front_left". No restrictions - on naming or uniqueness are currently enforced; ideally this should be - unique but it isn't strictly necessary at this time. - * `description`: _Optional._ A longer description of the camera, including - details like its location on the rover. - * *One* of the following. **If both are present, `filename` will take precedence.** - * `filename`: The file path or URI to a video device or stream that should - be opened. - * `camera_id`: The ID of a video device that will be opened. Given ID N, the - device at `/dev/videoN` will be opened. - * `intrinsic_params`: _Optional._ Value should be a nested key-value mapping, - defining a set of intrinsic camera parameters as defined in the section - [Intrinsic Parameters](@ref intrinsic) below. - * `extrinsic_params`: _Optional._ Should be a 4x4 matrix defining a - transformation from 3-dimensional coordinates in the camera's - frame of reference to the rover's frame of reference. This matrix - should ideally have a 3x3 [rotation - matrix](https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions) - in the first three rows and columns, and the first three rows in - the fourth column should be a translation vector. The fourth row - should be `0, 0, 0, 1` so that [homogeneous - coordinates](https://en.wikipedia.org/wiki/Homogeneous_coordinates) - (4-dimensional coordinates where the 4th coordinate is `1`) can be - used. - - Note that in the rover's frame of reference, the positive - x-axis is towards the front, the positive y-axis is towards the - left, and the positive z-axis is towards the top; in the camera's - frame of reference, the positive x-axis is to the right (parallel - to the image plane), the positive y-axis is down, and the positive - z-axis is in the direction the camera is pointing. - * `calib_info`: _Optional._ Information from the calibration process, generated - by the calibration program. See [Calibration Info](@ref calibinfo) below. - -### Intrinsic Parameters {#intrinsic} - -Intrinsic parameters are represented as a key-value mapping, with the following -keys: - * `camera_matrix`: A 3x3 matrix defining the projection of a 3D scene onto the - 2D image plane of the camera. - * `distortion_coefficients`: A column vector (i.e. matrix with one column) of - 4, 5, 8, 12, or 14 distortion coefficients, which define the lens distortion - of the camera. - * `image_width`: The width of the image in pixels. This (along with - `image_height`) will define the resolution the camera is set to, as - intrinsic parameters are only valid for the resolution at which they are - calibrated. - * `image_height`: The height of the image in pixels. - -### Calibration Info {#calibinfo} - -Calibration information is saved by the calibration program, and provides some -data relevant to the calibration process, such as the time and date of -calibration, the size of the board used, and the reprojection error. You should -not need to fill this section of the configuration file out yourself. - -Probably the most important part of this section if it is present is -`avg_reprojection_error`. The calibration process computes a projection matrix -for the camera; multiplying the coordinates of a point in space as a column -vector by this matrix will give the location they should appear in the -image. The reprojection error is the distance between the computed projection -and the actual location of the point in the image, and is measured in pixels; -the lower this number, the better. A general rule of thumb is that you should -try to aim for less than 1 pixel. - -### Example {#example} - -```yaml -%YAML:1.0 ---- -name: "example_camera" -description: "An example camera, not actually located on the rover." -# Camera ID: given ID N, will attempt to use /dev/videoN. -camera_id: 0 -# Information about the calibration process, such as the -# reprojection error and board size. -calib_info: - calibration_time: "Sat 08 May 2021 12:19:35 PM PDT" - board_width: 10 - board_height: 7 - square_size: 21. - avg_reprojection_error: 1.8641131183947526e-01 - flags: 0 -# The actual intrinsic parameters of the camera. -intrinsic_params: - camera_matrix: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [ 6.4859611612740241e+02, 0., 3.2923686348750090e+02, 0., - 6.5023092591714169e+02, 2.4355655685247874e+02, 0., 0., 1. ] - distortion_coefficients: !!opencv-matrix - rows: 5 - cols: 1 - dt: d - data: [ -4.1887226549228418e-01, 1.6973679483281634e-01, - 8.8881372228211163e-04, 9.6518575595955756e-04, 0. ] - image_width: 640 - image_height: 480 -``` diff --git a/src/camera/CameraParams.cpp b/src/camera/CameraParams.cpp deleted file mode 100644 index 5cb419732..000000000 --- a/src/camera/CameraParams.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include "CameraParams.h" - -#include "../../src/Constants.h" -#include "../../src/camera/CameraConfig.h" - -#include - -#include - -namespace cam { - -////////////// CONSTRUCTORS ////////////// - -CameraParams::CameraParams() {} - -void CameraParams::init(const cv::Mat& camera_matrix, const cv::Mat& dist_coeff, - cv::Size image_size) { - if (camera_matrix.size() != cv::Size(3, 3)) { - throw std::invalid_argument("Camera matrix must be 3x3"); - } - int n_coeffs = dist_coeff.rows * dist_coeff.cols; - if (!(n_coeffs == 4 || n_coeffs == 5 || n_coeffs == 8 || n_coeffs == 12 || - n_coeffs == 14)) { - throw std::invalid_argument( - "Number of distortion coefficients must be 4, 5, 8, 12, or 14"); - } - if (image_size.empty()) { - throw std::invalid_argument("Image size must not be empty"); - } - - dist_coeff.reshape(1, {n_coeffs, 1}).copyTo(this->_dist_coeff); - camera_matrix.copyTo(this->_camera_matrix); - this->_image_size = image_size; -} - -CameraParams::CameraParams(const cv::Mat& camera_matrix, const cv::Mat& dist_coeff, - cv::Size image_size) { - init(camera_matrix, dist_coeff, image_size); -} - -CameraParams::CameraParams(const CameraParams& other) { - cv::Mat newCam, newDist; - other._camera_matrix.copyTo(newCam); - other._dist_coeff.copyTo(newDist); - this->_camera_matrix = newCam; - this->_dist_coeff = newDist; - - this->_image_size = other._image_size; -} - -bool CameraParams::empty() const { - return _camera_matrix.empty() || _dist_coeff.empty() || _image_size.empty(); -} - -////////////// ACCESSORS ///////////////// - -cv::Mat CameraParams::getCameraMatrix() const { - return _camera_matrix; -} - -cv::Mat CameraParams::getDistCoeff() const { - return _dist_coeff; -} - -cv::Size CameraParams::getImageSize() const { - return _image_size; -} - -std::vector CameraParams::getIntrinsicList() { - std::vector intrinsic_list1D; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - double x = _camera_matrix.at(i, j); - intrinsic_list1D.push_back(x); - } - } - return intrinsic_list1D; -} - -CameraParams& CameraParams::operator=(const CameraParams& other) { - if (this != &other) { - // Copy members from 'other' to 'this' - this->_camera_matrix = other._camera_matrix.clone(); - this->_dist_coeff = other._dist_coeff.clone(); - this->_image_size = other._image_size; - } - return *this; -} -////////////// SERIALIZATION ////////////// - -void CameraParams::readFromFileNode(const cv::FileNode& file_node) { - cv::Mat cam, dist; - file_node[KEY_CAMERA_MATRIX] >> cam; - file_node[KEY_DIST_COEFFS] >> dist; - int w, h; - w = (int)file_node[KEY_IMAGE_WIDTH]; - h = (int)file_node[KEY_IMAGE_HEIGHT]; - cv::Size size(w, h); - // call init to do validation - init(cam, dist, size); -} - -void CameraParams::writeToFileStorage(cv::FileStorage& file_storage) const { - file_storage << "{"; - file_storage << KEY_IMAGE_WIDTH << _image_size.width; - file_storage << KEY_IMAGE_HEIGHT << _image_size.height; - file_storage << KEY_CAMERA_MATRIX << _camera_matrix; - file_storage << KEY_DIST_COEFFS << _dist_coeff; - file_storage << "}"; -} - -void read(const cv::FileNode& node, cam::CameraParams& params, - const cam::CameraParams& default_value) { - if (node.empty()) { - params = default_value; - } else { - params.readFromFileNode(node); - } -} - -void write(cv::FileStorage& fs, [[maybe_unused]] const std::string& name, - const cam::CameraParams& params) { - params.writeToFileStorage(fs); -} - -} // namespace cam diff --git a/src/camera/CameraParams.h b/src/camera/CameraParams.h deleted file mode 100644 index 73b6dedeb..000000000 --- a/src/camera/CameraParams.h +++ /dev/null @@ -1,159 +0,0 @@ -#pragma once - -#include - -#include - -namespace cam { - -/** - @addtogroup camera - @{ - */ - -/** - @name Configuration File Keys - - The following are constants for the keys used in the camera configuration files. See @ref - cameraconfig for more details. - */ -/**@{*/ -/** - Config file key for image width. - */ -const std::string KEY_IMAGE_WIDTH = "image_width"; -/** - Config file key for image height. - */ -const std::string KEY_IMAGE_HEIGHT = "image_height"; -/** - Config file key for the camera matrix. - */ -const std::string KEY_CAMERA_MATRIX = "camera_matrix"; -/** - Config file key for the distortion coefficients. - */ -const std::string KEY_DIST_COEFFS = "distortion_coefficients"; - -/**@}*/ - -/** - @brief Represents a set of intrinsic camera parameters. - - Camera parameters (also called "intrinsic parameters") are obtained from - camera calibration and are unique to and constant for every camera. They - define how the camera projects a 3D space onto a 2D image plane. - - Each set of CameraParams contains a camera matrix which defines how points in - 3D space are linearly projected to the 2D image plane of the camera, and a - set of distortion coefficients which can be used to correct for radial - distortion in the image. The projection is linear which means that projected - points are unique only up to scaling. - - @warning Note that camera parameters are different for different resolutions - of the camera! It is often not enough to simply scale the image centers/focal - lengths for different resolutions because many cameras have different fields - of view (and therefore different projections/distortion) at different - resolutions. For this reason, a CameraParams object has an associated - cv::Size which contains the image size the parameters are valid for. -*/ -class CameraParams { -private: - cv::Mat _camera_matrix; - cv::Mat _dist_coeff; - cv::Size _image_size; - void init(const cv::Mat& camera_matrix, const cv::Mat& dist_coeff, cv::Size image_size); - -public: - /** - Constructs a default or empty set of camera parameters. - - @warning Empty sets of camera parameters are not suitable for actual use! Check to make - sure they are non-empty with empty(). - */ - CameraParams(); - /** - Constructs a set of camera parameters with the given camera matrix, distortion - coefficients, and image size. - - @param camera_matrix A 3x3 matrix defining the projection of the camera. - - @param dist_coeff A set of distortion coefficients defining the lens distortion of the - camera. Must be a 1xN or Nx1 matrix where N is the number of coefficients. N must be 4, - 5, 8, 12, or 14. - - @param image_size The size of the image the parameters are calibrated for. Defaults to - 640x480 as this is the default resolution of many webcams. - */ - CameraParams(const cv::Mat& camera_matrix, const cv::Mat& dist_coeff, - cv::Size image_size = cv::Size(640, 480)); - /** - @brief Copy constructor. - - Constructs a set of camera parameters by copying another. Underlying data like the - camera matrix and distortion coefficients are actually copied, so the copy may be - modified without affecting the original. - */ - CameraParams(const CameraParams& other); - bool empty() const; - /** - Gets the camera matrix associated with this set of camera parameters. - */ - cv::Mat getCameraMatrix() const; - /** - Gets the distortion coefficients (as a column vector) associated with - this set of camera parameters. - */ - cv::Mat getDistCoeff() const; - /** - Gets the image size this set of intrinsic parameters was calibrated for. - */ - cv::Size getImageSize() const; - /** - @brief Gets the camera intrinsics as a 1d list. - */ - std::vector getIntrinsicList(); - - /** - @brief Define a copy assignment operator - */ - CameraParams& operator=(const CameraParams& other); - - /** - @brief Reads the data for this CameraParams object from the given cv::FileNode object. - - Used for serialization - you should not need to call this method directly but should - instead use the >> operator on a cv::FileNode object. - */ - void readFromFileNode(const cv::FileNode& file_node); - /** - @brief Writes the data for this CameraParams object to the given cv::FileStorage - object. - - Used for serialization - you should not need to call this method directly but should - instead use the << operator on a cv::FileStorage object. - */ - void writeToFileStorage(cv::FileStorage& file_storage) const; -}; - -/** - @brief Reads a CameraParams object from the given cv::FileNode object. - - Used for serialization - you should not need to call this method directly but should - instead use the >> operator on a cv::FileNode object. -*/ -void read(const cv::FileNode& node, CameraParams& params, - const CameraParams& default_value = CameraParams()); -/** - @brief Writes the given CameraParams object to the given cv::FileStorage object. - - Used for serialization - you should not need to call this method directly but should - instead use the << operator on a cv::FileStorage object. -*/ -void write(cv::FileStorage& fs, const std::string& name, const CameraParams& params); - -/** @} */ - -} // namespace cam - -// namespace cam From e3f55d2723d7e3023d20d387b3e1386f2ccc1b92 Mon Sep 17 00:00:00 2001 From: Isaac Date: Sat, 26 Oct 2024 13:14:36 -0700 Subject: [PATCH 03/10] Disabled ar/, update interface camera code --- src/CMakeLists.txt | 11 +++--- src/camera/Camera.cpp | 2 +- src/world_interface/real_world_interface.cpp | 11 +++--- src/world_interface/simulator_interface.cpp | 41 ++------------------ src/world_interface/world_interface.h | 21 ---------- 5 files changed, 17 insertions(+), 69 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 03cbfaeb9..43ce8e471 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -67,6 +67,7 @@ message("========================================") # here to avoid the compiler spamming warnings at us for stuff in the Eigen headers. find_package(Eigen3 REQUIRED NO_MODULE) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) +include_directories("/usr/local/include") # Find the WebSocket++ library and Boost (provides the ASIO backend for Websocket++ and also # provides the program_options argument parser). Only the `system` component of Boost is @@ -197,11 +198,11 @@ endif() # hardware-agnostic utilities and common code for world interface add_library(world_interface_core STATIC world_interface/gps_common_interface.cpp - ar/Detector.cpp - ar/MarkerSet.cpp - ar/MarkerPattern.cpp - ar/Tag.cpp - ar/read_landmarks.cpp + #ar/Detector.cpp + #ar/MarkerSet.cpp + #ar/MarkerPattern.cpp + #ar/Tag.cpp + #ar/read_landmarks.cpp world_interface/motor/base_motor.cpp ) target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index e7f33144b..21c2a550d 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -26,7 +26,7 @@ Camera::Camera(robot::types::CameraID camera_id) _frame_lock(std::make_shared()), _capture_lock(std::make_shared()), _running(std::make_shared(false)) { - open(camera_id); + this->open(camera_id); } Camera::Camera(const Camera& other) diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index 5bcf2fc32..bc4932e6f 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -96,10 +96,10 @@ void initMotors() { } } -void openCamera(CameraID camID, const char* cameraPath) { +void openCamera(CameraID camID) { try { auto cam = std::make_shared(); - bool success = cam->openFromConfigFile(cameraPath); + bool success = cam->open(camID) if (success) { cameraMap[camID] = cam; } else { @@ -112,10 +112,11 @@ void openCamera(CameraID camID, const char* cameraPath) { } void setupCameras() { - openCamera(Constants::MAST_CAMERA_ID, Constants::MAST_CAMERA_CONFIG_PATH); - openCamera(Constants::FOREARM_CAMERA_ID, Constants::FOREARM_CAMERA_CONFIG_PATH); - openCamera(Constants::HAND_CAMERA_ID, Constants::HAND_CAMERA_CONFIG_PATH); + openCamera(Constants::MAST_CAMERA_ID); + openCamera(Constants::FOREARM_CAMERA_ID); + openCamera(Constants::HAND_CAMERA_ID); } + } // namespace void world_interface_init( diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 727319d15..5691dcc03 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -1,7 +1,6 @@ #include "../Constants.h" #include "../base64/base64_img.h" #include "../camera/Camera.h" -#include "../camera/CameraConfig.h" #include "../kinematics/DiffDriveKinematics.h" #include "../navtypes.h" #include "../network/websocket/WebSocketProtocol.h" @@ -79,8 +78,6 @@ std::unordered_map> cameraFrameMap; // but this one is guaranteed to have indices, if there are any std::unordered_map cameraLastFrameIdxMap; std::shared_mutex cameraFrameMapMutex; // protects both of the above maps -// not modified after startup, no need to synchronize -std::unordered_map cameraConfigMap; std::condition_variable connectionCV; std::mutex connectionMutex; @@ -91,7 +88,7 @@ void sendJSON(const json& obj) { } static void openCamera(CameraID cam, std::optional> list1d = std::nullopt, - uint8_t fps = 20, uint16_t width = 640, uint16_t height = 480) { + uint8_t fps = 30, uint16_t width = 640, uint16_t height = 480) { if (list1d) { json msg = {{"type", "simCameraStreamOpenRequest"}, {"camera", cam}, @@ -112,11 +109,9 @@ static void openCamera(CameraID cam, std::optional> list1d = } void initCameras() { - // auto cfg = cam::readConfigFromFile(Constants::MAST_CAMERA_CONFIG_PATH); - // cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg; - // openCamera(Constants::HAND_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); - // openCamera(Constants::FOREARM_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); - // openCamera(Constants::MAST_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList()); + openCamera(Constants::HAND_CAMERA_ID); + openCamera(Constants::WRIST_CAMERA_ID); + openCamera(Constants::MAST_CAMERA_ID); } void initMotors() { @@ -317,34 +312,6 @@ DataPoint readCamera(CameraID cameraID) { } } -std::optional getCameraIntrinsicParams(CameraID cameraID) { - auto entry = cameraConfigMap.find(cameraID); - if (entry != cameraConfigMap.end()) { - auto cfg = entry->second; - if (cfg.intrinsicParams) { - return cfg.intrinsicParams; - } else { - return {}; - } - } else { - return {}; - } -} - -std::optional getCameraExtrinsicParams(CameraID cameraID) { - auto entry = cameraConfigMap.find(cameraID); - if (entry != cameraConfigMap.end()) { - auto cfg = entry->second; - if (cfg.extrinsicParams) { - return cfg.extrinsicParams; - } else { - return {}; - } - } else { - return {}; - } -} - landmarks_t readLandmarks() { return {}; } diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index e90448028..de108a290 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -12,11 +12,6 @@ #include using namespace kinematics; -// forward declare cam::CameraParams instead of including it -// we do this to avoid unnecessarily including OpenCV in all build targets -namespace cam { -class CameraParams; -} /** * @namespace robot @@ -107,22 +102,6 @@ bool hasNewCameraFrame(types::CameraID camera, uint32_t oldFrameNum); */ types::DataPoint readCamera(types::CameraID camera); -/** - * @brief Get the intrinsic params of the specified camera, if it exists. - * - * @param camera The ID of the camera for which to get the intrinsic params. - * @returns The intrinsic params if it exists, else an empty optional object. - */ -std::optional getCameraIntrinsicParams(types::CameraID camera); - -/** - * @brief Get the extrinsic params of the specified camera, if it exists. - * - * @param camera The ID of the camera for which to get the extrinsic params. - * @returns The extrinsic params if it exists, else an empty optional object. - */ -std::optional getCameraExtrinsicParams(types::CameraID camera); - /** * @brief Read measurement from the CV system. As of now, returns a vector of fixed length, one * for each post in the competition. From b0b6c28da97e34787db0edf9ad9542eea40094be Mon Sep 17 00:00:00 2001 From: imisaacwu Date: Sat, 26 Oct 2024 20:54:23 -0700 Subject: [PATCH 04/10] Camera IDs, Simulator fixes --- src/network/MissionControlProtocol.cpp | 8 +++--- src/network/MissionControlTasks.cpp | 2 +- src/world_interface/real_world_interface.cpp | 28 ++---------------- src/world_interface/simulator_interface.cpp | 30 ++++++-------------- 4 files changed, 16 insertions(+), 52 deletions(-) diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index cec6d2646..f20919f67 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -299,11 +299,11 @@ static void handleJointPositionRequest([[maybe_unused]] const json& j) { } static bool validateCameraStreamOpenRequest(const json& j) { - return util::validateKey(j, "camera", val_t::string); + return util::validateKey(j, "cameraID", val_t::number_unsigned); } void MissionControlProtocol::handleCameraStreamOpenRequest(const json& j) { - CameraID cam = j["camera"]; + CameraID cam = j["cameraID"]; std::unordered_set supported_cams = robot::getCameras(); if (supported_cams.find(cam) != supported_cams.end()) { _camera_stream_task.openStream(cam, j["fps"]); @@ -311,11 +311,11 @@ void MissionControlProtocol::handleCameraStreamOpenRequest(const json& j) { } static bool validateCameraStreamCloseRequest(const json& j) { - return util::validateKey(j, "camera", val_t::string); + return util::validateKey(j, "cameraID", val_t::number_unsigned); } void MissionControlProtocol::handleCameraStreamCloseRequest(const json& j) { - CameraID cam = j["camera"]; + CameraID cam = j["cameraID"]; _camera_stream_task.closeStream(cam); } diff --git a/src/network/MissionControlTasks.cpp b/src/network/MissionControlTasks.cpp index 7b4cf8742..4eecc7071 100644 --- a/src/network/MissionControlTasks.cpp +++ b/src/network/MissionControlTasks.cpp @@ -164,7 +164,7 @@ void CameraStreamTask::task(std::unique_lock&) { // convert frame to encoded data and send it auto data_vector = encoder->encode_frame(frame); json msg = {{"type", CAMERA_STREAM_REP_TYPE}, - {"camera", cam}, + {"cameraID", cam}, {"data", data_vector}}; _server.sendJSON(Constants::MC_PROTOCOL_NAME, msg); } diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index bc4932e6f..278a65ace 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -99,7 +99,7 @@ void initMotors() { void openCamera(CameraID camID) { try { auto cam = std::make_shared(); - bool success = cam->open(camID) + bool success = cam->open(camID); if (success) { cameraMap[camID] = cam; } else { @@ -113,7 +113,7 @@ void openCamera(CameraID camID) { void setupCameras() { openCamera(Constants::MAST_CAMERA_ID); - openCamera(Constants::FOREARM_CAMERA_ID); + openCamera(Constants::WRIST_CAMERA_ID); openCamera(Constants::HAND_CAMERA_ID); } @@ -186,30 +186,6 @@ DataPoint readCamera(CameraID cameraID) { } } -std::optional getCameraIntrinsicParams(CameraID cameraID) { - auto itr = cameraMap.find(cameraID); - if (itr != cameraMap.end()) { - auto camera = itr->second; - return camera->hasIntrinsicParams() ? camera->getIntrinsicParams() - : std::optional{}; - } else { - LOG_F(WARNING, "Invalid camera id: %s", util::to_string(cameraID).c_str()); - return {}; - } -} - -std::optional getCameraExtrinsicParams(CameraID cameraID) { - auto itr = cameraMap.find(cameraID); - if (itr != cameraMap.end()) { - auto camera = itr->second; - return camera->hasExtrinsicParams() ? camera->getExtrinsicParams() - : std::optional{}; - } else { - LOG_F(WARNING, "Invalid camera id: %s", util::to_string(cameraID).c_str()); - return {}; - } -} - // Distance between left and right wheels. constexpr double WHEEL_BASE = 0.66; // Effective distance between wheels. Tweaked so that actual rover angular rate diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 5691dcc03..7e0df2cc0 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -87,25 +87,14 @@ void sendJSON(const json& obj) { wsServer->get().sendJSON(PROTOCOL_PATH, obj); } -static void openCamera(CameraID cam, std::optional> list1d = std::nullopt, - uint8_t fps = 30, uint16_t width = 640, uint16_t height = 480) { - if (list1d) { - json msg = {{"type", "simCameraStreamOpenRequest"}, - {"camera", cam}, - {"fps", fps}, - {"width", width}, - {"height", height}, - {"intrinsics", list1d.value()}}; - sendJSON(msg); - } else { - json msg = {{"type", "simCameraStreamOpenRequest"}, - {"camera", cam}, - {"fps", fps}, - {"width", width}, - {"height", height}, - {"intrinsics", nullptr}}; - sendJSON(msg); - } +static void openCamera(CameraID cam, uint8_t fps = 30, uint16_t width = 640, uint16_t height = 480) { + json msg = {{"type", "simCameraStreamOpenRequest"}, + {"cameraID", cam}, + {"fps", fps}, + {"width", width}, + {"height", height}}; + sendJSON(msg); + } void initCameras() { @@ -142,8 +131,7 @@ void handleIMU(json msg) { } void handleCamFrame(json msg) { - std::string cam = msg["camera"]; - robot::types::CameraID id = Constants::CAMERA_NAME_TO_ID.at(cam); + robot::types::CameraID id = msg["cameraID"]; std::string b64 = msg["data"]; cv::Mat mat = base64::decodeMat(b64); From 03581e5e06bc6d5fb5adba0ca2998852f159fa7a Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 17:42:13 -0800 Subject: [PATCH 05/10] Remove all ar/ --- src/CMakeLists.txt | 1 - src/ar/ARTester.cpp | 249 -------------------------------------- src/ar/CMakeLists.txt | 12 -- src/ar/Detector.cpp | 93 -------------- src/ar/Detector.h | 42 ------- src/ar/DetectorTests.cpp | 9 -- src/ar/MarkerPattern.cpp | 53 -------- src/ar/MarkerPattern.h | 72 ----------- src/ar/MarkerSet.cpp | 157 ------------------------ src/ar/MarkerSet.h | 194 ----------------------------- src/ar/MarkerSetTests.cpp | 107 ---------------- src/ar/Tag.cpp | 25 ---- src/ar/Tag.h | 65 ---------- src/ar/read_landmarks.cpp | 138 --------------------- src/ar/read_landmarks.h | 9 -- 15 files changed, 1226 deletions(-) delete mode 100644 src/ar/ARTester.cpp delete mode 100644 src/ar/CMakeLists.txt delete mode 100644 src/ar/Detector.cpp delete mode 100644 src/ar/Detector.h delete mode 100644 src/ar/DetectorTests.cpp delete mode 100644 src/ar/MarkerPattern.cpp delete mode 100644 src/ar/MarkerPattern.h delete mode 100644 src/ar/MarkerSet.cpp delete mode 100644 src/ar/MarkerSet.h delete mode 100644 src/ar/MarkerSetTests.cpp delete mode 100644 src/ar/Tag.cpp delete mode 100644 src/ar/Tag.h delete mode 100644 src/ar/read_landmarks.cpp delete mode 100644 src/ar/read_landmarks.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 43ce8e471..7309455c1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -367,7 +367,6 @@ add_compile_options( -Werror ) -add_subdirectory(ar) add_subdirectory(camera) add_subdirectory(CAN) add_subdirectory(network) diff --git a/src/ar/ARTester.cpp b/src/ar/ARTester.cpp deleted file mode 100644 index bd1dfbb2f..000000000 --- a/src/ar/ARTester.cpp +++ /dev/null @@ -1,249 +0,0 @@ -#include "../camera/Camera.h" -#include "Detector.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -constexpr int NUM_SKIP = 5; - -const std::string WINDOW_NAME = "Image"; - -const std::string keys = - "{h help | | Show this help message.}" - "{cam_config c | | The path to a camera configuration file " - "defining the camera you would like to use.}" - "{file_override fo | | If present, should be the name of a video file to open " - "rather than the one defined in the configuration file.}" - "{cam_override co | -1 | If present, should be the ID of a camera to open " - "rather than the one defined in the configuration file. NOTE: Will take precedence over " - "file_override if both are present.}" - "{marker_set m | urc | The set of markers to look for. Currently " - "only \"urc\" and \"circ\" are supported.}" - "{frame_by_frame f | | If present, program will go frame-by-frame " - "instead of capturing continuously.}"; - -// TODO: come up with a better solution for examining video files -// cam::Camera cap; -cv::VideoCapture cap; -std::shared_ptr MARKER_SET; - -std::vector projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec); -std::vector projectGrid(cv::Size imageSize, int spacing); -static inline void noValue(std::string option); - -int main(int argc, char* argv[]) { - cv::CommandLineParser parser(argc, argv, keys); - if (!parser.check()) { - parser.printErrors(); - } - parser.about("Program to open a camera and look for AR tags."); - - // print help message if "-h" or "--help" option is passed - if (parser.has("h")) { - parser.printMessage(); - return EXIT_SUCCESS; - } - - if (!parser.has("co")) { - std:cerr << "Error: camera ID required." << std::endl; - parser.printMessage(); - return EXIT_FAILURE; - } - - std::string marker_set = parser.get("m"); - std::transform(marker_set.begin(), marker_set.end(), marker_set.begin(), ::tolower); - - if (marker_set == "circ") { - MARKER_SET = AR::Markers::CIRC_MARKERS(); - } else if (marker_set == "urc") { - MARKER_SET = AR::Markers::URC_MARKERS(); - } else if (marker_set.empty()) { - noValue("marker_set"); - } else { - std::cerr << "Unsupported marker set: \"" << marker_set << "\"" << std::endl; - parser.printMessage(); - return EXIT_FAILURE; - } - - int cam_id = parser.get("co", -1); - - cv::Mat frame; - [[maybe_unused]] uint32_t fnum = 0; - - std::cout << "Opening camera..." << std::endl; - bool open_success = false; - if (cam_id > -1) { - open_success = cap.open(cam_id); - } else { - std::cerr << "Error: invalid camera ID" << std::endl; - return EXIT_FAILURE; - } - - if (!open_success) { - std::cerr << "ERROR! Unable to open camera" << std::endl; - return EXIT_FAILURE; - } - - const int w = cap.getWidth(); - const int h = cap.getHeight(); - cap.set(cv::CAP_PROP_FRAME_WIDTH, w); - cap.set(cv::CAP_PROP_FRAME_HEIGHT, h); - std::cout << "Set image dimensions to " << w << " x " << h << std::endl; - - std::cout << "Opening image window, press Q to quit" << std::endl; - - cv::namedWindow(WINDOW_NAME); - - AR::Detector detector(MARKER_SET, PARAMS); - - int count = 0; - bool frame_by_frame = parser.has("f"); - - bool show_grid = false; - int grid_spacing = 20; - bool show_rejected = false; - - bool loop = true; - cv::Size imageSize = PARAMS.getImageSize(); - - while (loop) { - // Grabs frame - if (!cap.grab()) { - continue; - } - cap.retrieve(frame); - if (frame.empty()) { - std::cerr << "ERROR! Blank frame grabbed" << std::endl; - continue; - } - - // Passes frame to the detector class. - // Tags will be located and returned. - std::vector> rejected; - std::vector tags = detector.detectTags(frame, rejected, false); - - // Draws an outline around the tag and a cross in the center - // Projects a cube onto the tag to debug TVec and RVec - for (AR::Tag tag : tags) { - std::cout << "Tag ID: " << tag.getMarker().getId() << std::endl; - std::vector cubePoints = - projectCube(MARKER_SET->getPhysicalSize(), tag.getRVec(), tag.getTVec()); - std::cout << "rvec: " << tag.getRVec() << std::endl; - cv::Vec3d tvec = tag.getTVec(); - double dist = sqrt(pow(tvec[0], 2) + pow(tvec[1], 2) + pow(tvec[2], 2)); - std::cout << "tvec: " << tvec << "(distance: " << dist << ")" << std::endl; - std::cout << "coordinates: " << tag.getCoordinates() << std::endl; - - for (size_t i = 0; i < 4; i++) { - size_t next = (i == 3 ? 0 : i + 1); - cv::line(frame, cubePoints[i], cubePoints[next], cv::Scalar(0, 0, 255), 3); - cv::line(frame, cubePoints[i], cubePoints[i + 4], cv::Scalar(0, 255, 0), 3); - cv::line(frame, cubePoints[i + 4], cubePoints[next + 4], cv::Scalar(255, 0, 0), - 3); - } - } - - if (show_rejected) { - for (const auto& points : rejected) { - for (size_t i = 0; i < points.size() - 1; i++) { - const auto& p1 = points[i]; - const auto& p2 = points[i + 1]; - cv::line(frame, p1, p2, cv::Scalar(255, 0, 255)); - } - } - } - - if (show_grid) { - std::vector grid = projectGrid(imageSize, grid_spacing); - for (cv::Point2f pt : grid) { - cv::Point2f newPt(pt.x * (float)imageSize.width, - pt.y * (float)imageSize.height); - cv::drawMarker(frame, newPt, cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 10, 1); - } - } - - cv::imshow(WINDOW_NAME, frame); - - int delay = (frame_by_frame && count % NUM_SKIP == 0) ? 0 : 1; - switch (cv::waitKey(delay)) { - case 'q': - loop = false; - break; - case 'g': - show_grid = true; - std::cout << "Grid on" << std::endl; - break; - case 'h': - show_grid = false; - std::cout << "Grid off" << std::endl; - break; - case 'r': - show_rejected = true; - std::cout << "Rejected points on" << std::endl; - break; - case 'l': - show_rejected = false; - std::cout << "Rejected points off" << std::endl; - default: - break; - } - count++; - } - - return 0; -} - -std::vector projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec) { - std::vector object_points; - std::vector image_points; - - object_points.push_back(cv::Point3d(-(len / 2), (len / 2), 0)); - object_points.push_back(cv::Point3d((len / 2), (len / 2), 0)); - object_points.push_back(cv::Point3d((len / 2), -(len / 2), 0)); - object_points.push_back(cv::Point3d(-(len / 2), -(len / 2), 0)); - object_points.push_back(cv::Point3d(-(len / 2), (len / 2), len)); - object_points.push_back(cv::Point3d((len / 2), (len / 2), len)); - object_points.push_back(cv::Point3d((len / 2), -(len / 2), len)); - object_points.push_back(cv::Point3d(-(len / 2), -(len / 2), len)); - cv::projectPoints(object_points, rvec, tvec, PARAMS.getCameraMatrix(), - PARAMS.getDistCoeff(), image_points); - - return image_points; -} - -std::vector projectGrid(cv::Size imageSize, int spacing) { - cv::Point2f center((float)imageSize.width / 2.0f, (float)imageSize.height / 2.0f); - std::vector grid_points; - std::vector projected_points; - float xf, yf; - for (int x = 0; x < imageSize.width / 2; x += spacing) { - for (int y = 0; y < imageSize.height / 2; y += spacing) { - xf = (float)x; - yf = (float)y; - grid_points.push_back(cv::Point2f(xf, yf) + center); - if (x != 0 || y != 0) { - grid_points.push_back(cv::Point2f(-xf, -yf) + center); - grid_points.push_back(cv::Point2f(-xf, yf) + center); - grid_points.push_back(cv::Point2f(xf, -yf) + center); - } - } - } - cv::undistortPoints(grid_points, projected_points, PARAMS.getCameraMatrix(), - PARAMS.getDistCoeff()); - return projected_points; -} - -static inline void noValue(std::string option) { - std::cerr << "Error: No value given for " << option << " option!" << std::endl - << "Please remember to use '=' between flags and values." << std::endl; - exit(EXIT_FAILURE); -} diff --git a/src/ar/CMakeLists.txt b/src/ar/CMakeLists.txt deleted file mode 100644 index 615071c3a..000000000 --- a/src/ar/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -find_package(OpenCV REQUIRED) -find_package(Threads REQUIRED) - -add_executable(ar_tester - ARTester.cpp - ../camera/Camera.cpp - Detector.cpp - MarkerSet.cpp - MarkerPattern.cpp - Tag.cpp) - -target_link_libraries(ar_tester ${OpenCV_LIBS} opencv_aruco Threads::Threads utils) diff --git a/src/ar/Detector.cpp b/src/ar/Detector.cpp deleted file mode 100644 index 52c2c0590..000000000 --- a/src/ar/Detector.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "Detector.h" - -#include "Tag.h" - -#include -#include -#include - -#include -#include -#include -#include - -namespace AR { - -Detector::Detector() { -} - -Detector::Detector(std::shared_ptr marker_set, - cv::Ptr detector_params) - : marker_set_(marker_set), - detector_params_(detector_params) { - if (!this->empty()) { - this->detector_params_->markerBorderBits = marker_set->getBorderSize(); - this->detector_params_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; - cv::initUndistortRectifyMap(camera_params_.getCameraMatrix(), - camera_params_.getDistCoeff(), cv::Mat_::eye(3, 3), - camera_params_.getCameraMatrix(), - camera_params_.getImageSize(), CV_16SC2, map1_, map2_); - } -} - -bool Detector::empty() const { - return (marker_set_ == nullptr || camera_params_.empty() || detector_params_ == nullptr); -} - -std::vector Detector::detectTags(const cv::Mat& input, bool undistort) { - return _detectTagsImpl(input, nullptr, undistort); -} - -std::vector Detector::detectTags(const cv::Mat& input, - std::vector>& rejectedPoints, - bool undistort) { - return _detectTagsImpl(input, &rejectedPoints, undistort); -} - -std::vector Detector::_detectTagsImpl(const cv::Mat& input, - std::vector>* rejected, - bool undistort) { - if (this->empty()) { - return {}; - } - std::vector> corners; - std::vector ids; - - cv::Mat undistorted; - if (undistort) { - cv::remap(input, undistorted, this->map1_, this->map2_, cv::INTER_LINEAR); - } else { - undistorted = input; - } - - cv::aruco::detectMarkers(undistorted, this->marker_set_->getDict(), corners, ids, - this->detector_params_, - (rejected == nullptr ? cv::noArray() : *rejected)); - if (undistort) { - } - - std::vector rvecs, tvecs; - cv::aruco::estimatePoseSingleMarkers(corners, this->marker_set_->getPhysicalSize(), - this->camera_params_.getCameraMatrix(), - this->camera_params_.getDistCoeff(), rvecs, tvecs); - - std::vector tags; - for (size_t i = 0; i < ids.size(); i++) { - size_t id = static_cast(ids[i]); - MarkerPattern marker = this->marker_set_->getMarkers()[id]; - Tag current(marker, rvecs[i], tvecs[i]); - tags.push_back(current); - } - - return tags; -} - -cv::aruco::DetectorParameters Detector::getDetectorParams() { - return *detector_params_; -} - -void Detector::setDetectorParams(cv::aruco::DetectorParameters params) { - *detector_params_ = params; -} - -} // namespace AR diff --git a/src/ar/Detector.h b/src/ar/Detector.h deleted file mode 100644 index 2b1955954..000000000 --- a/src/ar/Detector.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include "MarkerSet.h" -#include "Tag.h" - -#include -#include - -#include -#include - -namespace AR { - -/** - @addtogroup ar - @{ - */ -class Detector { -private: - std::shared_ptr marker_set_; - cv::Ptr detector_params_; - cv::Mat map1_, map2_; - std::vector _detectTagsImpl(const cv::Mat& input, - std::vector>* rejected, - bool undistort); - -public: - Detector(); - Detector(std::shared_ptr marker_set, - cv::Ptr detector_params = - cv::aruco::DetectorParameters::create()); - std::vector detectTags(const cv::Mat& input, - std::vector>& rejected_points, - bool undistort = true); - std::vector detectTags(const cv::Mat& input, bool undistort = true); - cv::aruco::DetectorParameters getDetectorParams(); - void setDetectorParams(cv::aruco::DetectorParameters params); - bool empty() const; -}; -/** @} */ - -} // namespace AR diff --git a/src/ar/DetectorTests.cpp b/src/ar/DetectorTests.cpp deleted file mode 100644 index e4b460965..000000000 --- a/src/ar/DetectorTests.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "Detector.h" - -#include - -namespace AR { -// TODO add Detector tests later. This is sort of a "nice to have" feature; we can already -// test the detector manually using the ar_tester program, but this would allow us to test -// certain limited cases automatically as part of our test suite. -} // namespace AR diff --git a/src/ar/MarkerPattern.cpp b/src/ar/MarkerPattern.cpp deleted file mode 100644 index 93163bfc4..000000000 --- a/src/ar/MarkerPattern.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#include "MarkerPattern.h" - -#include -#include -#include -#include - -#include - -using mat_ptr = cv::Ptr; - -namespace AR { - -///////// Marker class implementation /////////////// -MarkerPattern::MarkerPattern(uint8_t data_region_size, uint8_t border_size, cv::Mat bits, - int id) - : data_region_size(data_region_size), border_size(border_size), id(id), - data_bits(cv::Ptr(new cv::Mat(bits))) { - CHECK_GT_F(data_region_size, 0); - CHECK_GT_F(border_size, 0); - CHECK_EQ_F(bits.rows, data_region_size); - CHECK_EQ_F(bits.cols, data_region_size); -} - -MarkerPattern::MarkerPattern() {} - -bool MarkerPattern::empty() const { - return this->data_bits.empty(); -} - -uint8_t MarkerPattern::getDataRegionSize() const { - return this->data_region_size; -} - -uint8_t MarkerPattern::getBorderSize() const { - return this->border_size; -} - -const mat_ptr MarkerPattern::getDataBits() const { - return this->data_bits; -} - -int MarkerPattern::getId() const { - return this->id; -} - -bool MarkerPattern::operator==(const MarkerPattern& other) const { - return (this->data_region_size == other.data_region_size) && - (this->border_size == other.border_size) && (this->id == other.id) && - (std::equal(this->data_bits->begin(), this->data_bits->end(), - other.data_bits->begin())); -} -} // namespace AR diff --git a/src/ar/MarkerPattern.h b/src/ar/MarkerPattern.h deleted file mode 100644 index 9f61b88f7..000000000 --- a/src/ar/MarkerPattern.h +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace AR { - -/** - @addtogroup ar - @{ - */ - -/** - A class to represent the pattern that may appear on a physical AR tag, NOT a physical - instance of a tag itself; each individual instance of the pattern as it appears in the world - is represented by an instance of the Tag class, and there may be multiple Tags corresponding - to the same MarkerPattern. MarkerPatterns are square patterns of either black or white - pixels, usually with a border on the outside and a data region inside the - border. MarkerPatterns should be unique and referred to uniquely by an ID, which is a - non-negative integer that should be considered arbitrary and not guaranteed to have any - actual meaning. - - You should likely not have to construct any instances of this class yourself; this will be - done internally by the Detector class. - */ -class MarkerPattern { -private: - /** - The width in pixels of the data region of each marker. Does not include the border. - */ - uint8_t data_region_size; - /** - The width in pixels of the border of the marker. Note that this is only for ONE side of - the square; if the border is 1 pixel wide, then the marker's total size will be the data - region size + 2, for each border. - */ - uint8_t border_size; - /** - The ID of the marker. Note that this need not actually be encoded in the data of the - marker, just a unique ID to distinguish this marker from the others. - */ - int id; - /** - The actual bits stored in the data region of the marker, NOT including the border. - */ - cv::Ptr data_bits; - -public: - /** - Creates an empty marker, with a data region size, border size, and ID of 0, and an empty - matrix for the data bits. This is a default constructor just for convenience and you - should not try to use these empty markers. You can check if a marker is empty using the - Marker::empty() method. - */ - MarkerPattern(); - /** - Creates a marker, with the given data region size, border size, data bits, and id. - */ - MarkerPattern(uint8_t data_region_size, uint8_t border_size, cv::Mat bits, int id); - bool empty() const; - uint8_t getDataRegionSize() const; - uint8_t getBorderSize() const; - const cv::Ptr getDataBits() const; - int getId() const; - bool operator==(const MarkerPattern& other) const; -}; - -/** @} */ - -} // namespace AR diff --git a/src/ar/MarkerSet.cpp b/src/ar/MarkerSet.cpp deleted file mode 100644 index 512bdcc16..000000000 --- a/src/ar/MarkerSet.cpp +++ /dev/null @@ -1,157 +0,0 @@ -#include "MarkerSet.h" - -#include -#include -#include -#include - -#include -#include - -using ar_dict = cv::aruco::Dictionary; -using ar_dict_ptr = cv::Ptr; -using mat_ptr = cv::Ptr; - -namespace AR { - -///////// MarkerSet class implementation ////////////// -MarkerSet::MarkerSet(uint8_t data_region_size, uint8_t border_size, float physical_size, - ar_dict markerDict) { - ar_dict* dict_ = new ar_dict(markerDict); - init(data_region_size, border_size, physical_size, ar_dict_ptr(dict_)); -} - -MarkerSet::MarkerSet(uint8_t data_region_size, uint8_t border_size, float physical_size, - ar_dict_ptr markerDictPtr) { - CHECK_NOTNULL_F(markerDictPtr); - init(data_region_size, border_size, physical_size, markerDictPtr); -} - -void MarkerSet::init(uint8_t data_region_size, uint8_t border_size, float physical_size, - ar_dict_ptr markerDict) { - CHECK_GT_F(data_region_size, 0); - CHECK_GT_F(border_size, 0); - CHECK_GT_F(physical_size, 0); - this->data_region_size = data_region_size; - this->border_size = border_size; - this->physical_size = physical_size; - this->dict = markerDict; - - std::vector markerVec; - cv::Mat bytesList; - dict->bytesList.copyTo(bytesList); - for (int i = 0; i < bytesList.rows; i++) { - cv::Mat row = bytesList.row(i); - cv::Mat markerBits = ar_dict::getBitsFromByteList(row, data_region_size); - MarkerPattern current(data_region_size, border_size, markerBits, i); - markerVec.push_back(current); - } - this->markers = markerVec; -} - -void MarkerSet::addIDMapping(int id, int mapping) { - this->id_mappings[id] = mapping; - this->reverse_mappings[mapping] = id; -} - -int MarkerSet::getIDMapping(int id) const { - return this->id_mappings.at(id); -} - -int& MarkerSet::operator[](int id) { - return this->id_mappings[id]; -} - -ar_dict_ptr MarkerSet::getDict() const { - return this->dict; -} - -uint8_t MarkerSet::getDataRegionSize() const { - return this->data_region_size; -} - -uint8_t MarkerSet::getBorderSize() const { - return this->border_size; -} - -float MarkerSet::getPhysicalSize() const { - return this->physical_size; -} - -std::vector MarkerSet::getMarkers() const { - return this->markers; -} - -bool MarkerSet::isIDMapped(int id) const { - return this->id_mappings.count(id) == 1; -} - -bool MarkerSet::getMarkerByID(int id, MarkerPattern& out) const { - if (id < 0 || static_cast(id) > markers.size()) { - return false; - } - out = markers[id]; - return true; -} - -bool MarkerSet::getMarkerByMappedID(int mapped_id, MarkerPattern& out) const { - try { - out = markers[this->reverse_mappings.at(mapped_id)]; - return true; - } catch (std::out_of_range&) { - return false; - } -} - -namespace Markers { - -// Constants - please note that these are only used internally. -/** The size in pixels of the data region of an ARUco marker. */ -constexpr size_t ARUCO_BIT_SIZE = 4; -/** The border size in pixels of one side of an ARUco marker. */ -constexpr size_t ARUCO_BORDER_SIZE = 1; -/** The physical size (in mm) of an ARUco marker. */ -constexpr float ARUCO_PHYS_SIZE = 0.15; // we don't know this yet, change later - -/** - Constructs the URC marker set - */ -MarkerSet makeURCSet() { - static const cv::Ptr urc_dict_ptr = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - MarkerSet set(ARUCO_BIT_SIZE, ARUCO_BORDER_SIZE, ARUCO_PHYS_SIZE, urc_dict_ptr); - set.addIDMapping(0, START); - set.addIDMapping(1, POST1); - set.addIDMapping(2, POST2); - set.addIDMapping(3, POST3); - set.addIDMapping(4, GATEL); - set.addIDMapping(5, GATER); - - return set; -} - -/** - Constructs the CIRC marker set - */ -MarkerSet makeCIRCSet() { - static const cv::Ptr circ_dict_ptr = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - MarkerSet set(ARUCO_BIT_SIZE, ARUCO_BORDER_SIZE, ARUCO_PHYS_SIZE, circ_dict_ptr); - set.addIDMapping(0, CIRCMarker1); - // TODO: add all mappings when we figure out what the CIRC markers are - return set; -} - -const std::shared_ptr URC_MARKERS() { - static const MarkerSet URC_SET = makeURCSet(); - return std::make_shared(URC_SET); -} - -const std::shared_ptr CIRC_MARKERS() { - static const MarkerSet CIRC_SET = makeCIRCSet(); - return std::make_shared(CIRC_SET); -} - -}; // namespace Markers - -} // namespace AR diff --git a/src/ar/MarkerSet.h b/src/ar/MarkerSet.h deleted file mode 100644 index cce6b9143..000000000 --- a/src/ar/MarkerSet.h +++ /dev/null @@ -1,194 +0,0 @@ -#pragma once - -#include "MarkerPattern.h" - -#include -#include -#include - -#include -#include - -namespace AR { -/** - @addtogroup ar - @{ - */ - -/** - Represents a "set" of markers used for a competition, containing the markers that should be - recognized for that competition and also some other information like the physical - (real-world) size of the tags. Additionally maps markers to some kind of descriptive meaning - in the context for the competition; for example, a MarkerSet could be made for URC that - contains all the marker patterns used by the ALVAR system, and maps certain marker patterns - to enum constants describing which leg of the autonomous mission the markers represent. - - A MarkerSet is constructed from an ARUco Dictionary, which contains a list of all defined - marker patterns, which all have some internal ID; the first marker in the Dictionary has ID - 0, the second has ID 1, etc. These IDs are usually meaningless and only refer to the order - of the markers in the dictionary. Therefore, the MarkerSet class introduces a concept of an - ID mapping; that is, a mapping from a marker's actual ID (as defined by the ARUco - dictionary) to a user-defined ID that may have some actual meaning in context. Not all - markers in the dictionary have to be mapped; the user may choose to map only some meaningful - ones and leave others without a mapping. - - Note that you should probably not need to construct any instances of this class yourself; - Markers.h will contain predefined MarkerSets that you should use. - */ -class MarkerSet { -private: - cv::Ptr dict; - std::vector markers; - float physical_size; - uint8_t data_region_size; - uint8_t border_size; - std::unordered_map id_mappings; - std::unordered_map reverse_mappings; - void init(uint8_t data_region_size, uint8_t border_size, float physical_size, - cv::Ptr markerDict); - -public: - /** - Constructs a new marker set. Note that you should likely not need to do this as a client - of the AR tag detection system. - - Parameters: - - data_region_size: The size of the data region on the marker in pixels/blocks - (NOT including the border). - - - border_size: The size of the border on ONE side of the marker. For example, if the - total marker size is 9 pixels and the data region is only 5 pixels wide, then the border - size will be 2 (instead of 4). - - - physical_size: The size (in real-world units) of the tags that the markers will appear - on. It is important that this is the same scale as the units used for camera calibration - or otherwise the estimated measurements will be off by factors of 10. - - - markerDict: The ARUco dictionary containing marker patterns. - */ - MarkerSet(uint8_t data_region_size, uint8_t border_size, float physical_size, - cv::aruco::Dictionary markerDict); - /** - Constructs a new marker set. Note that you should likely not need to do this as a client - of the AR tag detection system. - - Parameters: - - data_region_size: The size of the data region on the marker in pixels/blocks - (NOT including the border). - - - border_size: The size of the border on ONE side of the marker. For example, if the - total marker size is 9 pixels and the data region is only 5 pixels wide, then the border - size will be 2 (instead of 4). - - - physical_size: The size (in real-world units) of the tags that the markers will appear - on. It is important that this is the same scale as the units used for camera calibration - or otherwise the estimated measurements will be off by factors of 10. - - - markerDictPtr: An OpenCV shared pointer (cv::Ptr) to an ARUCO dictionary containing - marker patterns. - */ - MarkerSet(uint8_t data_region_size, uint8_t border_size, float physical_size, - cv::Ptr markerDictPtr); - /** - Adds an ID mapping; that is, a mapping from a marker's actual ID (as defined by the - ARUco dictionary) to a user-defined ID that may have some actual meaning in context. - */ - void addIDMapping(int id, int mapping); - /** - Gets an ID mapping. The ID mapping should actually be defined; this method will throw a - std::out_of_range exception if the ID mapping does not exist. See isIDMapped(). - */ - int getIDMapping(int id) const; - /** - Gets an ID mapping, statically cast to the type given in the template type - parameter. The type given should be a type that an integer can be statically cast to - (e.g. enums). This method will throw a std::out_of_range exception if the ID mapping - does not exist. See isIDMapped(). - */ - template IDMapping_t getIDMappingCast(int id) const { - return static_cast(this->getIDMapping(id)); - } - /** - Returns the underlying ARUco dictionary defining the marker patterns. - */ - cv::Ptr getDict() const; - /** - Returns the data region size; i.e. the size of the data region of the marker (NOT - including border). - */ - uint8_t getDataRegionSize() const; - /** - Returns the border size on ONE side of the marker. For example if the marker is 9 pixels - wide and the data region is 5 pixels wide, the border size will be 2 (since the border - is on both sides of the marker). - */ - uint8_t getBorderSize() const; - /** - Returns the physical size (in real-world units) of the tags that the markers will appear - on. The unit scale is defined by the user when an instance of this class is - constructed. - */ - float getPhysicalSize() const; - /** - Returns a vector of the Markers in this marker set. - */ - std::vector getMarkers() const; - /** - Gets a Marker by its ID, as defined by the ARUco Dictionary. If the Marker exists, this - method will return true and the Marker will be returned through the output parameter; if - it does not exist, this method will return false and the output parameter will not be - modified. - */ - bool getMarkerByID(int id, MarkerPattern& out) const; - /** - Gets a Marker by its mapped ID (as defined with addIDMapping()). If the mapping exists, - this method will return true and the Marker will be returned through the output - parameter; if it does not exist, this method will return false and the output parameter - will not be modified. - */ - bool getMarkerByMappedID(int mapped_id, MarkerPattern& out) const; - /** - Returns true if the given user-defined ID mapping exists and false if it does not. - */ - bool isIDMapped(int id) const; - /** - Operator to add or get ID mappings; can be used instead of addIDMapping/getIDMapping. - */ - int& operator[](int id); -}; - -namespace Markers { -/** - Enum of marker names for URC. - */ -enum URCMarkerName { - START, - POST1, - POST2, - POST3, - GATEL, - GATER -}; - -/** - Enum of marker names for CIRC. - - NOTE: I currently have no information about what the markers will represent for CIRC and - which marker IDs are important. Will update when I get more information about that. As of - right now, all members of this enum are placeholders. - */ -enum CIRCMarkerName { CIRCMarker1, CIRCMarker2 }; - -/** - Returns the set of markers that will be used in URC. -*/ -const std::shared_ptr URC_MARKERS(); - -/** - Returns the set of markers that will be used in CIRC. -*/ -const std::shared_ptr CIRC_MARKERS(); - -} // namespace Markers -/** @} */ -} // namespace AR diff --git a/src/ar/MarkerSetTests.cpp b/src/ar/MarkerSetTests.cpp deleted file mode 100644 index f050b7572..000000000 --- a/src/ar/MarkerSetTests.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "MarkerSet.h" - -#include - -#define TAG "[ar][marker_set]" -const cv::Ptr DICT_4X4_50 = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - -static AR::MarkerSet makeSet() { - AR::MarkerSet ms = AR::MarkerSet(4, 1, 0.1, DICT_4X4_50); - CHECK(ms.getDataRegionSize() == 4); - CHECK(ms.getDataRegionSize() == DICT_4X4_50->markerSize); - CHECK(ms.getBorderSize() == 1); - CHECK(ms.getPhysicalSize() == Approx(0.1)); - return ms; -} - -namespace AR { - -TEST_CASE("Constructing MarkerSet test", TAG) { - REQUIRE_NOTHROW(makeSet()); -} - -TEST_CASE("Predefined MarkerSets defined correctly", TAG) { - CHECK(Markers::URC_MARKERS()); - CHECK(Markers::CIRC_MARKERS()); -} - -TEST_CASE("Adding and retrieving ID mappings works", TAG) { - MarkerSet ms = makeSet(); - SECTION("Mappings that are added can be retrieved") { - ms.addIDMapping(0, 0); - ms.addIDMapping(1, 3); - CHECK(ms.isIDMapped(0)); - CHECK(ms.isIDMapped(1)); - CHECK(ms.getIDMapping(0) == 0); - CHECK(ms.getIDMapping(1) == 3); - } - SECTION("Retrieving non-existent mapping throws exception") { - ms.addIDMapping(0, 0); - ms.addIDMapping(1, 3); - REQUIRE_THROWS_AS(ms.getIDMapping(2), std::out_of_range); - REQUIRE_THROWS_AS(ms.getIDMapping(45), std::out_of_range); - } - SECTION("[] operator can be used to get/set mappings") { - ms[4] = 5; - ms[6] = -1; - CHECK(ms[6] == -1); - CHECK(ms[4] == 5); - } - - SECTION("Mappings can be retrieved as a compatible type") { - enum TestType { A, B, C, D }; - - ms.addIDMapping(0, 0); - ms.addIDMapping(1, 1); - ms.addIDMapping(4, 2); - ms.addIDMapping(6, 3); - CHECK(ms.getIDMappingCast(0) == TestType::A); - CHECK(ms.getIDMappingCast(1) == TestType::B); - CHECK(ms.getIDMappingCast(4) == TestType::C); - CHECK(ms.getIDMappingCast(6) == TestType::D); - } -} - -TEST_CASE("Markers can be retrieved from the MarkerSet") { - MarkerSet ms = makeSet(); - std::vector markers = ms.getMarkers(); - // marker vector should have the same number of markers as are in the - // dictionary - CHECK(markers.size() == static_cast(DICT_4X4_50->bytesList.rows)); - SECTION("Markers should be the same as in the dictionary") { - for (size_t i = 0; i < markers.size(); i++) { - cv::Mat dict_marker_bits = DICT_4X4_50->getBitsFromByteList( - DICT_4X4_50->bytesList.row(i), ms.getDataRegionSize()); - CHECK(std::equal(dict_marker_bits.begin(), - dict_marker_bits.end(), - markers[i].getDataBits()->begin())); - CHECK(markers[i].getBorderSize() == ms.getBorderSize()); - CHECK(markers[i].getDataRegionSize() == ms.getDataRegionSize()); - } - } - SECTION("Markers can be retrieved by ID") { - for (size_t i = 0; i < markers.size(); i++) { - MarkerPattern from_vector = markers[i]; - MarkerPattern from_set; - CHECK(ms.getMarkerByID(i, from_set)); - CHECK(from_vector == from_set); - } - } - SECTION("Markers can be retrieved by a mapped ID") { - std::array nums{0, 3, 7, 11, 13}; - for (size_t i = 0; i < nums.size(); i++) { - ms.addIDMapping(nums[i], i); - } - for (size_t i = 0; i < nums.size(); i++) { - MarkerPattern m; - CHECK(ms.getMarkerByMappedID(i, m)); - CHECK(markers[nums[i]] == m); - } - // non-existent ID should return false - MarkerPattern m; - CHECK_FALSE(ms.getMarkerByMappedID(-1, m)); - } -} - -} // namespace AR diff --git a/src/ar/Tag.cpp b/src/ar/Tag.cpp deleted file mode 100644 index f37e194e5..000000000 --- a/src/ar/Tag.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "Tag.h" - -namespace AR { - -Tag::Tag(MarkerPattern marker, cv::Vec3d rvec, cv::Vec3d tvec) - : rvec_(rvec), tvec_(tvec), marker_(marker) { -} - -cv::Vec3d Tag::getRVec() const { - return rvec_; -} - -cv::Vec3d Tag::getTVec() const { - return tvec_; -} - -cv::Vec3d Tag::getCoordinates() const { - return tvec_; -} - -MarkerPattern Tag::getMarker() const { - return marker_; -} - -} // namespace AR diff --git a/src/ar/Tag.h b/src/ar/Tag.h deleted file mode 100644 index bcafc6359..000000000 --- a/src/ar/Tag.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "MarkerPattern.h" - -#include - -namespace AR { - -/** - @addtogroup ar - @{ - */ - -/** - @brief Class representing an AR tag detected in an image. - - This class is simply used as a way to represent the result of a detection, and can - provide location and orientation information. You should likely never need to construct - a Tag yourself; this is done internally by the detection code. All tags are immutable - after construction. - */ -class Tag { -private: - cv::Vec3d rvec_; - cv::Vec3d tvec_; - MarkerPattern marker_; - -public: - Tag(MarkerPattern marker, cv::Vec3d rvec, cv::Vec3d tvec); - Tag() = delete; - /** - Gets the rotation vector of the tag. This vector defines a line through the origin - around which points are rotated to transform them from the object reference frame to the - camera reference frame; its length is the angle of rotation. You will likely not need - this for most uses. - */ - cv::Vec3d getRVec() const; - /** - Gets the translation vector of the tag. This vector defines the translation necessary to - transform points from the object reference frame to the camera reference frame. For the - center of the tag (0,0,0), this vector is in effect the 3D coordinates of the tag - relative to the camera. - - This function will return real world units; these are dependent upon the camera - parameters you are currently using. - */ - cv::Vec3d getTVec() const; - /** - Gets the 3D coordinates of the tag with respect to the camera. The return value is - currently the same as getTVec, but this function is provided for convenience as its name - is more intuitive. - - This function will return real world units; these are dependent upon the camera - parameters you are currently using. - */ - cv::Vec3d getCoordinates() const; - /** - Gets the Marker associated with the tag. - */ - MarkerPattern getMarker() const; -}; - -/** @} */ - -} // namespace AR diff --git a/src/ar/read_landmarks.cpp b/src/ar/read_landmarks.cpp deleted file mode 100644 index 72ef9360e..000000000 --- a/src/ar/read_landmarks.cpp +++ /dev/null @@ -1,138 +0,0 @@ -#include "read_landmarks.h" - -#include "../Constants.h" -#include "../camera/Camera.h" -#include "../world_interface/world_interface.h" -#include "Detector.h" - -#include -#include -#include -#include -#include - -#include - -using namespace robot::types; - -namespace AR { -constexpr size_t NUM_LANDMARKS = 11; -constexpr std::chrono::milliseconds LANDMARK_FRESH_PERIOD(100); - -const landmarks_t zero_landmarks(NUM_LANDMARKS); - -Detector ar_detector(Markers::URC_MARKERS(), cam::CameraParams()); - -std::atomic fresh_data(false); -std::mutex landmark_lock; -landmarks_t current_landmarks(NUM_LANDMARKS); -std::thread landmark_thread; -bool initialized = false; - -void detectLandmarksLoop() { - loguru::set_thread_name("LandmarkDetection"); - cv::Mat frame; - uint32_t last_frame_no = 0; - while (true) { - if (robot::hasNewCameraFrame(Constants::MAST_CAMERA_ID, last_frame_no)) { - auto camData = robot::readCamera(Constants::MAST_CAMERA_ID); - if (!camData) - continue; - auto camFrame = camData.getData(); - datatime_t frameTime = camData.getTime(); - frame = camFrame.first; - last_frame_no = camFrame.second; - - std::vector tags = ar_detector.detectTags(frame); - LOG_F(2, "readLandmarks(): %ld tags spotted", tags.size()); - - // build up map with first tag of each ID spotted. - landmarks_t output(NUM_LANDMARKS); - std::map ids_to_camera_coords; - for (AR::Tag tag : tags) { - int id = tag.getMarker().getId(); - if (ids_to_camera_coords.find(id) == ids_to_camera_coords.end()) { - ids_to_camera_coords[id] = tag.getCoordinates(); - } - } - - // for every possible landmark ID, go through and if the map contains a value for - // that ID, add it to the output array (doing appropriate coordinate space - // transforms). If not, add a zero point. - for (size_t i = 0; i < NUM_LANDMARKS; i++) { - int id = static_cast(i); - if (ids_to_camera_coords.find(id) != ids_to_camera_coords.end()) { - cv::Vec3d coords = ids_to_camera_coords[id]; - // if we have extrinsic parameters, multiply coordinates by them to do - // appropriate transformation. - auto extrinsic = - robot::getCameraExtrinsicParams(Constants::MAST_CAMERA_ID); - if (extrinsic) { - cv::Vec4d coords_homogeneous = {coords[0], coords[1], coords[2], 1}; - cv::Mat transformed = extrinsic.value() * cv::Mat(coords_homogeneous); - output[i] = { - frameTime, - {transformed.at(0, 0), transformed.at(1, 0), 1.0}}; - } else { - // just account for coordinate axis change; rover frame has +x front, - // +y left, +z up while camera has +x right, +y down, +z front. - output[i] = {frameTime, {coords[2], -coords[0], 1.0}}; - } - } else { - output[i] = {}; - } - } - - landmark_lock.lock(); - for (size_t i = 0; i < NUM_LANDMARKS; i++) { - // only overwrite data if the new data is valid or the previous data is expired - // detection is a bit spotty, so we don't want to overwrite good data witih - // false negatives - if (output[i].isValid() || - !current_landmarks[i].isFresh(LANDMARK_FRESH_PERIOD)) { - current_landmarks[i] = output[i]; - } - } - fresh_data = true; - landmark_lock.unlock(); - } - } -} - -bool initializeLandmarkDetection() { - auto intrinsic = robot::getCameraIntrinsicParams(Constants::MAST_CAMERA_ID); - if (intrinsic) { - ar_detector = Detector(Markers::URC_MARKERS(), intrinsic.value()); - landmark_thread = std::thread(&detectLandmarksLoop); - } else { - LOG_F(ERROR, "Camera does not have intrinsic parameters! AR tag detection " - "cannot be performed."); - return false; - } - if (!robot::getCameraExtrinsicParams(Constants::MAST_CAMERA_ID)) { - LOG_F(WARNING, "Camera does not have extrinsic parameters! Coordinates returned " - "for AR tags will be relative to camera"); - } - initialized = true; - return true; -} - -bool isLandmarkDetectionInitialized() { - return initialized; -} - -landmarks_t readLandmarks() { - if (isLandmarkDetectionInitialized()) { - if (fresh_data) { - landmarks_t output; - landmark_lock.lock(); - output = current_landmarks; - fresh_data = false; - landmark_lock.unlock(); - return output; - } - } - return zero_landmarks; -} - -} // namespace AR diff --git a/src/ar/read_landmarks.h b/src/ar/read_landmarks.h deleted file mode 100644 index 11ae2b72f..000000000 --- a/src/ar/read_landmarks.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -#include "../world_interface/data.h" - -namespace AR { -bool initializeLandmarkDetection(); -bool isLandmarkDetectionInitialized(); -robot::types::landmarks_t readLandmarks(); -} // namespace AR From 16dcb5a7b7dcb8d51766b641eeab443f7db5e5cc Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 17:50:07 -0800 Subject: [PATCH 06/10] Remove CameraParamsTests --- src/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7309455c1..707e3ed8d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -311,7 +311,7 @@ if(WITH_TESTS) # ar/DetectorTests.cpp # ar/MarkerSetTests.cpp # Camera tests - ../tests/camera/CameraParamsTests.cpp + # ../tests/camera/CameraParamsTests.cpp # GPS tests ../tests/gps/GPSDatumTest.cpp ../tests/gps/GPSConverterTest.cpp From fa3a6002ac15862a04db6b524999f32aeb0ade8b Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 18:08:45 -0800 Subject: [PATCH 07/10] Add frozen maps, fix formatting --- src/CMakeLists.txt | 132 ++++++++++---------- src/Constants.cpp | 18 ++- src/Constants.h | 5 +- src/camera/Camera.cpp | 12 +- src/world_interface/simulator_interface.cpp | 4 +- 5 files changed, 84 insertions(+), 87 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 707e3ed8d..14a1f7425 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.13) # Latest versions of Ubuntu should have at least 3.16; - # argparse library requires 3.12.4 currently +cmake_minimum_required(VERSION 3.13) #Latest versions of Ubuntu should have at least 3.16; +#argparse library requires 3.12.4 currently set(CMAKE_CXX_STANDARD 17) # 11 For the json library, 17 for std library features set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -8,13 +8,14 @@ project(Rover LANGUAGES CXX C) include(FetchContent) -# If a build type was not explicitly specified, default to "RelWithDebInfo" (Same optimization -# as the "Release" configuration but with debugging symbols). +#If a build type was not explicitly specified, default to "RelWithDebInfo"(Same optimization +#as the "Release" configuration but with debugging symbols). # -# To change, you can pass -DCMAKE_BUILD_TYPE=... where ... is the configuration you would like -# to use. For example, if you want to do a full debug build (little to no optimization, with -# debugging symbols), add -DCMAKE_BUILD_TYPE=Debug to your `cmake` command. You may want to do -# a full debug build if code you want to debug gets optimized out by the compiler. +#To change, you can pass - DCMAKE_BUILD_TYPE = ... where... is the configuration you would like +#to use.For example, if you want to do a full debug build(little to no optimization, with +#debugging symbols), \ + add - DCMAKE_BUILD_TYPE = Debug to your `cmake` command.You may want to do +#a full debug build if code you want to debug gets optimized out by the compiler. set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "The type of build to perform.") set(AVAILABLE_BUILD_TYPES "Debug" "Release" "RelWithDebInfo" "MinSizeRel") set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${AVAILABLE_BUILD_TYPES}) @@ -23,17 +24,17 @@ Building ${CMAKE_BUILD_TYPE} configuration...\n\ Add -DCMAKE_BUILD_TYPE=... to change\n\ ========================================") -# Generate a compile_commands.json file by default unless explicitly disabled. -# This allows editors like Visual Studio Code to be able to index the project correctly and -# locate header files and libraries. +#Generate a compile_commands.json file by default unless explicitly disabled. +#This allows editors like Visual Studio Code to be able to index the project correctly and +#locate header files and libraries. if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE STRING "Enable/Disable output of compile commands during generation." FORCE) endif() ##====== Module settings ====== -# These variables control whether various modules are enabled or disabled. The goal is to allow -# a user to selectively exclude modules that they do not need. +#These variables control whether various modules are enabled or disabled.The goal is to allow +#a user to selectively exclude modules that they do not need. set(GPS "ARDUPILOT" CACHE STRING "GPS interface to use.") set(AVAILABLE_GPS "USB" "ARDUPILOT" "NONE") set_property(CACHE GPS PROPERTY STRINGS ${AVAILABLE_GPS}) @@ -60,38 +61,37 @@ message(" World Interface:\t${WORLD_INTERFACE}") message("========================================") ##====== Dependencies ====== -# Some of these are optional depending on module settings above. +#Some of these are optional depending on module settings above. -# Get the Eigen linear algebra library. Eigen's CMake config includes its headers as user -# headers instead of system headers by default, so explicitly force them to be system includes -# here to avoid the compiler spamming warnings at us for stuff in the Eigen headers. +#Get the Eigen linear algebra library.Eigen's CMake config includes its headers as user +#headers instead of system headers by default, so explicitly force them to be system includes +#here to avoid the compiler spamming warnings at us for stuff in the Eigen headers. find_package(Eigen3 REQUIRED NO_MODULE) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) include_directories("/usr/local/include") -# Find the WebSocket++ library and Boost (provides the ASIO backend for Websocket++ and also -# provides the program_options argument parser). Only the `system` component of Boost is -# currently required. +#Find the WebSocket++ library and Boost(provides the ASIO backend for Websocket++ and also +#provides the program_options argument parser).Only the `system` component of Boost is +#currently required. find_package(websocketpp REQUIRED) find_package(Boost REQUIRED COMPONENTS system) -# Find Frozen, library used for constexpr immutable containers +#Find Frozen, library used for constexpr immutable containers find_package(frozen REQUIRED) -# Frozen is just an interface library, only containing headers and some CMake settings. It -# should be fine to "link" the library to every target, since if the headers are not included, -# it will not have any effect on size or performance. This will help us not have to worry about -# making sure the headers are available in every file that includes Constants.h, for example. +#Frozen is just an interface library, only containing headers and some CMake settings.It +#should be fine to "link" the library to every target, since if the headers are not included, +#it will not have any effect on size or performance.This will help us not have to worry about +#making sure the headers are available in every file that includes Constants.h, for example. link_libraries(frozen::frozen) -# Find argparse, library used for parsing command line arguments +#Find argparse, library used for parsing command line arguments find_package(argparse REQUIRED) - -# Find the JSON library +#Find the JSON library find_package(nlohmann_json 3.2.0 REQUIRED) -# Find the CAN library; contains packet and protocol definitions and does not -# actually require physical CAN to be present. +#Find the CAN library; contains packet and protocol definitions and does not +#actually require physical CAN to be present. FetchContent_Declare( HindsightCAN GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git @@ -103,7 +103,7 @@ FetchContent_Declare(LoguruGitRepo GIT_REPOSITORY "https://github.com/emilk/loguru" GIT_TAG "master" ) -# set any loguru compile-time flags before calling MakeAvailable() +#set any loguru compile - time flags before calling MakeAvailable() set(LOGURU_WITH_STREAMS TRUE) set(CMAKE_POSITION_INDEPENDENT_CODE ON) FetchContent_MakeAvailable(LoguruGitRepo) # defines target 'loguru::loguru' @@ -116,13 +116,13 @@ if(WITH_TESTS) find_package(Catch2 REQUIRED) endif() -# Libraries needed to interface with real-life sensors. If we aren't using the real world -# interface, these aren't needed. +#Libraries needed to interface with real - life sensors.If we aren't using the real world +#interface, these aren't needed. if(WORLD_INTERFACE STREQUAL "REAL") if(GPS STREQUAL "USB") - # Find pkg-config (needed for libgps, as it doesn't have CMake configuration) +#Find pkg - config(needed for libgps, as it doesn't have CMake configuration) find_package(PkgConfig REQUIRED) - # Find the libgps USB GPS library. +#Find the libgps USB GPS library. pkg_check_modules(libgps REQUIRED libgps) endif() endif() @@ -136,7 +136,7 @@ add_library(camera SHARED ) target_link_libraries(camera PUBLIC ${OpenCV_LIBS}) -# shared library for utility code +#shared library for utility code add_library(utils SHARED utils/core.cpp utils/random.cpp @@ -155,22 +155,22 @@ add_library(utils SHARED target_link_libraries(utils ${OpenCV_LIBS}) ## ====== CAN Interfaces ======= -# Stub CAN interface is used for the tests (and for the Rover if -# CAN is disabled) and the real CAN interface is used for the Rover if CAN is -# enabled. +#Stub CAN interface is used for the tests(and for the Rover if +#CAN is disabled) and the real CAN interface is used for the Rover if CAN is +#enabled. -# Common CAN source files +#Common CAN source files -# **DON'T MAKE THIS SHARED** -# Making this library shared causes some memory fuckery -# No clue why but CAN I/O goes to shit. Don't do it. +#** DON'T MAKE THIS SHARED** +#Making this library shared causes some memory fuckery +#No clue why but CAN I / O goes to shit.Don't do it. add_library(can_interface STATIC) target_sources(can_interface PRIVATE CAN/CANMotor.cpp CAN/CANUtils.cpp ) -# Hardware specific source files +#Hardware specific source files target_sources(can_interface PRIVATE CAN/CAN.cpp) target_link_libraries(can_interface PUBLIC @@ -195,20 +195,20 @@ else() endif() ## ====== World Interfaces ======= -# hardware-agnostic utilities and common code for world interface +#hardware - agnostic utilities and common code for world interface add_library(world_interface_core STATIC world_interface/gps_common_interface.cpp - #ar/Detector.cpp - #ar/MarkerSet.cpp - #ar/MarkerPattern.cpp - #ar/Tag.cpp - #ar/read_landmarks.cpp +#ar / Detector.cpp +#ar / MarkerSet.cpp +#ar / MarkerPattern.cpp +#ar / Tag.cpp +#ar / read_landmarks.cpp world_interface/motor/base_motor.cpp ) target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) target_link_libraries(world_interface_core PUBLIC ${vision_libs} opencv_aruco ${OpenCV_LIBS} utils camera) -# CAN library (above) requires some utilities from this +#CAN library(above) requires some utilities from this target_link_libraries(can_interface PUBLIC utils) add_library(stub_world_interface STATIC @@ -268,9 +268,9 @@ add_library(constants SHARED Constants.cpp) link_libraries(constants) add_executable(Rover Rover.cpp) -# **DON'T MAKE THIS SHARED** -# Making this library shared causes some memory fuckery -# No clue why but CAN I/O goes to shit. Don't do it. +#** DON'T MAKE THIS SHARED** +#Making this library shared causes some memory fuckery +#No clue why but CAN I / O goes to shit.Don't do it. add_library(rover_common STATIC Globals.cpp control_interface.cpp @@ -307,38 +307,38 @@ target_link_libraries(TunePID ${vision_libs}) if(WITH_TESTS) add_executable(tests Tests.cpp - # AR Detection tests - # ar/DetectorTests.cpp - # ar/MarkerSetTests.cpp - # Camera tests - # ../tests/camera/CameraParamsTests.cpp - # GPS tests +#AR Detection tests +#ar / DetectorTests.cpp +#ar / MarkerSetTests.cpp +#Camera tests +#../ tests / camera / CameraParamsTests.cpp +#GPS tests ../tests/gps/GPSDatumTest.cpp ../tests/gps/GPSConverterTest.cpp - # Controller tests +#Controller tests ../tests/control/TrapezoidalVelocityProfileTest.cpp ../tests/control/JacobianControllerTest.cpp ../tests/control/PIDControllerTest.cpp ../tests/control/PlanarArmControllerTest.cpp - # Kinematics tests +#Kinematics tests ../tests/kinematics/DiffDriveKinematicsTest.cpp ../tests/kinematics/SwerveDriveKinematicsTest.cpp ../tests/kinematics/PlanarArmFKTest.cpp ../tests/kinematics/FabrikSolver2DTest.cpp - # Filter tests +#Filter tests ../tests/filters/RollingAvgFilterTest.cpp ../tests/filters/EKFTest.cpp ../tests/filters/MultiSensorEKFTest.cpp ../tests/filters/StateSpaceUtilsTest.cpp - # Command tests +#Command tests ../tests/commands/DriveToWaypointCommandTest.cpp - # Util tests +#Util tests ../tests/util/CoreTest.cpp ../tests/util/DataTest.cpp ../tests/util/MathTest.cpp ../tests/util/TimeTest.cpp ../tests/util/SchedulerTest.cpp - # Protocol/teleop tests +#Protocol / teleop tests ../tests/kinematics/DiffWristKinematicsTest.cpp) target_link_libraries(tests diff --git a/src/Constants.cpp b/src/Constants.cpp index 941356d7c..0eea9edc1 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -51,17 +51,13 @@ const robot::types::CameraID MAST_CAMERA_ID = 40; const robot::types::CameraID WRIST_CAMERA_ID = 30; const robot::types::CameraID HAND_CAMERA_ID = 20; -const std::unordered_map CAMERA_CONFIG_PATHS = { - {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, - {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, - {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"} -}; - -const std::unordered_map CAMERA_NAME_TO_ID = { - {"mast", MAST_CAMERA_ID}, - {"wrist", WRIST_CAMERA_ID}, - {"hand", HAND_CAMERA_ID} -}; +constexpr frozen::unordered_map CAMERA_CONFIG_PATHS = { + {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, + {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, + {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"}}; + +constexpr frozen::unordered_map CAMERA_NAME_TO_ID = { + {"mast", MAST_CAMERA_ID}, {"wrist", WRIST_CAMERA_ID}, {"hand", HAND_CAMERA_ID}}; /** @deprecated No need for this constant once we fully switch over the Mission Control PlanViz diff --git a/src/Constants.h b/src/Constants.h index 2e853c2a8..0822c0ffd 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -57,8 +57,9 @@ extern const robot::types::CameraID MAST_CAMERA_ID; extern const robot::types::CameraID WRIST_CAMERA_ID; extern const robot::types::CameraID HAND_CAMERA_ID; -extern const std::unordered_map CAMERA_CONFIG_PATHS; -extern const std::unordered_map CAMERA_NAME_TO_ID; +extern constexpr frozen::unordered_map + CAMERA_CONFIG_PATHS; +extern constexpr frozen::unordered_map CAMERA_NAME_TO_ID; extern const uint16_t WS_SERVER_PORT; diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 21c2a550d..7a3dd5a7d 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -1,4 +1,5 @@ #include "Camera.h" + #include "../Constants.h" #include @@ -14,8 +15,7 @@ using namespace robot::types; namespace cam { Camera::Camera() : _frame(std::make_shared()), _frame_num(std::make_shared(0)), - _frame_time(std::make_shared()), - _capture(), + _frame_time(std::make_shared()), _capture(), _frame_lock(std::make_shared()), _capture_lock(std::make_shared()), _running(std::make_shared(false)) {} @@ -24,8 +24,7 @@ Camera::Camera(robot::types::CameraID camera_id) _frame_time(std::make_shared()), _capture(std::make_shared(camera_id)), _frame_lock(std::make_shared()), - _capture_lock(std::make_shared()), - _running(std::make_shared(false)) { + _capture_lock(std::make_shared()), _running(std::make_shared(false)) { this->open(camera_id); } @@ -65,7 +64,8 @@ void Camera::init() { std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id), cv::FileStorage::READ); if (!fs.isOpened()) { - throw std::invalid_argument("Configuration file of camera ID" + std::to_string(camera_id) + " does not exist"); + throw std::invalid_argument("Configuration file of camera ID" + + std::to_string(camera_id) + " does not exist"); } std::stringstream gstr_ss; @@ -89,7 +89,7 @@ std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { gstr_ss << ",height=" << _height; gstr_ss << ",framerate=" << fs[KEY_FRAMERATE].operator std::string() << "/1"; gstr_ss << " ! " << fs[KEY_FORMAT].operator std::string() << "dec ! videoconvert"; - + gstr_ss << " ! appsink"; LOG_F(INFO, "GSTR: %s", gstr_ss.str().c_str()); diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 7e0df2cc0..d600e5d0e 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -87,14 +87,14 @@ void sendJSON(const json& obj) { wsServer->get().sendJSON(PROTOCOL_PATH, obj); } -static void openCamera(CameraID cam, uint8_t fps = 30, uint16_t width = 640, uint16_t height = 480) { +static void openCamera(CameraID cam, uint8_t fps = 30, uint16_t width = 640, + uint16_t height = 480) { json msg = {{"type", "simCameraStreamOpenRequest"}, {"cameraID", cam}, {"fps", fps}, {"width", width}, {"height", height}}; sendJSON(msg); - } void initCameras() { From 3fde55010827a83768a7accceeb751d2c9fe1278 Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 18:13:48 -0800 Subject: [PATCH 08/10] Undo clang-format on CMakeLists --- src/CMakeLists.txt | 132 ++++++++++++++++++++++----------------------- 1 file changed, 66 insertions(+), 66 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 14a1f7425..707e3ed8d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.13) #Latest versions of Ubuntu should have at least 3.16; -#argparse library requires 3.12.4 currently +cmake_minimum_required(VERSION 3.13) # Latest versions of Ubuntu should have at least 3.16; + # argparse library requires 3.12.4 currently set(CMAKE_CXX_STANDARD 17) # 11 For the json library, 17 for std library features set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -8,14 +8,13 @@ project(Rover LANGUAGES CXX C) include(FetchContent) -#If a build type was not explicitly specified, default to "RelWithDebInfo"(Same optimization -#as the "Release" configuration but with debugging symbols). +# If a build type was not explicitly specified, default to "RelWithDebInfo" (Same optimization +# as the "Release" configuration but with debugging symbols). # -#To change, you can pass - DCMAKE_BUILD_TYPE = ... where... is the configuration you would like -#to use.For example, if you want to do a full debug build(little to no optimization, with -#debugging symbols), \ - add - DCMAKE_BUILD_TYPE = Debug to your `cmake` command.You may want to do -#a full debug build if code you want to debug gets optimized out by the compiler. +# To change, you can pass -DCMAKE_BUILD_TYPE=... where ... is the configuration you would like +# to use. For example, if you want to do a full debug build (little to no optimization, with +# debugging symbols), add -DCMAKE_BUILD_TYPE=Debug to your `cmake` command. You may want to do +# a full debug build if code you want to debug gets optimized out by the compiler. set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "The type of build to perform.") set(AVAILABLE_BUILD_TYPES "Debug" "Release" "RelWithDebInfo" "MinSizeRel") set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${AVAILABLE_BUILD_TYPES}) @@ -24,17 +23,17 @@ Building ${CMAKE_BUILD_TYPE} configuration...\n\ Add -DCMAKE_BUILD_TYPE=... to change\n\ ========================================") -#Generate a compile_commands.json file by default unless explicitly disabled. -#This allows editors like Visual Studio Code to be able to index the project correctly and -#locate header files and libraries. +# Generate a compile_commands.json file by default unless explicitly disabled. +# This allows editors like Visual Studio Code to be able to index the project correctly and +# locate header files and libraries. if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE STRING "Enable/Disable output of compile commands during generation." FORCE) endif() ##====== Module settings ====== -#These variables control whether various modules are enabled or disabled.The goal is to allow -#a user to selectively exclude modules that they do not need. +# These variables control whether various modules are enabled or disabled. The goal is to allow +# a user to selectively exclude modules that they do not need. set(GPS "ARDUPILOT" CACHE STRING "GPS interface to use.") set(AVAILABLE_GPS "USB" "ARDUPILOT" "NONE") set_property(CACHE GPS PROPERTY STRINGS ${AVAILABLE_GPS}) @@ -61,37 +60,38 @@ message(" World Interface:\t${WORLD_INTERFACE}") message("========================================") ##====== Dependencies ====== -#Some of these are optional depending on module settings above. +# Some of these are optional depending on module settings above. -#Get the Eigen linear algebra library.Eigen's CMake config includes its headers as user -#headers instead of system headers by default, so explicitly force them to be system includes -#here to avoid the compiler spamming warnings at us for stuff in the Eigen headers. +# Get the Eigen linear algebra library. Eigen's CMake config includes its headers as user +# headers instead of system headers by default, so explicitly force them to be system includes +# here to avoid the compiler spamming warnings at us for stuff in the Eigen headers. find_package(Eigen3 REQUIRED NO_MODULE) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) include_directories("/usr/local/include") -#Find the WebSocket++ library and Boost(provides the ASIO backend for Websocket++ and also -#provides the program_options argument parser).Only the `system` component of Boost is -#currently required. +# Find the WebSocket++ library and Boost (provides the ASIO backend for Websocket++ and also +# provides the program_options argument parser). Only the `system` component of Boost is +# currently required. find_package(websocketpp REQUIRED) find_package(Boost REQUIRED COMPONENTS system) -#Find Frozen, library used for constexpr immutable containers +# Find Frozen, library used for constexpr immutable containers find_package(frozen REQUIRED) -#Frozen is just an interface library, only containing headers and some CMake settings.It -#should be fine to "link" the library to every target, since if the headers are not included, -#it will not have any effect on size or performance.This will help us not have to worry about -#making sure the headers are available in every file that includes Constants.h, for example. +# Frozen is just an interface library, only containing headers and some CMake settings. It +# should be fine to "link" the library to every target, since if the headers are not included, +# it will not have any effect on size or performance. This will help us not have to worry about +# making sure the headers are available in every file that includes Constants.h, for example. link_libraries(frozen::frozen) -#Find argparse, library used for parsing command line arguments +# Find argparse, library used for parsing command line arguments find_package(argparse REQUIRED) -#Find the JSON library + +# Find the JSON library find_package(nlohmann_json 3.2.0 REQUIRED) -#Find the CAN library; contains packet and protocol definitions and does not -#actually require physical CAN to be present. +# Find the CAN library; contains packet and protocol definitions and does not +# actually require physical CAN to be present. FetchContent_Declare( HindsightCAN GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git @@ -103,7 +103,7 @@ FetchContent_Declare(LoguruGitRepo GIT_REPOSITORY "https://github.com/emilk/loguru" GIT_TAG "master" ) -#set any loguru compile - time flags before calling MakeAvailable() +# set any loguru compile-time flags before calling MakeAvailable() set(LOGURU_WITH_STREAMS TRUE) set(CMAKE_POSITION_INDEPENDENT_CODE ON) FetchContent_MakeAvailable(LoguruGitRepo) # defines target 'loguru::loguru' @@ -116,13 +116,13 @@ if(WITH_TESTS) find_package(Catch2 REQUIRED) endif() -#Libraries needed to interface with real - life sensors.If we aren't using the real world -#interface, these aren't needed. +# Libraries needed to interface with real-life sensors. If we aren't using the real world +# interface, these aren't needed. if(WORLD_INTERFACE STREQUAL "REAL") if(GPS STREQUAL "USB") -#Find pkg - config(needed for libgps, as it doesn't have CMake configuration) + # Find pkg-config (needed for libgps, as it doesn't have CMake configuration) find_package(PkgConfig REQUIRED) -#Find the libgps USB GPS library. + # Find the libgps USB GPS library. pkg_check_modules(libgps REQUIRED libgps) endif() endif() @@ -136,7 +136,7 @@ add_library(camera SHARED ) target_link_libraries(camera PUBLIC ${OpenCV_LIBS}) -#shared library for utility code +# shared library for utility code add_library(utils SHARED utils/core.cpp utils/random.cpp @@ -155,22 +155,22 @@ add_library(utils SHARED target_link_libraries(utils ${OpenCV_LIBS}) ## ====== CAN Interfaces ======= -#Stub CAN interface is used for the tests(and for the Rover if -#CAN is disabled) and the real CAN interface is used for the Rover if CAN is -#enabled. +# Stub CAN interface is used for the tests (and for the Rover if +# CAN is disabled) and the real CAN interface is used for the Rover if CAN is +# enabled. -#Common CAN source files +# Common CAN source files -#** DON'T MAKE THIS SHARED** -#Making this library shared causes some memory fuckery -#No clue why but CAN I / O goes to shit.Don't do it. +# **DON'T MAKE THIS SHARED** +# Making this library shared causes some memory fuckery +# No clue why but CAN I/O goes to shit. Don't do it. add_library(can_interface STATIC) target_sources(can_interface PRIVATE CAN/CANMotor.cpp CAN/CANUtils.cpp ) -#Hardware specific source files +# Hardware specific source files target_sources(can_interface PRIVATE CAN/CAN.cpp) target_link_libraries(can_interface PUBLIC @@ -195,20 +195,20 @@ else() endif() ## ====== World Interfaces ======= -#hardware - agnostic utilities and common code for world interface +# hardware-agnostic utilities and common code for world interface add_library(world_interface_core STATIC world_interface/gps_common_interface.cpp -#ar / Detector.cpp -#ar / MarkerSet.cpp -#ar / MarkerPattern.cpp -#ar / Tag.cpp -#ar / read_landmarks.cpp + #ar/Detector.cpp + #ar/MarkerSet.cpp + #ar/MarkerPattern.cpp + #ar/Tag.cpp + #ar/read_landmarks.cpp world_interface/motor/base_motor.cpp ) target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) target_link_libraries(world_interface_core PUBLIC ${vision_libs} opencv_aruco ${OpenCV_LIBS} utils camera) -#CAN library(above) requires some utilities from this +# CAN library (above) requires some utilities from this target_link_libraries(can_interface PUBLIC utils) add_library(stub_world_interface STATIC @@ -268,9 +268,9 @@ add_library(constants SHARED Constants.cpp) link_libraries(constants) add_executable(Rover Rover.cpp) -#** DON'T MAKE THIS SHARED** -#Making this library shared causes some memory fuckery -#No clue why but CAN I / O goes to shit.Don't do it. +# **DON'T MAKE THIS SHARED** +# Making this library shared causes some memory fuckery +# No clue why but CAN I/O goes to shit. Don't do it. add_library(rover_common STATIC Globals.cpp control_interface.cpp @@ -307,38 +307,38 @@ target_link_libraries(TunePID ${vision_libs}) if(WITH_TESTS) add_executable(tests Tests.cpp -#AR Detection tests -#ar / DetectorTests.cpp -#ar / MarkerSetTests.cpp -#Camera tests -#../ tests / camera / CameraParamsTests.cpp -#GPS tests + # AR Detection tests + # ar/DetectorTests.cpp + # ar/MarkerSetTests.cpp + # Camera tests + # ../tests/camera/CameraParamsTests.cpp + # GPS tests ../tests/gps/GPSDatumTest.cpp ../tests/gps/GPSConverterTest.cpp -#Controller tests + # Controller tests ../tests/control/TrapezoidalVelocityProfileTest.cpp ../tests/control/JacobianControllerTest.cpp ../tests/control/PIDControllerTest.cpp ../tests/control/PlanarArmControllerTest.cpp -#Kinematics tests + # Kinematics tests ../tests/kinematics/DiffDriveKinematicsTest.cpp ../tests/kinematics/SwerveDriveKinematicsTest.cpp ../tests/kinematics/PlanarArmFKTest.cpp ../tests/kinematics/FabrikSolver2DTest.cpp -#Filter tests + # Filter tests ../tests/filters/RollingAvgFilterTest.cpp ../tests/filters/EKFTest.cpp ../tests/filters/MultiSensorEKFTest.cpp ../tests/filters/StateSpaceUtilsTest.cpp -#Command tests + # Command tests ../tests/commands/DriveToWaypointCommandTest.cpp -#Util tests + # Util tests ../tests/util/CoreTest.cpp ../tests/util/DataTest.cpp ../tests/util/MathTest.cpp ../tests/util/TimeTest.cpp ../tests/util/SchedulerTest.cpp -#Protocol / teleop tests + # Protocol/teleop tests ../tests/kinematics/DiffWristKinematicsTest.cpp) target_link_libraries(tests From a8fe4f50cc0843150e1aacf4261df2e01dd53d41 Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 18:33:27 -0800 Subject: [PATCH 09/10] Fix frozen_map usage --- src/Constants.cpp | 4 ++-- src/Constants.h | 4 ++-- src/camera/Camera.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Constants.cpp b/src/Constants.cpp index 0eea9edc1..3edb963d2 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -51,12 +51,12 @@ const robot::types::CameraID MAST_CAMERA_ID = 40; const robot::types::CameraID WRIST_CAMERA_ID = 30; const robot::types::CameraID HAND_CAMERA_ID = 20; -constexpr frozen::unordered_map CAMERA_CONFIG_PATHS = { +constexpr frozen::unordered_map CAMERA_CONFIG_PATHS = { {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"}}; -constexpr frozen::unordered_map CAMERA_NAME_TO_ID = { +constexpr frozen::unordered_map CAMERA_NAME_TO_ID = { {"mast", MAST_CAMERA_ID}, {"wrist", WRIST_CAMERA_ID}, {"hand", HAND_CAMERA_ID}}; /** diff --git a/src/Constants.h b/src/Constants.h index 0822c0ffd..f2bf9dd73 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -57,9 +57,9 @@ extern const robot::types::CameraID MAST_CAMERA_ID; extern const robot::types::CameraID WRIST_CAMERA_ID; extern const robot::types::CameraID HAND_CAMERA_ID; -extern constexpr frozen::unordered_map +extern const frozen::unordered_map CAMERA_CONFIG_PATHS; -extern constexpr frozen::unordered_map CAMERA_NAME_TO_ID; +extern const frozen::unordered_map CAMERA_NAME_TO_ID; extern const uint16_t WS_SERVER_PORT; diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 7a3dd5a7d..54ddfb5e0 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -62,7 +62,7 @@ void Camera::init() { } std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { - cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id), cv::FileStorage::READ); + cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id).data(), cv::FileStorage::READ); if (!fs.isOpened()) { throw std::invalid_argument("Configuration file of camera ID" + std::to_string(camera_id) + " does not exist"); From e510bf8d0c402c4cd0152b54776d1763f02eae61 Mon Sep 17 00:00:00 2001 From: Isaac Date: Mon, 4 Nov 2024 18:36:52 -0800 Subject: [PATCH 10/10] Fix formatting --- src/Constants.cpp | 12 ++++++------ src/Constants.h | 3 ++- src/camera/Camera.cpp | 3 ++- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/Constants.cpp b/src/Constants.cpp index 3edb963d2..271246991 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -51,13 +51,13 @@ const robot::types::CameraID MAST_CAMERA_ID = 40; const robot::types::CameraID WRIST_CAMERA_ID = 30; const robot::types::CameraID HAND_CAMERA_ID = 20; -constexpr frozen::unordered_map CAMERA_CONFIG_PATHS = { - {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, - {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, - {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"}}; +constexpr frozen::unordered_map + CAMERA_CONFIG_PATHS = {{MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"}, + {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"}, + {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"}}; -constexpr frozen::unordered_map CAMERA_NAME_TO_ID = { - {"mast", MAST_CAMERA_ID}, {"wrist", WRIST_CAMERA_ID}, {"hand", HAND_CAMERA_ID}}; +constexpr frozen::unordered_map CAMERA_NAME_TO_ID = + {{"mast", MAST_CAMERA_ID}, {"wrist", WRIST_CAMERA_ID}, {"hand", HAND_CAMERA_ID}}; /** @deprecated No need for this constant once we fully switch over the Mission Control PlanViz diff --git a/src/Constants.h b/src/Constants.h index f2bf9dd73..20ad359eb 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -59,7 +59,8 @@ extern const robot::types::CameraID HAND_CAMERA_ID; extern const frozen::unordered_map CAMERA_CONFIG_PATHS; -extern const frozen::unordered_map CAMERA_NAME_TO_ID; +extern const frozen::unordered_map + CAMERA_NAME_TO_ID; extern const uint16_t WS_SERVER_PORT; diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 54ddfb5e0..046f4232b 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -62,7 +62,8 @@ void Camera::init() { } std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { - cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id).data(), cv::FileStorage::READ); + cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(camera_id).data(), + cv::FileStorage::READ); if (!fs.isOpened()) { throw std::invalid_argument("Configuration file of camera ID" + std::to_string(camera_id) + " does not exist");