diff --git a/camera-config/HandCameraCalibration.yml b/camera-config/HandCameraCalibration.yml index 4197a3811..da20fcf88 100644 --- a/camera-config/HandCameraCalibration.yml +++ b/camera-config/HandCameraCalibration.yml @@ -18,13 +18,14 @@ intrinsic_params: rows: 3 cols: 3 dt: d - data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0., - 6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ] + data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, + 0., 6.4776804982533793e+02, 2.4785660889077488e+02, + 0., 0., 1. ] distortion_coefficients: !!opencv-matrix rows: 5 cols: 1 dt: d - data: [ -4.5670452858419575e-01, 2.1512832208218641e-01, + data: [ -4.5670452858419575e-01,2.1512832208218641e-01, -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ] extrinsic_params: !!opencv-matrix rows: 4 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/rover-config/50-rover-cameras.rules b/rover-config/50-rover-cameras.rules index e18658723..1ca3e35a0 100644 --- a/rover-config/50-rover-cameras.rules +++ b/rover-config/50-rover-cameras.rules @@ -13,4 +13,4 @@ SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="6369", ATT SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video30" # Mast camera -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video40" +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video40" \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b55de633b..17adca147 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 @@ -132,8 +133,6 @@ endif() add_library(camera SHARED camera/Camera.cpp - camera/CameraParams.cpp - camera/CameraConfig.cpp ) target_link_libraries(camera PUBLIC ${OpenCV_LIBS}) @@ -199,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}) @@ -298,21 +297,21 @@ target_link_libraries(Rover ${vision_libs} ) -add_executable(TunePID TunePID.cpp) -target_link_libraries(TunePID - ${rover_libs} - can_interface -) -target_link_libraries(TunePID ${vision_libs}) +# add_executable(TunePID TunePID.cpp) +# target_link_libraries(TunePID +# ${rover_libs} +# can_interface +# ) +# target_link_libraries(TunePID ${vision_libs}) 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 + # ../tests/camera/CameraParamsTests.cpp # GPS tests ../tests/gps/GPSDatumTest.cpp ../tests/gps/GPSConverterTest.cpp @@ -352,9 +351,9 @@ if(WITH_TESTS) catch_discover_tests(tests) endif() -add_executable(gpsd_test - gps/gpsd_test.cpp) -target_link_libraries(gpsd_test gps) +# add_executable(gpsd_test +# gps/gpsd_test.cpp) +# target_link_libraries(gpsd_test gps) if (WORLD_INTERFACE STREQUAL "REAL") add_executable(LimitSwitchCalibration diff --git a/src/Constants.cpp b/src/Constants.cpp index e9d67ad22..61d81b30e 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -47,15 +47,19 @@ 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 robot::types::CameraID TEST_CAMERA_ID = 0; + +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/HandCameraConfig.yml"}, + {TEST_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 01c3e9530..86ada630b 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -53,15 +53,15 @@ 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 frozen::unordered_map + CAMERA_CONFIG_PATHS; +extern const frozen::unordered_map + CAMERA_NAME_TO_ID; + extern const uint16_t WS_SERVER_PORT; /** diff --git a/src/ar/ARTester.cpp b/src/ar/ARTester.cpp index 0f52006ac..cc5edc2cd 100644 --- a/src/ar/ARTester.cpp +++ b/src/ar/ARTester.cpp @@ -1,281 +1,35 @@ -#include "../camera/Camera.h" -#include "../camera/CameraConfig.h" -#include "../camera/CameraParams.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; -cam::CameraParams PARAMS; -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("c") || parser.get("c").empty()) { - std::cerr << "Error: camera configuration file is 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); - 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 = static_cast(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; - - std::cout << "Opening camera..." << std::endl; - 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); - } - - if (!open_success) { - std::cerr << "ERROR! Unable to open camera" << std::endl; - return EXIT_FAILURE; - } - - const int w = PARAMS.getImageSize().width; - const int h = PARAMS.getImageSize().height; - 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); - } - } +#include "../camera/Camera.h" - 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)); - } - } - } +const std::string WINDOW_NAME = "Test"; - if (show_grid) { - std::vector grid = projectGrid(imageSize, grid_spacing); - for (cv::Point2f pt : grid) { - cv::Point2f newPt(pt.x * static_cast(imageSize.width), - pt.y * static_cast(imageSize.height)); - cv::drawMarker(frame, newPt, cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 10, 1); - } - } +int main() { + cv::VideoCapture cap(0); + // cam::Camera c; + // c.open(40); - cv::imshow(WINDOW_NAME, frame); + // cap.open(0); - 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++; - } + cv::namedWindow(WINDOW_NAME); - return 0; -} + cv::Mat frame; + // uint32_t frame_num; -std::vector projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec) { - std::vector object_points; - std::vector image_points; + while (true) { + if (!cap.grab()) { continue; } - 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); + cap.retrieve(frame); - return image_points; -} + if (frame.empty()) { continue; } -std::vector projectGrid(cv::Size imageSize, int spacing) { - cv::Point2f center(static_cast(imageSize.width) / 2.0f, - static_cast(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 = static_cast(x); - yf = static_cast(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; -} + // c.next(frame, frame_num); + cv::imshow(WINDOW_NAME, frame); -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); -} + cv::waitKey(0); + } +} \ No newline at end of file diff --git a/src/ar/CMakeLists.txt b/src/ar/CMakeLists.txt index 2cf907468..7bec328b6 100644 --- a/src/ar/CMakeLists.txt +++ b/src/ar/CMakeLists.txt @@ -3,12 +3,6 @@ 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 - Tag.cpp) + ../camera/Camera.cpp) -target_link_libraries(ar_tester ${OpenCV_LIBS} opencv_aruco Threads::Threads utils) +target_link_libraries(ar_tester ${OpenCV_LIBS} opencv_aruco Threads::Threads utils) \ No newline at end of file diff --git a/src/ar/Detector.cpp b/src/ar/Detector.cpp deleted file mode 100644 index 6d8e4840e..000000000 --- a/src/ar/Detector.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include "Detector.h" - -#include "../camera/CameraParams.h" -#include "Tag.h" - -#include -#include -#include - -#include -#include -#include -#include - -namespace AR { - -Detector::Detector() { -} - -Detector::Detector(std::shared_ptr marker_set, cam::CameraParams camera_params, - cv::Ptr detector_params) - : marker_set_(marker_set), camera_params_(camera_params), - 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 3e8534e83..000000000 --- a/src/ar/Detector.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include "../camera/CameraParams.h" -#include "MarkerSet.h" -#include "Tag.h" - -#include -#include - -#include -#include - -namespace AR { - -/** - @addtogroup 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, - std::vector>* rejected, - bool undistort); - -public: - Detector(); - Detector(std::shared_ptr marker_set, cam::CameraParams camera_params, - 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 diff --git a/src/camera/Camera.cpp b/src/camera/Camera.cpp index 84d06aa73..046f4232b 100644 --- a/src/camera/Camera.cpp +++ b/src/camera/Camera.cpp @@ -1,9 +1,11 @@ #include "Camera.h" -#include "CameraConfig.h" +#include "../Constants.h" #include +#include + using cv::Mat; using cv::Size; using std::string; @@ -13,63 +15,40 @@ using namespace robot::types; namespace cam { Camera::Camera() : _frame(std::make_shared()), _frame_num(std::make_shared(0)), - _frame_time(std::make_shared()), - _capture(std::make_shared()), + _frame_time(std::make_shared()), _capture(), _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) { - if (*_running) { - return false; - } - _capture_lock->lock(); - bool result = this->_capture->open(camera_id); - _capture_lock->unlock(); - LOG_F(INFO, "Opening camera %d... %s", camera_id, result ? "success" : "failed"); - this->_intrinsic_params = intrinsic_params; - init(extrinsic_params); - return result; +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)) { + this->open(camera_id); } -bool Camera::open(string filename, CameraParams intrinsic_params, 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), _running(other._running) {} + +bool Camera::open(robot::types::CameraID camera_id) { if (*_running) { return false; } _capture_lock->lock(); - bool result = this->_capture->open(filename); + 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(); - this->_intrinsic_params = intrinsic_params; - init(extrinsic_params); + LOG_F(INFO, "Opening camera %d... %s", camera_id, result ? "success" : "failed"); + 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) { @@ -82,56 +61,63 @@ 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) {} +std::stringstream Camera::GStreamerFromFile(robot::types::CameraID camera_id) { + 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"); + } -bool Camera::openFromConfigFile(std::string filename) { - CameraConfig cfg = readConfigFromFile(filename); + std::stringstream gstr_ss; + gstr_ss << "v4l2src device=/dev/video" << camera_id << " ! "; - cv::Mat extrinsics; - if (cfg.extrinsicParams) { - extrinsics = cfg.extrinsicParams.value(); - } + 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"; - CameraParams intrinsics; - if (cfg.intrinsicParams) { - intrinsics = cfg.intrinsicParams.value(); - } + _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(); - 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"); - } + gstr_ss << "image/" << fs[KEY_FORMAT].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"; + + 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); - 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(); @@ -174,22 +160,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; } @@ -198,6 +168,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 9d21c30a5..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,21 +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. + @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. */ @@ -204,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. @@ -228,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 f83ea50e1..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 = static_cast(file_node[KEY_IMAGE_WIDTH]); - h = static_cast(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 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/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/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index 5bcf2fc32..278a65ace 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::WRIST_CAMERA_ID); + openCamera(Constants::HAND_CAMERA_ID); } + } // namespace void world_interface_init( @@ -185,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 cbeb1ae42..d78ccbd0f 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; @@ -90,33 +87,20 @@ void sendJSON(const json& obj) { wsServer->get().sendJSON(PROTOCOL_PATH, obj); } -static void openCamera(CameraID cam, std::optional> list1d = std::nullopt, - uint8_t fps = 20, 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() { - 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() { @@ -147,13 +131,13 @@ void handleIMU(json msg) { } void handleCamFrame(json msg) { - std::string cam = msg["camera"]; + robot::types::CameraID id = msg["cameraID"]; 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 +145,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) { @@ -316,34 +300,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 b9507cf98..c710c264f 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -11,12 +11,6 @@ #include #include -// 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 * @brief Collection of functions that allows interfacing with the hardware and the world. @@ -106,22 +100,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.