From cb74553579032006722efd173e402b65b2d8f32d Mon Sep 17 00:00:00 2001 From: Sriram Kodey <52345525+SriramKodey@users.noreply.github.com> Date: Sun, 28 Jan 2024 01:08:37 +0530 Subject: [PATCH] change AR_CAMERA to MAST_CAMERA (#281) * change AR_CAMERA to MAST_CAMERA * Formatting fix * Formatting fix * Fix formatting --------- Co-authored-by: Quinn Pfeifer <72775766+QuinnP2910@users.noreply.github.com> --- src/Constants.h | 4 ++-- src/ar/read_landmarks.cpp | 11 ++++++----- src/world_interface/real_world_interface.cpp | 2 +- src/world_interface/simulator_interface.cpp | 6 +++--- tests/camera/CameraParamsTests.cpp | 2 +- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/Constants.h b/src/Constants.h index 17e82fb4d..fbece020c 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -58,8 +58,8 @@ const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE) // TODO: We need to recalibrate the camera, since we replaced it with a different one. // TODO: rename cameras (in MC as well) as appropriate -constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml"; -const robot::types::CameraID AR_CAMERA_ID = "front"; // TODO: replace with real camera name +constexpr const char* MAST_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml"; +const robot::types::CameraID MAST_CAMERA_ID = "front"; // TODO: replace with real camera name constexpr const char* FOREARM_CAMERA_CONFIG_PATH = "../camera-config/ForearmCameraCalibration.yml"; diff --git a/src/ar/read_landmarks.cpp b/src/ar/read_landmarks.cpp index 0d1f5c43d..72ef9360e 100644 --- a/src/ar/read_landmarks.cpp +++ b/src/ar/read_landmarks.cpp @@ -34,8 +34,8 @@ void detectLandmarksLoop() { cv::Mat frame; uint32_t last_frame_no = 0; while (true) { - if (robot::hasNewCameraFrame(Constants::AR_CAMERA_ID, last_frame_no)) { - auto camData = robot::readCamera(Constants::AR_CAMERA_ID); + 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(); @@ -65,7 +65,8 @@ void detectLandmarksLoop() { 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::AR_CAMERA_ID); + 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); @@ -99,7 +100,7 @@ void detectLandmarksLoop() { } bool initializeLandmarkDetection() { - auto intrinsic = robot::getCameraIntrinsicParams(Constants::AR_CAMERA_ID); + auto intrinsic = robot::getCameraIntrinsicParams(Constants::MAST_CAMERA_ID); if (intrinsic) { ar_detector = Detector(Markers::URC_MARKERS(), intrinsic.value()); landmark_thread = std::thread(&detectLandmarksLoop); @@ -108,7 +109,7 @@ bool initializeLandmarkDetection() { "cannot be performed."); return false; } - if (!robot::getCameraExtrinsicParams(Constants::AR_CAMERA_ID)) { + 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"); } diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index 6ef6f196b..484702019 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -128,7 +128,7 @@ void openCamera(CameraID camID, const char* cameraPath) { } void setupCameras() { - openCamera(Constants::AR_CAMERA_ID, Constants::AR_CAMERA_CONFIG_PATH); + openCamera(Constants::MAST_CAMERA_ID, Constants::MAST_CAMERA_CONFIG_PATH); openCamera(Constants::HAND_CAMERA_ID, Constants::HAND_CAMERA_CONFIG_PATH); } } // namespace diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 88e6155eb..0312fe423 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -104,9 +104,9 @@ static void openCamera(CameraID cam, std::optional> list1d = } void initCameras() { - auto cfg = cam::readConfigFromFile(Constants::AR_CAMERA_CONFIG_PATH); - cameraConfigMap[Constants::AR_CAMERA_ID] = cfg; - // openCamera(Constants::AR_CAMERA_ID); + auto cfg = cam::readConfigFromFile(Constants::MAST_CAMERA_CONFIG_PATH); + cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg; + // openCamera(Constants::MAST_CAMERA_ID); openCamera("front", cfg.intrinsicParams->getIntrinsicList()); openCamera("rear", cfg.intrinsicParams->getIntrinsicList()); openCamera("upperArm", cfg.intrinsicParams->getIntrinsicList()); diff --git a/tests/camera/CameraParamsTests.cpp b/tests/camera/CameraParamsTests.cpp index ce7136994..d70239c4b 100644 --- a/tests/camera/CameraParamsTests.cpp +++ b/tests/camera/CameraParamsTests.cpp @@ -79,7 +79,7 @@ TEST_CASE("Serialization works", "[camera][camera_params]") { TEST_CASE("List Conversation Works"){ std::unordered_map cameraConfigMap; auto cfg = cam::readConfigFromFile("../camera-config/global.yml"); - cameraConfigMap[Constants::AR_CAMERA_ID] = cfg; + cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg; std::vector x1 = cfg.intrinsicParams->getIntrinsicList(); std::vector x1test {8.2342295124138286e+02, 0., 3.0932599345522056e+02, 0., 8.2343518498999936e+02, 2.5535163960018482e+02, 0., 0., 1.};