From 3f9ac56341dc11e79523274052d4931f8ba39f79 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Tue, 26 Sep 2023 01:21:46 +0000 Subject: [PATCH 01/11] Implemented broken futures. --- CONTRIBUTING.md | 1 + examples/vision/OpenBasicCam.hpp | 4 +- src/interfaces/Camera.hpp | 5 +- src/main.cpp | 2 +- src/threads/CameraHandlerThread.cpp | 48 +- src/threads/CameraHandlerThread.h | 7 +- src/util/vision/FetchContainers.hpp | 29 +- src/vision/cameras/BasicCam.cpp | 36 +- src/vision/cameras/BasicCam.h | 4 +- src/vision/cameras/ZEDCam.cpp | 3203 ++++++++++++++------------- src/vision/cameras/ZEDCam.h | 242 +- 11 files changed, 1792 insertions(+), 1789 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index d658aab5..88d34631 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -93,6 +93,7 @@ Examples: `int g_nExampleGlobalInteger` or `int m_nExampleMemberInteger` - Lock > `lk` > Example: `lock lkExampleUseOfLock` - Struct > `st` > Example: `StructName stExampleUseOfStruct` - Future > `fu` > Example: `future fuExampleUseOfFuture` +- Promise > `pm` > Example: `promise pmExampleUseIfPromise` #### External Types - OpenCV > `cv` > Example: `cv::Mat cvExampleMat` diff --git a/examples/vision/OpenBasicCam.hpp b/examples/vision/OpenBasicCam.hpp index 31907f97..67763191 100644 --- a/examples/vision/OpenBasicCam.hpp +++ b/examples/vision/OpenBasicCam.hpp @@ -34,7 +34,9 @@ void RunExample() while (true) { // Grab normal frame from camera. - if (TestCamera1->GrabFrame(cvNormalFrame1)) + std::future fuTest = TestCamera1->GrabFrame(cvNormalFrame1); + cvNormalFrame1 = fuTest.get(); + if (!cvNormalFrame1.empty()) { // Print info. LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); diff --git a/src/interfaces/Camera.hpp b/src/interfaces/Camera.hpp index 48046a9a..c80820ca 100644 --- a/src/interfaces/Camera.hpp +++ b/src/interfaces/Camera.hpp @@ -12,6 +12,7 @@ #define CAMERA_HPP #include "../util/IPS.hpp" +#include // Declare global/file-scope enumerator. enum PIXEL_FORMATS @@ -67,8 +68,8 @@ class Camera double m_dPropVerticalFOV; // Declare interface class pure virtual functions. (These must be overriden by inheritor.) - virtual bool GrabFrame(T& tFrame) = 0; // This is where the code to retrieve an image from the camera is put. - virtual bool GetCameraIsOpen() = 0; // This is where the code to check if the camera is current open goes. + virtual std::future GrabFrame(T& tFrame) = 0; // This is where the code to retrieve an image from the camera is put. + virtual bool GetCameraIsOpen() = 0; // This is where the code to check if the camera is current open goes. // Declare protected object pointers. IPS m_IPS = IPS(); diff --git a/src/main.cpp b/src/main.cpp index 26d79db3..507ac33c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,7 +13,7 @@ #include #include -#include "../examples/vision/OpenZEDCam.hpp" +#include "../examples/vision/OpenBasicCam.hpp" #include "./AutonomyGlobals.h" #include "./AutonomyLogging.h" #include "./interfaces/StateMachine.hpp" diff --git a/src/threads/CameraHandlerThread.cpp b/src/threads/CameraHandlerThread.cpp index c7cefd2c..3dca036e 100644 --- a/src/threads/CameraHandlerThread.cpp +++ b/src/threads/CameraHandlerThread.cpp @@ -21,16 +21,16 @@ CameraHandlerThread::CameraHandlerThread() { // Initialize main ZED camera. - m_pMainCam = new ZEDCam(constants::ZED_MAINCAM_RESOLUTIONX, - constants::ZED_MAINCAM_RESOLUTIONY, - constants::ZED_MAINCAM_FPS, - constants::ZED_MAINCAM_HORIZONTAL_FOV, - constants::ZED_MAINCAM_VERTICAL_FOV, - constants::ZED_DEFAULT_MINIMUM_DISTANCE, - constants::ZED_DEFAULT_MAXIMUM_DISTANCE, - constants::ZED_MAINCAM_USE_GPU_MAT, - constants::ZED_MAINCAM_USE_HALF_PRECISION_DEPTH, - constants::ZED_MAINCAN_SERIAL); + // m_pMainCam = new ZEDCam(constants::ZED_MAINCAM_RESOLUTIONX, + // constants::ZED_MAINCAM_RESOLUTIONY, + // constants::ZED_MAINCAM_FPS, + // constants::ZED_MAINCAM_HORIZONTAL_FOV, + // constants::ZED_MAINCAM_VERTICAL_FOV, + // constants::ZED_DEFAULT_MINIMUM_DISTANCE, + // constants::ZED_DEFAULT_MAXIMUM_DISTANCE, + // constants::ZED_MAINCAM_USE_GPU_MAT, + // constants::ZED_MAINCAM_USE_HALF_PRECISION_DEPTH, + // constants::ZED_MAINCAN_SERIAL); // Initialize Left acruco eye. m_pLeftCam = new BasicCam(constants::BASIC_LEFTCAM_INDEX, @@ -52,17 +52,17 @@ CameraHandlerThread::CameraHandlerThread() CameraHandlerThread::~CameraHandlerThread() { // Signal and wait for cameras to stop. - m_pMainCam->RequestStop(); + // m_pMainCam->RequestStop(); m_pLeftCam->RequestStop(); - m_pMainCam->Join(); + // m_pMainCam->Join(); m_pLeftCam->Join(); // Delete dynamic memory. - delete m_pMainCam; + // delete m_pMainCam; delete m_pLeftCam; // Set dangling pointers to nullptr. - m_pMainCam = nullptr; + // m_pMainCam = nullptr; m_pLeftCam = nullptr; } @@ -76,7 +76,7 @@ CameraHandlerThread::~CameraHandlerThread() void CameraHandlerThread::StartAllCameras() { // Start ZED cams. - m_pMainCam->Start(); + // m_pMainCam->Start(); // Start basic cams. m_pLeftCam->Start(); @@ -91,15 +91,15 @@ void CameraHandlerThread::StartAllCameras() * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-01 ******************************************************************************/ -ZEDCam* CameraHandlerThread::GetZED(ZEDCamName eCameraName) -{ - // Determine which camera should be returned. - switch (eCameraName) - { - case eHeadMainCam: return m_pMainCam; - default: return m_pMainCam; - } -} +// ZEDCam* CameraHandlerThread::GetZED(ZEDCamName eCameraName) +// { +// // Determine which camera should be returned. +// switch (eCameraName) +// { +// case eHeadMainCam: return m_pMainCam; +// default: return m_pMainCam; +// } +// } /****************************************************************************** * @brief Accessor for Basic cameras. diff --git a/src/threads/CameraHandlerThread.h b/src/threads/CameraHandlerThread.h index 4a30ce90..6e3d06da 100644 --- a/src/threads/CameraHandlerThread.h +++ b/src/threads/CameraHandlerThread.h @@ -15,7 +15,8 @@ #include "../interfaces/AutonomyThread.hpp" #include "../vision/cameras/BasicCam.h" -#include "../vision/cameras/ZEDCam.h" + +// #include "../vision/cameras/ZEDCam.h" /****************************************************************************** * @brief The CameraHandlerThread class is responsible for managing all of the @@ -31,7 +32,7 @@ class CameraHandlerThread { private: // Declare private class member variables. - ZEDCam* m_pMainCam; + // ZEDCam* m_pMainCam; BasicCam* m_pLeftCam; public: @@ -53,7 +54,7 @@ class CameraHandlerThread void StartAllCameras(); // Accessors. - ZEDCam* GetZED(ZEDCamName eCameraName); + // ZEDCam* GetZED(ZEDCamName eCameraName); BasicCam* GetBasicCam(BasicCamName eCameraName); }; diff --git a/src/util/vision/FetchContainers.hpp b/src/util/vision/FetchContainers.hpp index ea575c3c..198cb80b 100644 --- a/src/util/vision/FetchContainers.hpp +++ b/src/util/vision/FetchContainers.hpp @@ -14,6 +14,7 @@ #ifndef FETCH_CONTAINERS_HPP #define FETCH_CONTAINERS_HPP +#include #include #include @@ -42,10 +43,9 @@ namespace containers { public: // Declare and define public struct member variables. - std::condition_variable cdMatWriteSuccess; - std::mutex muConditionMutex; - T& tFrame; + std::shared_ptr pFrame; PIXEL_FORMATS eFrameType; + std::shared_ptr> pCopiedFrame; /****************************************************************************** * @brief Construct a new Frame Fetch Container object. @@ -57,7 +57,28 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : tFrame(tFrame), eFrameType(eFrameType) {} + FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : + pFrame(std::make_shared(tFrame)), eFrameType(eFrameType), pCopiedFrame(std::make_shared>()) + {} + + FrameFetchContainer(const FrameFetchContainer& stOtherFrameContainer) : + pFrame(stOtherFrameContainer.pFrame), eFrameType(stOtherFrameContainer.eFrameType), pCopiedFrame(stOtherFrameContainer.pCopiedFrame) + {} + + FrameFetchContainer& operator=(const FrameFetchContainer& stOtherFrameContainer) + { + // Check if the passed in container is the same as this one. + if (this != &stOtherFrameContainer) + { + // Copy struct attributes. + this->pFrame = stOtherFrameContainer.pFrame; + this->eFrameType = stOtherFrameContainer.eFrameType; + this->pCopiedFrame = stOtherFrameContainer.pCopiedFrame; + } + + // Return pointer to this object which now contains the copied values. + return *this; + } }; /****************************************************************************** diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index be1d8aeb..0f45f9c9 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -196,20 +196,14 @@ void BasicCam::PooledLinearCode() if (!m_qFrameCopySchedule.empty()) { // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); + containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); // Pop out of queue. m_qFrameCopySchedule.pop(); // Release lock. lkFrameQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); // Copy frame to data container. - stContainer.tFrame = m_cvFrame.clone(); - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + *(stContainer.pFrame) = m_cvFrame.clone(); } } @@ -225,10 +219,10 @@ void BasicCam::PooledLinearCode() * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ -bool BasicCam::GrabFrame(cv::Mat& cvFrame) +std::future BasicCam::GrabFrame(cv::Mat& cvFrame) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvFrame, m_ePropPixelFormat); + containers::FrameFetchContainer stContainer(cvFrame, m_ePropPixelFormat); // Acquire lock on frame copy queue. std::unique_lock lkScheduler(m_muPoolScheduleMutex); @@ -237,26 +231,8 @@ bool BasicCam::GrabFrame(cv::Mat& cvFrame) // Release lock on the frame schedule queue. lkScheduler.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrame->get_future(); } /****************************************************************************** diff --git a/src/vision/cameras/BasicCam.h b/src/vision/cameras/BasicCam.h index 499bef2d..b8ef99c1 100644 --- a/src/vision/cameras/BasicCam.h +++ b/src/vision/cameras/BasicCam.h @@ -39,7 +39,7 @@ class BasicCam : public Camera, public AutonomyThread const double dPropHorizontalFOV, const double dPropVerticalFOV); ~BasicCam(); - bool GrabFrame(cv::Mat& cvFrame) override; + std::future GrabFrame(cv::Mat& cvFrame) override; ///////////////////////////////////////// // Getters. @@ -62,7 +62,7 @@ class BasicCam : public Camera, public AutonomyThread cv::Mat m_cvFrame; // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue>> m_qFrameCopySchedule; + std::queue> m_qFrameCopySchedule; std::shared_mutex m_muPoolScheduleMutex; std::mutex m_muFrameCopyMutex; diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 50eea642..5f93b4c1 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -1,1601 +1,1602 @@ -/****************************************************************************** - * @brief Implements the ZEDCam class. - * - * @file ZEDCam.cpp - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - * - * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved - ******************************************************************************/ - -#include "ZEDCam.h" -#include "../../AutonomyLogging.h" -#include "../../util/vision/ImageOperations.hpp" - -/****************************************************************************** - * @brief This struct is part of the ZEDCam class and is used as a container for all - * bounding box data that is going to be passed to the zed api via the ZEDCam's - * TrackCustomBoxObjects() method. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-29 - ******************************************************************************/ -struct ZEDCam::ZedObjectData - -{ - private: - // Declare and define private struct member variables. - std::string szObjectUUID = sl::generate_unique_id().get(); // This will automatically generate a guaranteed unique id so the object is traceable. - - // Declare a private struct for holding point data. - /****************************************************************************** - * @brief This struct is internal to the ZedObjectData struct is used to store - * an X and Y value for the corners of a bounding box. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-29 - ******************************************************************************/ - struct Corner - { - public: - // Declare public struct member variables. - unsigned int nX; - unsigned int nY; - }; - - public: - // Declare and define public struct member variables. - struct Corner CornerTL; // The top left corner of the bounding box. - struct Corner CornerTR; // The top right corner of the bounding box. - struct Corner CornerBL; // The bottom left corner of the bounding box. - struct Corner CornerBR; // The bottom right corner of bounding box. - int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the same. - float fConfidence; // This info is passed through from your detection algorithm and will help improve tracking by throwing out bad detections. - // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class number - // to avoid issues. - bool bObjectRemainsOnFloorPlane = false; - - // Declare and define public struct getters. - std::string GetObjectUUID() { return szObjectUUID; }; -}; - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/****************************************************************************** - * @brief Construct a new Zed Cam:: Zed Cam object. - * - * @param nPropResolutionX - X res of camera. Must be smaller than ZED_BASE_RESOLUTION. - * @param nPropResolutionY - Y res of camera. Must be smaller than ZED_BASE_RESOLUTION. - * @param nPropFramesPerSecond - FPS camera is running at. - * @param dPropHorizontalFOV - The horizontal field of view. - * @param dPropVerticalFOV - The vertical field of view. - * @param fMinSenseDistance - The minimum distance to include in depth measures. - * @param fMaxSenseDistance - The maximim distance to include in depth measures. - * @param bMemTypeGPU - Whether or not to use the GPU memory for operations. - * @param unCameraSerialNumber - The serial number of the camera to open. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -ZEDCam::ZEDCam(const int nPropResolutionX, - const int nPropResolutionY, - const int nPropFramesPerSecond, - const double dPropHorizontalFOV, - const double dPropVerticalFOV, - const float fMinSenseDistance, - const float fMaxSenseDistance, - const bool bMemTypeGPU, - const bool bUseHalfDepthPrecision, - const unsigned int unCameraSerialNumber) : - Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) -{ - // Assign member variables. - bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; - bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; - m_unCameraSerialNumber = unCameraSerialNumber; - - // Setup camera params. - m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; - m_slCameraParams.camera_fps = nPropFramesPerSecond; - m_slCameraParams.coordinate_units = constants::ZED_MEASURE_UNITS; - m_slCameraParams.coordinate_system = constants::ZED_COORD_SYSTEM; - m_slCameraParams.depth_mode = constants::ZED_DEPTH_MODE; - m_slCameraParams.depth_minimum_distance = fMinSenseDistance; - m_slCameraParams.depth_maximum_distance = fMaxSenseDistance; - m_slCameraParams.depth_stabilization = constants::ZED_DEPTH_STABILIZATION; - // Only set serial number if necessary. - if (unCameraSerialNumber != static_cast(0)) - { - m_slCameraParams.input.setFromSerialNumber(unCameraSerialNumber); - } - - // Setup camera runtime params. - m_slRuntimeParams.enable_fill_mode = constants::ZED_SENSING_FILL; - - // Setup positional tracking parameters. - m_slPoseTrackingParams.mode = constants::ZED_POSETRACK_MODE; - m_slPoseTrackingParams.enable_area_memory = constants::ZED_POSETRACK_AREA_MEMORY; - m_slPoseTrackingParams.enable_pose_smoothing = constants::ZED_POSETRACK_POSE_SMOOTHING; - m_slPoseTrackingParams.set_floor_as_origin = constants::ZED_POSETRACK_FLOOR_IS_ORIGIN; - m_slPoseTrackingParams.enable_imu_fusion = constants::ZED_POSETRACK_ENABLE_IMU_FUSION; - m_slPoseTrackingParams.depth_min_range = constants::ZED_POSETRACK_USABLE_DEPTH_MIN; - m_slPoseTrackingParams.set_gravity_as_origin = constants::ZED_POSETRACK_USE_GRAVITY_ORIGIN; - - // Setup spatial mapping parameters. - m_slSpatialMappingParams.map_type = constants::ZED_MAPPING_TYPE; - m_slSpatialMappingParams.resolution_meter = constants::ZED_MAPPING_RESOLUTION_METER; - m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); - m_slSpatialMappingParams.save_texture = true; - m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; - m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; - - // Setup object detection/tracking parameters. - m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; - m_slObjectDetectionParams.image_sync = constants::ZED_OBJDETECTION_IMG_SYNC; - m_slObjectDetectionParams.enable_tracking = constants::ZED_OBJDETECTION_TRACK_OBJ; - m_slObjectDetectionParams.enable_segmentation = constants::ZED_OBJDETECTION_SEGMENTATION; - m_slObjectDetectionParams.filtering_mode = constants::ZED_OBJDETECTION_FILTERING; - m_slObjectDetectionParams.prediction_timeout_s = constants::ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT; - // Setup object detection/tracking batch parameters. - m_slObjectDetectionBatchParams.enable = false; - m_slObjectDetectionBatchParams.id_retention_time = constants::ZED_OBJDETECTION_BATCH_RETENTION_TIME; - m_slObjectDetectionBatchParams.latency = constants::ZED_OBJDETECTION_BATCH_LATENCY; - m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; - - // Attempt to open camera. - sl::ERROR_CODE slReturnCode = m_slCamera.open(m_slCameraParams); - // Check if the camera was successfully opened. - if (m_slCamera.isOpened()) - { - // Submit logger message. - LOG_DEBUG(g_qSharedLogger, - "{} stereo camera with serial number {} has been succsessfully opened.", - this->GetCameraModel(), - m_slCamera.getCameraInformation().serial_number); - } - else - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Unable to open stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } -} - -/****************************************************************************** - * @brief Destroy the Zed Cam:: Zed Cam object. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -ZEDCam::~ZEDCam() -{ - // Stop threaded code. - this->RequestStop(); - this->Join(); - - // Close the ZEDCam. - m_slCamera.close(); -} - -/****************************************************************************** - * @brief The code inside this private method runs in a seperate thread, but still - * has access to this*. This method continuously calls the grab() function of - * the ZEDSDK, which updates all frames (RGB, depth, cloud) and all other data - * such as positional and spatial mapping. Then a thread pool is started and joined - * once per iteration to mass copy the frames and/or measure to any other thread - * waiting in the queues. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-09-01 - ******************************************************************************/ -void ZEDCam::ThreadedContinuousCode() -{ - // Check if camera is opened. - if (!m_slCamera.isOpened()) - { - // Shutdown threads for this ZEDCam. - this->RequestStop(); - // Submit logger message. - LOG_CRITICAL(g_qSharedLogger, - "Camera start was attempted for camera at {}, but camera never properly opened or it has been closed/rebooted!", - m_unCameraSerialNumber); - } - else - { - // Acquire write lock for camera object. - std::unique_lock lkSharedCameraLock(m_muCameraMutex); - // Call generalized update method of zed api. - sl::ERROR_CODE slReturnCode = m_slCamera.grab(m_slRuntimeParams); - // Check if new frame was computed successfully. - if (slReturnCode == sl::ERROR_CODE::SUCCESS) - { - // Grab regular image and store it in member variable. - slReturnCode = m_slCamera.retrieveImage(m_slFrame, constants::ZED_RETRIEVE_VIEW, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new frame image for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Grab depth measure and store it in member variable. - slReturnCode = m_slCamera.retrieveMeasure(m_slDepthMeasure, m_slDepthMeasureType, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new depth measure for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Grab depth grayscale image and store it in member variable. - slReturnCode = m_slCamera.retrieveImage(m_slDepthImage, sl::VIEW::DEPTH, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new depth image for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Grab regular resized image and store it in member variable. - slReturnCode = m_slCamera.retrieveMeasure(m_slPointCloud, sl::MEASURE::XYZ, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new point cloud for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Get and store the SensorData object from the camera. Get data from the most recent image grab. - // Using TIME_REFERENCE::CURRENT requires high rate polling and can introduce error as the most recent - // IMU data could be in the future of the camera image. - slReturnCode = m_slCamera.getSensorsData(m_slSensorData, sl::TIME_REFERENCE::IMAGE); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new sensors data for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Check if positional tracking is enabled. - if (m_slCamera.isPositionalTrackingEnabled()) - { - // Get the current pose of the camera. - sl::POSITIONAL_TRACKING_STATE slPoseTrackReturnCode = m_slCamera.getPosition(m_slCameraPose, sl::REFERENCE_FRAME::WORLD); - // Check that the regular frame was retrieved successfully. - if (slPoseTrackReturnCode != sl::POSITIONAL_TRACKING_STATE::OK) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new positional tracking pose for stereo camera {} ({})! sl::POSITIONAL_TRACKING_STATE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slPoseTrackReturnCode).get()); - } - } - // Check if object detection is enabled. - if (m_slCamera.isObjectDetectionEnabled()) - { - // Get updated objects from camera. - slReturnCode = m_slCamera.retrieveObjects(m_slDetectedObjects); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Check if batched object data is enabled. - if (m_slObjectDetectionBatchParams.enable) - { - // Get updated batched objects from camera. - slReturnCode = m_slCamera.getObjectsBatch(m_slDetectedObjectsBatched); - // Check that the regular frame was retrieved successfully. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Unable to retrieve new batched object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - } - } - // Release camera lock. - lkSharedCameraLock.unlock(); - - // Call FPS tick. - m_IPS.Tick(); - } - else - { - // Release camera lock. - lkSharedCameraLock.unlock(); - - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Unable to update stereo camera {} ({}) frames, measurements, and sensors! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Acquire a shared_lock on the frame copy queue. - std::shared_lock lkSchedulers(m_muPoolScheduleMutex); - // Check if the frame copy queue is empty. - if (!m_qFrameCopySchedule.empty() || !m_qGPUFrameCopySchedule.empty() || !m_qCustomBoxInjestSchedule.empty() || !m_qPoseCopySchedule.empty() || - !m_qIMUDataCopySchedule.empty() || !m_qObjectDataCopySchedule.empty() || !m_qObjectBatchedDataCopySchedule.empty()) - { - // Find the queue with the longest length. - size_t siMaxQueueLength = std::max({m_qFrameCopySchedule.size(), - m_qGPUFrameCopySchedule.size(), - m_qCustomBoxInjestSchedule.size(), - m_qPoseCopySchedule.size(), - m_qIMUDataCopySchedule.size(), - m_qObjectDataCopySchedule.size(), - m_qObjectBatchedDataCopySchedule.size()}); - - // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. - this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); - // Wait for thread pool to finish. - this->JoinPool(); - // Release lock on frame copy queue. - lkSchedulers.unlock(); - } - } -} - -/****************************************************************************** - * @brief This method holds the code that is ran in the thread pool started by - * the ThreadedLinearCode() method. It copies the data from the different - * data objects to references of the same type stored in a vector queued up by the - * Grab methods. - * - * - * @author ClayJay3 (claytonraycowen@gmail.com) - * @date 2023-09-08 - ******************************************************************************/ -void ZEDCam::PooledLinearCode() -{ - ///////////////////////////// - // Frame queue. - ///////////////////////////// - // Check if we are using CPU or GPU mats. - if (m_slMemoryType == sl::MEM::CPU) - { - // Aqcuire mutex for getting frames out of the queue. - std::unique_lock lkFrameQueue(m_muFrameCopyMutex); - // Check if the queue is empty. - if (!m_qFrameCopySchedule.empty()) - { - // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); - // Pop out of queue. - m_qFrameCopySchedule.pop(); - // Release lock. - lkFrameQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - - // Determine which frame should be copied. - switch (stContainer.eFrameType) - { - case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; - case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; - case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; - case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; - default: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; - } - - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } - } - // Use GPU mat. - else - { - // Check if the queue is empty. - if (!m_qGPUFrameCopySchedule.empty()) - { - // Aqcuire mutex for getting frames out of the queue. - std::unique_lock lkFrameQueue(m_muFrameCopyMutex); - // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qGPUFrameCopySchedule.front(); - // Pop out of queue. - m_qGPUFrameCopySchedule.pop(); - // Release lock. - lkFrameQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - - // Determine which frame should be copied. - switch (stContainer.eFrameType) - { - case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; - case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; - case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; - case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; - default: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; - } - - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } - } - - ///////////////////////////// - // Pose queue. - ///////////////////////////// - // Aqcuire mutex for getting frames out of the pose queue. - std::unique_lock lkPoseQueue(m_muPoseCopyMutex); - // Check if the queue is empty. - if (!m_qPoseCopySchedule.empty()) - { - // Get frame container out of queue. - containers::DataFetchContainer& stContainer = m_qPoseCopySchedule.front(); - // Pop out of queue. - m_qPoseCopySchedule.pop(); - // Release lock. - lkPoseQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Copy pose. - stContainer.tData = sl::Pose(m_slCameraPose); - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } - - ///////////////////////////// - // IMU data queue. - ///////////////////////////// - // Aqcuire mutex for getting frames out of the pose queue. - std::unique_lock lkIMUQueue(m_muIMUDataCopyMutex); - // Check if the queue is empty. - if (!m_qIMUDataCopySchedule.empty()) - { - // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qIMUDataCopySchedule.front(); - // Pop out of queue. - m_qIMUDataCopySchedule.pop(); - // Release lock. - lkIMUQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - - // Get IMU orientation in degrees. - sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); - // Get IMU linear acceleration. - sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; - // Repackage angles and accels into vector. - stContainer.tData.emplace_back(slAngles.x); - stContainer.tData.emplace_back(slAngles.y); - stContainer.tData.emplace_back(slAngles.z); - stContainer.tData.emplace_back(slLinearAccels.x); - stContainer.tData.emplace_back(slLinearAccels.y); - stContainer.tData.emplace_back(slLinearAccels.z); - - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } - - ///////////////////////////// - // ObjectData queue. - ///////////////////////////// - // Aqcuire mutex for getting frames out of the pose queue. - std::unique_lock lkObjectDataQueue(m_muObjectDataCopyMutex); - // Check if the queue is empty. - if (!m_qObjectDataCopySchedule.empty()) - { - // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qObjectDataCopySchedule.front(); - // Pop out of queue. - m_qObjectDataCopySchedule.pop(); - // Release lock. - lkIMUQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) - stContainer.tData = m_slDetectedObjects.object_list; - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } - - ///////////////////////////// - // ObjectData Batched queue. - ///////////////////////////// - // Aqcuire mutex for getting frames out of the pose queue. - std::unique_lock lkObjectBatchedDataQueue(m_muObjectBatchedDataCopyMutex); - // Check if the queue is empty. - if (!m_qObjectBatchedDataCopySchedule.empty()) - { - // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qObjectBatchedDataCopySchedule.front(); - // Pop out of queue. - m_qObjectBatchedDataCopySchedule.pop(); - // Release lock. - lkIMUQueue.unlock(); - - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) - stContainer.tData = m_slDetectedObjectsBatched; - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); - } -} - -/****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. - * Remember this code will be ran in whatever class/thread calls it. - * - * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author ClayJay3 (claytonraycowen@gmail.com) - * @date 2023-09-09 - ******************************************************************************/ -bool ZEDCam::GrabFrame(cv::Mat& cvFrame) -{ - // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvFrame, eBGRA); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and - * stores it in a GPU mat. - * Remember this code will be ran in whatever class/thread calls it. - * - * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author ClayJay3 (claytonraycowen@gmail.com) - * @date 2023-09-09 - ******************************************************************************/ -bool ZEDCam::GrabFrame(cv::cuda::GpuMat& cvGPUFrame) -{ - // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qGPUFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Grabs a depth measure or image from the camera. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. - * - * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. - * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image - * purposes other than displaying depth. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) -{ - // Create instance variables. - PIXEL_FORMATS eFrameType; - - // Check if the container should be set to retrieve an image or a measure. - bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; - // Assemble container. - containers::FrameFetchContainer stContainer(cvDepth, eFrameType); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. - * - * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. - * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image - * purposes other than displaying depth. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) -{ - // Create instance variables. - PIXEL_FORMATS eFrameType; - - // Check if the container should be set to retrieve an image or a measure. - bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; - // Assemble container. - containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qGPUFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal - * image but with three XYZ values replacing the old color values in the 3rd dimension. - * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM - * constants set in AutonomyConstants.h. - * - * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) -{ - // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal - * image but with three XYZ values replacing the old color values in the 3rd dimension. - * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM - * constants set in AutonomyConstants.h. - * - * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -bool ZEDCam::GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud) -{ - // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qGPUFrameCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Resets the cameras X,Y,Z translation and Roll,Pitch,Yaw orientation back - * to 0. THINK CAREFULLY! Do you actually want to reset this? It will also realign - * the coordinate system to whichever way the camera happens to be facing. - * - * @return sl::ERROR_CODE - Status of the positional tracking reset. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::ResetPositionalTracking() -{ - // Create new translation to set position back to zero. - sl::Translation slZeroTranslation(0.0, 0.0, 0.0); - // This will reset position and coordinate frame. - sl::Rotation slZeroRotation; - slZeroRotation.setEulerAngles(sl::float3(0.0, 0.0, 0.0), false); - - // Store new translation and rotation in a tranform object. - sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); - - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Reset the positional tracking location of the camera. - return m_slCamera.resetPositionalTracking(slZeroTransform); -} - -/****************************************************************************** - * @brief A vector containing CustomBoxObjectData objects. These objects simply store - * information about your detected objects from an external object detection model. - * You will need to take your inference results and package them into a sl::CustomBoxObjectData - * so the the ZEDSDK can properly interpret your detections. - * - * Giving the bounding boxes of your detected objects to the ZEDSDK will enable positional - * tracking and velocity estimation for each object. Even when not in view. The IDs of objects - * will also become persistent. - * - * @param vCustomObjects - A vector of sl::CustomBoxObjectData objects. - * @return sl::ERROR_CODE - The return status of ingestion. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::TrackCustomBoxObjects(std::vector& vCustomObjects) -{ - // Create instance varables. - std::vector vCustomBoxData; - - // Repack detection data into sl specific object. - for (ZedObjectData stObjectData : vCustomObjects) - { - // Create new sl CustomBoxObjectData struct. - sl::CustomBoxObjectData slCustomBox; - std::vector vCorners; - - // Assign simple attributes. - slCustomBox.unique_object_id = sl::String(stObjectData.GetObjectUUID().c_str()); - slCustomBox.label = stObjectData.nClassNumber; - slCustomBox.probability = stObjectData.fConfidence; - slCustomBox.is_grounded = stObjectData.bObjectRemainsOnFloorPlane; - // Repackage object corner data. - vCorners.emplace_back(sl::uint2(stObjectData.CornerTL.nX, stObjectData.CornerTL.nY)); - vCorners.emplace_back(sl::uint2(stObjectData.CornerTR.nX, stObjectData.CornerTR.nY)); - vCorners.emplace_back(sl::uint2(stObjectData.CornerBL.nX, stObjectData.CornerBL.nY)); - vCorners.emplace_back(sl::uint2(stObjectData.CornerBR.nX, stObjectData.CornerBR.nY)); - slCustomBox.bounding_box_2d = vCorners; - - // Append repackaged object to vector. - vCustomBoxData.emplace_back(slCustomBox); - } - - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Give the custom box data to the zed api. - sl::ERROR_CODE slReturnCode = m_slCamera.ingestCustomBoxObjects(vCustomBoxData); - // Release lock. - lkSharedLock.unlock(); - - // Check if successful. - if (slReturnCode == sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, - "Failed to ingest new objects for camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Return error code. - return slReturnCode; -} - -/****************************************************************************** - * @brief Performs a hardware reset of the ZED2 or ZED2i camera. - * - * @return sl::ERROR_CODE - Whether or not the camera reboot was successful. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::RebootCamera() -{ - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Reboot this camera and return the status code. - return sl::Camera::reboot(m_slCamera.getCameraInformation().serial_number); -} - -/****************************************************************************** - * @brief Enable the positional tracking functionality of the camera. - * - * @return sl::ERROR_CODE - Whether or not positional tracking was successfully enabled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::EnablePositionalTracking() -{ - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Enable pose tracking and store return code. - sl::ERROR_CODE slReturnCode = m_slCamera.enablePositionalTracking(m_slPoseTrackingParams); - // Release lock. - lkSharedLock.unlock(); - - // Check if positional tracking was enabled properly. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Failed to enabled positional tracking for camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Return error code. - return slReturnCode; -} - -/****************************************************************************** - * @brief Disable to positional tracking funcionality of the camera. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-26 - ******************************************************************************/ -void ZEDCam::DisablePositionalTracking() -{ - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Disable pose tracking. - m_slCamera.disablePositionalTracking(); -} - -/****************************************************************************** - * @brief Sets the pose of the positional tracking of the camera. XYZ will point - * in their respective directions according to ZED_COORD_SYSTEM defined in - * AutonomyConstants.h. - * - * Warning: This method is slow and should not be called in a loop. Setting the pose - * will temporarily block the entire camera from grabbed or copying frames to - * new threads. This method should only be called occasionally when absolutely needed. - * - * @param dX - The X position of the camera in ZED_MEASURE_UNITS. - * @param dY - The Y position of the camera in ZED_MEASURE_UNITS. - * @param dZ - The Z position of the camera in ZED_MEASURE_UNITS. - * @param dXO - The tilt of the camera around the X axis in degrees. - * @param dYO - The tilt of the camera around the Y axis in degrees. - * @param dZO - The tilt of the camera around the Z axis in degrees. - * @return sl::ERROR_CODE - Whether or not the pose was set successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) -{ - // Create new translation to set position back to user given values. - sl::Translation slZeroTranslation(dX, dY, dZ); - // This will reset position and coordinate frame. - sl::Rotation slZeroRotation; - slZeroRotation.setEulerAngles(sl::float3(dXO, dYO, dZO), false); - - // Store new translation and rotation in a tranform object. - sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); - - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Reset the positional tracking location of the camera. - return m_slCamera.resetPositionalTracking(slZeroTransform); -} - -/****************************************************************************** - * @brief Enabled the spatial mapping feature of the camera. Pose tracking will be - * enabled if it is not already. - * - * @param fTimeoutSeconds - The timeout used to wait for pose tracking to be on the OK state. Default is 10 seconds. - * @return sl::ERROR_CODE - Whether or not spatial mapping was successfully enabled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::EnableSpatialMapping(const int nTimeoutSeconds) -{ - // Create instance variables. - auto tmStartTime = std::chrono::steady_clock::now(); - sl::Pose slCameraPose; - sl::ERROR_CODE slReturnCode; - - // Check if positional tracking is enabled. - if (!m_slCamera.isPositionalTrackingEnabled()) - { - // Enable positional tracking. - this->EnablePositionalTracking(); - } - - // Wait for positional tracking state to be OK. Defualt Timeout of 10 seconds. - while (m_slCamera.getPosition(slCameraPose) != sl::POSITIONAL_TRACKING_STATE::OK && - std::chrono::steady_clock::now() - tmStartTime <= std::chrono::seconds(nTimeoutSeconds)) - { - // Sleep for one millisecond. - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - // Final check if positional tracking was successfully enabled. - if (m_slCamera.getPosition(slCameraPose) == sl::POSITIONAL_TRACKING_STATE::OK) - { - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Enable spatial mapping. - slReturnCode = m_slCamera.enableSpatialMapping(m_slSpatialMappingParams); - // Release lock. - lkSharedLock.unlock(); - - // Check if positional tracking was enabled properly. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Failed to enabled spatial mapping for camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - } - else - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Failed to enabled spatial mapping for camera {} ({}) because positional tracking could not be enabled! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Return error code. - return slReturnCode; -} - -/****************************************************************************** - * @brief Disabled the spatial mapping feature of the camera. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -void ZEDCam::DisableSpatialMapping() -{ - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Disable spatial mapping. - m_slCamera.disableSpatialMapping(); -} - -/****************************************************************************** - * @brief Enables the object detection and tracking feature of the camera. - * - * @return sl::ERROR_CODE - Whether or not object detection/tracking was successfully enabled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -sl::ERROR_CODE ZEDCam::EnableObjectDetection(const bool bEnableBatching) -{ - // Check if batching should be turned on. - bEnableBatching ? m_slObjectDetectionBatchParams.enable = true : m_slObjectDetectionBatchParams.enable = false; - // Give batch params to detection params. - m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; - - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Enable object detection. - sl::ERROR_CODE slReturnCode = m_slCamera.enableObjectDetection(m_slObjectDetectionParams); - // Release lock. - lkSharedLock.unlock(); - - // Check if positional tracking was enabled properly. - if (slReturnCode != sl::ERROR_CODE::SUCCESS) - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Failed to enabled object detection for camera {} ({})! sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getCameraInformation().camera_model).get(), - m_slCamera.getCameraInformation().serial_number, - sl::toString(slReturnCode).get()); - } - - // Return error code. - return slReturnCode; -} - -/****************************************************************************** - * @brief Disables the object detection and tracking feature of the camera. - * - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -void ZEDCam::DisableObjectDetection() -{ - // Acquire write lock. - std::unique_lock lkSharedLock(m_muCameraMutex); - // Disable object detection and tracking. - m_slCamera.disableObjectDetection(); -} - -/****************************************************************************** - * @brief Accessor for the current status of the camera. - * - * @return true - Camera is currently opened and functional. - * @return false - Camera is not opened and/or connected. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetCameraIsOpen() -{ - return m_slCamera.isOpened(); -} - -/****************************************************************************** - * @brief Accessor for if this ZED is storing it's frames in GPU memory. - * - * @return true - Using GPU memory for mats. - * @return false - Using CPU memory for mats. - * - * @author ClayJay3 (claytonraycowen@gmail.com) - * @date 2023-09-09 - ******************************************************************************/ -bool ZEDCam::GetUsingGPUMem() const -{ - // Check if we are using GPU memory. - if (m_slMemoryType == sl::MEM::GPU) - { - // Using GPU memory. - return true; - } - else - { - // Not using GPU memory. - return false; - } -} - -/****************************************************************************** - * @brief Accessor for the model enum from the ZEDSDK and represents the camera model as a string. - * - * @return std::string - The model of the zed camera. - * Possible values: ZED, ZED_MINI, ZED_2, ZED_2i, ZED_X, ZED_X_MINI, UNDEFINED_UNKNOWN - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -std::string ZEDCam::GetCameraModel() -{ - // Declare instance variables. - std::string szCameraModel; - - // Check if the camera is opened. - if (m_slCamera.isOpened()) - { - // Convert camera model to a string. - szCameraModel = sl::toString(m_slCamera.getCameraInformation().camera_model).get(); - } - else - { - // Set the model string to show camera isn't opened. - szCameraModel = "NOT_OPENED"; - } - - // Return model of camera represented as string. - return szCameraModel; -} - -/****************************************************************************** - * @brief Accessor for the camera's serial number. - * - * @return unsigned int - - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -unsigned int ZEDCam::GetCameraSerial() -{ - // Return connected camera serial number. - return m_slCamera.getCameraInformation().serial_number; -} - -/****************************************************************************** - * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. - * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. - * - * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. - * @return true - Pose was successfully retrieved and copied. - * @return false - Pose was not successfully retrieved and/or copied. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetPositionalPose(sl::Pose& slPose) -{ - // Check if positional tracking has been enabled. - if (m_slCamera.isPositionalTrackingEnabled()) - { - // Assemble the data container. - containers::DataFetchContainer stContainer(slPose); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qPoseCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving sl::Pose from threadpool queue."); - // Image was not written successfully. - return false; - } - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled!"); - // Return unsuccessful. - return false; - } -} - -/****************************************************************************** - * @brief Accessor for if the positional tracking functionality of the camera has been enabled. - * - * @return true - Positional tracking is enabled. - * @return false - Positional tracking is not enabled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetPositionalTrackingEnabled() -{ - return m_slCamera.isPositionalTrackingEnabled(); -} - -/****************************************************************************** - * @brief Gets the IMU data from the ZED camera. If getting the data fails, the - * last successfully retrieved value is returned. - * - * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. - * @return true - The IMU vals were copied and stored successfully. - * @return false - The IMU vals were copied and stored successfully. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetIMUData(std::vector& vIMUValues) -{ - // Assemble the data container. - containers::DataFetchContainer&> stContainer(vIMUValues); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qIMUDataCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving IMU data from threadpool queue."); - // Image was not written successfully. - return false; - } -} - -/****************************************************************************** - * @brief Accessor for the current state of the camera's spatial mapping feature. - * - * @return sl::SPATIAL_MAPPING_STATE - The enum value of the spatial mapping state. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -sl::SPATIAL_MAPPING_STATE ZEDCam::GetSpatialMappingState() -{ - // Return the current spatial mapping state of the camera. - return m_slCamera.getSpatialMappingState(); -} - -/****************************************************************************** - * @brief Retrieve the built spatial map from the camera. Spatial mapping must be enabled. - * This method takes in an std::future to eventually store the map in. - * It returns a enum code representing the successful scheduling of building the map. - * Any code other than SPATIAL_MAPPING_STATE::OK means the future will never be filled. - * - * @param std::future - The future to eventually store the map in. - * @return sl::SPATIAL_MAPPING_STATE - Whether or not the building of the map was successfully scheduled. - * Anything other than OK means the future will never be filled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -sl::SPATIAL_MAPPING_STATE ZEDCam::ExtractSpatialMapAsync(std::future& fuMeshFuture) -{ - // Get and store current state of spatial mapping. - sl::SPATIAL_MAPPING_STATE slReturnState = m_slCamera.getSpatialMappingState(); - - // Check if spatial mapping has been enabled and ready - if (slReturnState == sl::SPATIAL_MAPPING_STATE::OK) - { - // Request that the ZEDSDK begin processing the spatial map for export. - m_slCamera.requestSpatialMapAsync(); - - // Start an async thread to wait for spatial map processing to finish. Return resultant future object. - fuMeshFuture = std::async(std::launch::async, - [this]() - { - // Create instance variables. - sl::Mesh slSpatialMap; - - // Loop until map is finished generating. - while (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) - { - // Sleep for 10ms. - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - // Check if the spatial map was exported successfully. - if (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::SUCCESS) - { - // Get and store the spatial map. - m_slCamera.retrieveSpatialMapAsync(slSpatialMap); - - // Return spatial map. - return slSpatialMap; - } - else - { - // Submit logger message. - LOG_ERROR(g_qSharedLogger, - "Failed to extract ZED spatial map. sl::ERROR_CODE is: {}", - sl::toString(m_slCamera.getSpatialMapRequestStatusAsync()).get()); - - // Return empty point cloud. - return sl::Mesh(); - } - }); - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "ZED spatial mapping was never enabled, can't extract spatial map!"); - } - - // Return current spatial mapping state. - return slReturnState; -} - -/****************************************************************************** - * @brief Accessor for if the cameras object detection and tracking feature is enabled. - * - * @return true - Object detection and tracking is enabled. - * @return false - Object detection and tracking is not enabled. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetObjectDetectionEnabled() -{ - return m_slCamera.isObjectDetectionEnabled(); -} - -/****************************************************************************** - * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the - * given sl::ObjectData vector. - * - * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. - * @return true - The object data was successfully copied. - * @return false - The object data was not successfully copied. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-27 - ******************************************************************************/ -bool ZEDCam::GetObjects(std::vector& vObjectData) -{ - // Check if object detection has been enabled. - if (m_slCamera.isObjectDetectionEnabled()) - { - // Assemble the data container. - containers::DataFetchContainer&> stContainer(vObjectData); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qObjectDataCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving object data from threadpool queue."); - // Image was not written successfully. - return false; - } - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Attempted to get ZED object data but object detection/tracking is not enabled!"); - // Return unsuccessful. - return false; - } -} - -/****************************************************************************** - * @brief If batching is enabled, this retrieves the normal objects and passes them to - * the the iternal batching queue of the zed api. This performs short-term re-identification - * with deep learning and trajectories filtering. Batching must have been set to enabled when - * EnableObjectDetection() was called. Most of the time the vector will be empty and will be - * filled every ZED_OBJDETECTION_BATCH_LATENCY. - * - * @param vBatchedObjectData - A vector containing objects of sl::ObjectsBatch object that will - * have object data copied to. - * @return true - The batched objects data was successfully copied. - * @return false - The batched objects data was not successfully copied. - * - * @author clayjay3 (claytonraycowen@gmail.com) - * @date 2023-08-30 - ******************************************************************************/ -bool ZEDCam::GetBatchedObjects(std::vector& vBatchedObjectData) -{ - // Check if object detection and batching has been enabled. - if (m_slCamera.isObjectDetectionEnabled() && m_slObjectDetectionBatchParams.enable) - { - // Assemble the data container. - containers::DataFetchContainer&> stContainer(vBatchedObjectData); - - // Acquire lock on frame copy queue. - std::unique_lock lkSchedulers(m_muPoolScheduleMutex); - // Append frame fetch container to the schedule queue. - m_qObjectBatchedDataCopySchedule.push(stContainer); - // Release lock on the frame schedule queue. - lkSchedulers.unlock(); - - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving batched object data from threadpool queue."); - // Image was not written successfully. - return false; - } - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Attempted to get ZED batched object data but object detection/tracking is not enabled!"); - // Return unsuccessful. - return false; - } -} +// /****************************************************************************** +// * @brief Implements the ZEDCam class. +// * +// * @file ZEDCam.cpp +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// * +// * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved +// ******************************************************************************/ + +// #include "ZEDCam.h" +// #include "../../AutonomyLogging.h" +// #include "../../util/vision/ImageOperations.hpp" + +// /****************************************************************************** +// * @brief This struct is part of the ZEDCam class and is used as a container for all +// * bounding box data that is going to be passed to the zed api via the ZEDCam's +// * TrackCustomBoxObjects() method. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-29 +// ******************************************************************************/ +// struct ZEDCam::ZedObjectData + +// { +// private: +// // Declare and define private struct member variables. +// std::string szObjectUUID = sl::generate_unique_id().get(); // This will automatically generate a guaranteed unique id so the object is traceable. + +// // Declare a private struct for holding point data. +// /****************************************************************************** +// * @brief This struct is internal to the ZedObjectData struct is used to store +// * an X and Y value for the corners of a bounding box. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-29 +// ******************************************************************************/ +// struct Corner +// { +// public: +// // Declare public struct member variables. +// unsigned int nX; +// unsigned int nY; +// }; + +// public: +// // Declare and define public struct member variables. +// struct Corner CornerTL; // The top left corner of the bounding box. +// struct Corner CornerTR; // The top right corner of the bounding box. +// struct Corner CornerBL; // The bottom left corner of the bounding box. +// struct Corner CornerBR; // The bottom right corner of bounding box. +// int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the +// same. float fConfidence; // This info is passed through from your detection algorithm and will help improve tracking by throwing out bad detections. +// // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class +// number +// // to avoid issues. +// bool bObjectRemainsOnFloorPlane = false; + +// // Declare and define public struct getters. +// std::string GetObjectUUID() { return szObjectUUID; }; +// }; + +// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// /****************************************************************************** +// * @brief Construct a new Zed Cam:: Zed Cam object. +// * +// * @param nPropResolutionX - X res of camera. Must be smaller than ZED_BASE_RESOLUTION. +// * @param nPropResolutionY - Y res of camera. Must be smaller than ZED_BASE_RESOLUTION. +// * @param nPropFramesPerSecond - FPS camera is running at. +// * @param dPropHorizontalFOV - The horizontal field of view. +// * @param dPropVerticalFOV - The vertical field of view. +// * @param fMinSenseDistance - The minimum distance to include in depth measures. +// * @param fMaxSenseDistance - The maximim distance to include in depth measures. +// * @param bMemTypeGPU - Whether or not to use the GPU memory for operations. +// * @param unCameraSerialNumber - The serial number of the camera to open. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// ZEDCam::ZEDCam(const int nPropResolutionX, +// const int nPropResolutionY, +// const int nPropFramesPerSecond, +// const double dPropHorizontalFOV, +// const double dPropVerticalFOV, +// const float fMinSenseDistance, +// const float fMaxSenseDistance, +// const bool bMemTypeGPU, +// const bool bUseHalfDepthPrecision, +// const unsigned int unCameraSerialNumber) : +// Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) +// { +// // Assign member variables. +// bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; +// bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; +// m_unCameraSerialNumber = unCameraSerialNumber; + +// // Setup camera params. +// m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; +// m_slCameraParams.camera_fps = nPropFramesPerSecond; +// m_slCameraParams.coordinate_units = constants::ZED_MEASURE_UNITS; +// m_slCameraParams.coordinate_system = constants::ZED_COORD_SYSTEM; +// m_slCameraParams.depth_mode = constants::ZED_DEPTH_MODE; +// m_slCameraParams.depth_minimum_distance = fMinSenseDistance; +// m_slCameraParams.depth_maximum_distance = fMaxSenseDistance; +// m_slCameraParams.depth_stabilization = constants::ZED_DEPTH_STABILIZATION; +// // Only set serial number if necessary. +// if (unCameraSerialNumber != static_cast(0)) +// { +// m_slCameraParams.input.setFromSerialNumber(unCameraSerialNumber); +// } + +// // Setup camera runtime params. +// m_slRuntimeParams.enable_fill_mode = constants::ZED_SENSING_FILL; + +// // Setup positional tracking parameters. +// m_slPoseTrackingParams.mode = constants::ZED_POSETRACK_MODE; +// m_slPoseTrackingParams.enable_area_memory = constants::ZED_POSETRACK_AREA_MEMORY; +// m_slPoseTrackingParams.enable_pose_smoothing = constants::ZED_POSETRACK_POSE_SMOOTHING; +// m_slPoseTrackingParams.set_floor_as_origin = constants::ZED_POSETRACK_FLOOR_IS_ORIGIN; +// m_slPoseTrackingParams.enable_imu_fusion = constants::ZED_POSETRACK_ENABLE_IMU_FUSION; +// m_slPoseTrackingParams.depth_min_range = constants::ZED_POSETRACK_USABLE_DEPTH_MIN; +// m_slPoseTrackingParams.set_gravity_as_origin = constants::ZED_POSETRACK_USE_GRAVITY_ORIGIN; + +// // Setup spatial mapping parameters. +// m_slSpatialMappingParams.map_type = constants::ZED_MAPPING_TYPE; +// m_slSpatialMappingParams.resolution_meter = constants::ZED_MAPPING_RESOLUTION_METER; +// m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); +// m_slSpatialMappingParams.save_texture = true; +// m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; +// m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; + +// // Setup object detection/tracking parameters. +// m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; +// m_slObjectDetectionParams.image_sync = constants::ZED_OBJDETECTION_IMG_SYNC; +// m_slObjectDetectionParams.enable_tracking = constants::ZED_OBJDETECTION_TRACK_OBJ; +// m_slObjectDetectionParams.enable_segmentation = constants::ZED_OBJDETECTION_SEGMENTATION; +// m_slObjectDetectionParams.filtering_mode = constants::ZED_OBJDETECTION_FILTERING; +// m_slObjectDetectionParams.prediction_timeout_s = constants::ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT; +// // Setup object detection/tracking batch parameters. +// m_slObjectDetectionBatchParams.enable = false; +// m_slObjectDetectionBatchParams.id_retention_time = constants::ZED_OBJDETECTION_BATCH_RETENTION_TIME; +// m_slObjectDetectionBatchParams.latency = constants::ZED_OBJDETECTION_BATCH_LATENCY; +// m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; + +// // Attempt to open camera. +// sl::ERROR_CODE slReturnCode = m_slCamera.open(m_slCameraParams); +// // Check if the camera was successfully opened. +// if (m_slCamera.isOpened()) +// { +// // Submit logger message. +// LOG_DEBUG(g_qSharedLogger, +// "{} stereo camera with serial number {} has been succsessfully opened.", +// this->GetCameraModel(), +// m_slCamera.getCameraInformation().serial_number); +// } +// else +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Unable to open stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } +// } + +// /****************************************************************************** +// * @brief Destroy the Zed Cam:: Zed Cam object. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// ZEDCam::~ZEDCam() +// { +// // Stop threaded code. +// this->RequestStop(); +// this->Join(); + +// // Close the ZEDCam. +// m_slCamera.close(); +// } + +// /****************************************************************************** +// * @brief The code inside this private method runs in a seperate thread, but still +// * has access to this*. This method continuously calls the grab() function of +// * the ZEDSDK, which updates all frames (RGB, depth, cloud) and all other data +// * such as positional and spatial mapping. Then a thread pool is started and joined +// * once per iteration to mass copy the frames and/or measure to any other thread +// * waiting in the queues. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-09-01 +// ******************************************************************************/ +// void ZEDCam::ThreadedContinuousCode() +// { +// // Check if camera is opened. +// if (!m_slCamera.isOpened()) +// { +// // Shutdown threads for this ZEDCam. +// this->RequestStop(); +// // Submit logger message. +// LOG_CRITICAL(g_qSharedLogger, +// "Camera start was attempted for camera at {}, but camera never properly opened or it has been closed/rebooted!", +// m_unCameraSerialNumber); +// } +// else +// { +// // Acquire write lock for camera object. +// std::unique_lock lkSharedCameraLock(m_muCameraMutex); +// // Call generalized update method of zed api. +// sl::ERROR_CODE slReturnCode = m_slCamera.grab(m_slRuntimeParams); +// // Check if new frame was computed successfully. +// if (slReturnCode == sl::ERROR_CODE::SUCCESS) +// { +// // Grab regular image and store it in member variable. +// slReturnCode = m_slCamera.retrieveImage(m_slFrame, constants::ZED_RETRIEVE_VIEW, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new frame image for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Grab depth measure and store it in member variable. +// slReturnCode = m_slCamera.retrieveMeasure(m_slDepthMeasure, m_slDepthMeasureType, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new depth measure for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Grab depth grayscale image and store it in member variable. +// slReturnCode = m_slCamera.retrieveImage(m_slDepthImage, sl::VIEW::DEPTH, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new depth image for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Grab regular resized image and store it in member variable. +// slReturnCode = m_slCamera.retrieveMeasure(m_slPointCloud, sl::MEASURE::XYZ, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new point cloud for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Get and store the SensorData object from the camera. Get data from the most recent image grab. +// // Using TIME_REFERENCE::CURRENT requires high rate polling and can introduce error as the most recent +// // IMU data could be in the future of the camera image. +// slReturnCode = m_slCamera.getSensorsData(m_slSensorData, sl::TIME_REFERENCE::IMAGE); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new sensors data for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Check if positional tracking is enabled. +// if (m_slCamera.isPositionalTrackingEnabled()) +// { +// // Get the current pose of the camera. +// sl::POSITIONAL_TRACKING_STATE slPoseTrackReturnCode = m_slCamera.getPosition(m_slCameraPose, sl::REFERENCE_FRAME::WORLD); +// // Check that the regular frame was retrieved successfully. +// if (slPoseTrackReturnCode != sl::POSITIONAL_TRACKING_STATE::OK) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new positional tracking pose for stereo camera {} ({})! sl::POSITIONAL_TRACKING_STATE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slPoseTrackReturnCode).get()); +// } +// } +// // Check if object detection is enabled. +// if (m_slCamera.isObjectDetectionEnabled()) +// { +// // Get updated objects from camera. +// slReturnCode = m_slCamera.retrieveObjects(m_slDetectedObjects); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Check if batched object data is enabled. +// if (m_slObjectDetectionBatchParams.enable) +// { +// // Get updated batched objects from camera. +// slReturnCode = m_slCamera.getObjectsBatch(m_slDetectedObjectsBatched); +// // Check that the regular frame was retrieved successfully. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Unable to retrieve new batched object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } +// } +// } +// // Release camera lock. +// lkSharedCameraLock.unlock(); + +// // Call FPS tick. +// m_IPS.Tick(); +// } +// else +// { +// // Release camera lock. +// lkSharedCameraLock.unlock(); + +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Unable to update stereo camera {} ({}) frames, measurements, and sensors! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Acquire a shared_lock on the frame copy queue. +// std::shared_lock lkSchedulers(m_muPoolScheduleMutex); +// // Check if the frame copy queue is empty. +// if (!m_qFrameCopySchedule.empty() || !m_qGPUFrameCopySchedule.empty() || !m_qCustomBoxInjestSchedule.empty() || !m_qPoseCopySchedule.empty() || +// !m_qIMUDataCopySchedule.empty() || !m_qObjectDataCopySchedule.empty() || !m_qObjectBatchedDataCopySchedule.empty()) +// { +// // Find the queue with the longest length. +// size_t siMaxQueueLength = std::max({m_qFrameCopySchedule.size(), +// m_qGPUFrameCopySchedule.size(), +// m_qCustomBoxInjestSchedule.size(), +// m_qPoseCopySchedule.size(), +// m_qIMUDataCopySchedule.size(), +// m_qObjectDataCopySchedule.size(), +// m_qObjectBatchedDataCopySchedule.size()}); + +// // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. +// this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); +// // Wait for thread pool to finish. +// this->JoinPool(); +// // Release lock on frame copy queue. +// lkSchedulers.unlock(); +// } +// } +// } + +// /****************************************************************************** +// * @brief This method holds the code that is ran in the thread pool started by +// * the ThreadedLinearCode() method. It copies the data from the different +// * data objects to references of the same type stored in a vector queued up by the +// * Grab methods. +// * +// * +// * @author ClayJay3 (claytonraycowen@gmail.com) +// * @date 2023-09-08 +// ******************************************************************************/ +// void ZEDCam::PooledLinearCode() +// { +// ///////////////////////////// +// // Frame queue. +// ///////////////////////////// +// // Check if we are using CPU or GPU mats. +// if (m_slMemoryType == sl::MEM::CPU) +// { +// // Aqcuire mutex for getting frames out of the queue. +// std::unique_lock lkFrameQueue(m_muFrameCopyMutex); +// // Check if the queue is empty. +// if (!m_qFrameCopySchedule.empty()) +// { +// // Get frame container out of queue. +// containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); +// // Pop out of queue. +// m_qFrameCopySchedule.pop(); +// // Release lock. +// lkFrameQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); + +// // Determine which frame should be copied. +// switch (stContainer.eFrameType) +// { +// case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; +// case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; +// case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; +// case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; +// default: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; +// } + +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } +// } +// // Use GPU mat. +// else +// { +// // Check if the queue is empty. +// if (!m_qGPUFrameCopySchedule.empty()) +// { +// // Aqcuire mutex for getting frames out of the queue. +// std::unique_lock lkFrameQueue(m_muFrameCopyMutex); +// // Get frame container out of queue. +// containers::FrameFetchContainer& stContainer = m_qGPUFrameCopySchedule.front(); +// // Pop out of queue. +// m_qGPUFrameCopySchedule.pop(); +// // Release lock. +// lkFrameQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); + +// // Determine which frame should be copied. +// switch (stContainer.eFrameType) +// { +// case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; +// case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; +// case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; +// case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; +// default: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; +// } + +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } +// } + +// ///////////////////////////// +// // Pose queue. +// ///////////////////////////// +// // Aqcuire mutex for getting frames out of the pose queue. +// std::unique_lock lkPoseQueue(m_muPoseCopyMutex); +// // Check if the queue is empty. +// if (!m_qPoseCopySchedule.empty()) +// { +// // Get frame container out of queue. +// containers::DataFetchContainer& stContainer = m_qPoseCopySchedule.front(); +// // Pop out of queue. +// m_qPoseCopySchedule.pop(); +// // Release lock. +// lkPoseQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Copy pose. +// stContainer.tData = sl::Pose(m_slCameraPose); +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } + +// ///////////////////////////// +// // IMU data queue. +// ///////////////////////////// +// // Aqcuire mutex for getting frames out of the pose queue. +// std::unique_lock lkIMUQueue(m_muIMUDataCopyMutex); +// // Check if the queue is empty. +// if (!m_qIMUDataCopySchedule.empty()) +// { +// // Get frame container out of queue. +// containers::DataFetchContainer&>& stContainer = m_qIMUDataCopySchedule.front(); +// // Pop out of queue. +// m_qIMUDataCopySchedule.pop(); +// // Release lock. +// lkIMUQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); + +// // Get IMU orientation in degrees. +// sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); +// // Get IMU linear acceleration. +// sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; +// // Repackage angles and accels into vector. +// stContainer.tData.emplace_back(slAngles.x); +// stContainer.tData.emplace_back(slAngles.y); +// stContainer.tData.emplace_back(slAngles.z); +// stContainer.tData.emplace_back(slLinearAccels.x); +// stContainer.tData.emplace_back(slLinearAccels.y); +// stContainer.tData.emplace_back(slLinearAccels.z); + +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } + +// ///////////////////////////// +// // ObjectData queue. +// ///////////////////////////// +// // Aqcuire mutex for getting frames out of the pose queue. +// std::unique_lock lkObjectDataQueue(m_muObjectDataCopyMutex); +// // Check if the queue is empty. +// if (!m_qObjectDataCopySchedule.empty()) +// { +// // Get frame container out of queue. +// containers::DataFetchContainer&>& stContainer = m_qObjectDataCopySchedule.front(); +// // Pop out of queue. +// m_qObjectDataCopySchedule.pop(); +// // Release lock. +// lkIMUQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) +// stContainer.tData = m_slDetectedObjects.object_list; +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } + +// ///////////////////////////// +// // ObjectData Batched queue. +// ///////////////////////////// +// // Aqcuire mutex for getting frames out of the pose queue. +// std::unique_lock lkObjectBatchedDataQueue(m_muObjectBatchedDataCopyMutex); +// // Check if the queue is empty. +// if (!m_qObjectBatchedDataCopySchedule.empty()) +// { +// // Get frame container out of queue. +// containers::DataFetchContainer&>& stContainer = m_qObjectBatchedDataCopySchedule.front(); +// // Pop out of queue. +// m_qObjectBatchedDataCopySchedule.pop(); +// // Release lock. +// lkIMUQueue.unlock(); + +// // Acquire unique lock on container. +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) +// stContainer.tData = m_slDetectedObjectsBatched; +// // Release lock. +// lkConditionLock.unlock(); +// // Use the condition variable to notify other waiting threads that this thread is finished. +// stContainer.cdMatWriteSuccess.notify_all(); +// } +// } + +// /****************************************************************************** +// * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. +// * Remember this code will be ran in whatever class/thread calls it. +// * +// * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author ClayJay3 (claytonraycowen@gmail.com) +// * @date 2023-09-09 +// ******************************************************************************/ +// bool ZEDCam::GrabFrame(cv::Mat& cvFrame) +// { +// // Assemble the FrameFetchContainer. +// containers::FrameFetchContainer stContainer(cvFrame, eBGRA); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and +// * stores it in a GPU mat. +// * Remember this code will be ran in whatever class/thread calls it. +// * +// * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author ClayJay3 (claytonraycowen@gmail.com) +// * @date 2023-09-09 +// ******************************************************************************/ +// bool ZEDCam::GrabFrame(cv::cuda::GpuMat& cvGPUFrame) +// { +// // Assemble the FrameFetchContainer. +// containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qGPUFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Grabs a depth measure or image from the camera. This image has the same shape as +// * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in +// * AutonomyConstants.h. +// * +// * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. +// * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image +// * purposes other than displaying depth. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) +// { +// // Create instance variables. +// PIXEL_FORMATS eFrameType; + +// // Check if the container should be set to retrieve an image or a measure. +// bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; +// // Assemble container. +// containers::FrameFetchContainer stContainer(cvDepth, eFrameType); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as +// * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in +// * AutonomyConstants.h. +// * +// * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. +// * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image +// * purposes other than displaying depth. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) +// { +// // Create instance variables. +// PIXEL_FORMATS eFrameType; + +// // Check if the container should be set to retrieve an image or a measure. +// bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; +// // Assemble container. +// containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qGPUFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal +// * image but with three XYZ values replacing the old color values in the 3rd dimension. +// * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM +// * constants set in AutonomyConstants.h. +// * +// * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) +// { +// // Assemble the FrameFetchContainer. +// containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal +// * image but with three XYZ values replacing the old color values in the 3rd dimension. +// * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM +// * constants set in AutonomyConstants.h. +// * +// * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. +// * @return true - The frame was successfully copied. +// * @return false - The frame was not copied successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// bool ZEDCam::GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud) +// { +// // Assemble the FrameFetchContainer. +// containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qGPUFrameCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Resets the cameras X,Y,Z translation and Roll,Pitch,Yaw orientation back +// * to 0. THINK CAREFULLY! Do you actually want to reset this? It will also realign +// * the coordinate system to whichever way the camera happens to be facing. +// * +// * @return sl::ERROR_CODE - Status of the positional tracking reset. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::ResetPositionalTracking() +// { +// // Create new translation to set position back to zero. +// sl::Translation slZeroTranslation(0.0, 0.0, 0.0); +// // This will reset position and coordinate frame. +// sl::Rotation slZeroRotation; +// slZeroRotation.setEulerAngles(sl::float3(0.0, 0.0, 0.0), false); + +// // Store new translation and rotation in a tranform object. +// sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); + +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Reset the positional tracking location of the camera. +// return m_slCamera.resetPositionalTracking(slZeroTransform); +// } + +// /****************************************************************************** +// * @brief A vector containing CustomBoxObjectData objects. These objects simply store +// * information about your detected objects from an external object detection model. +// * You will need to take your inference results and package them into a sl::CustomBoxObjectData +// * so the the ZEDSDK can properly interpret your detections. +// * +// * Giving the bounding boxes of your detected objects to the ZEDSDK will enable positional +// * tracking and velocity estimation for each object. Even when not in view. The IDs of objects +// * will also become persistent. +// * +// * @param vCustomObjects - A vector of sl::CustomBoxObjectData objects. +// * @return sl::ERROR_CODE - The return status of ingestion. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::TrackCustomBoxObjects(std::vector& vCustomObjects) +// { +// // Create instance varables. +// std::vector vCustomBoxData; + +// // Repack detection data into sl specific object. +// for (ZedObjectData stObjectData : vCustomObjects) +// { +// // Create new sl CustomBoxObjectData struct. +// sl::CustomBoxObjectData slCustomBox; +// std::vector vCorners; + +// // Assign simple attributes. +// slCustomBox.unique_object_id = sl::String(stObjectData.GetObjectUUID().c_str()); +// slCustomBox.label = stObjectData.nClassNumber; +// slCustomBox.probability = stObjectData.fConfidence; +// slCustomBox.is_grounded = stObjectData.bObjectRemainsOnFloorPlane; +// // Repackage object corner data. +// vCorners.emplace_back(sl::uint2(stObjectData.CornerTL.nX, stObjectData.CornerTL.nY)); +// vCorners.emplace_back(sl::uint2(stObjectData.CornerTR.nX, stObjectData.CornerTR.nY)); +// vCorners.emplace_back(sl::uint2(stObjectData.CornerBL.nX, stObjectData.CornerBL.nY)); +// vCorners.emplace_back(sl::uint2(stObjectData.CornerBR.nX, stObjectData.CornerBR.nY)); +// slCustomBox.bounding_box_2d = vCorners; + +// // Append repackaged object to vector. +// vCustomBoxData.emplace_back(slCustomBox); +// } + +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Give the custom box data to the zed api. +// sl::ERROR_CODE slReturnCode = m_slCamera.ingestCustomBoxObjects(vCustomBoxData); +// // Release lock. +// lkSharedLock.unlock(); + +// // Check if successful. +// if (slReturnCode == sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, +// "Failed to ingest new objects for camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Return error code. +// return slReturnCode; +// } + +// /****************************************************************************** +// * @brief Performs a hardware reset of the ZED2 or ZED2i camera. +// * +// * @return sl::ERROR_CODE - Whether or not the camera reboot was successful. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::RebootCamera() +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Reboot this camera and return the status code. +// return sl::Camera::reboot(m_slCamera.getCameraInformation().serial_number); +// } + +// /****************************************************************************** +// * @brief Enable the positional tracking functionality of the camera. +// * +// * @return sl::ERROR_CODE - Whether or not positional tracking was successfully enabled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::EnablePositionalTracking() +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Enable pose tracking and store return code. +// sl::ERROR_CODE slReturnCode = m_slCamera.enablePositionalTracking(m_slPoseTrackingParams); +// // Release lock. +// lkSharedLock.unlock(); + +// // Check if positional tracking was enabled properly. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Failed to enabled positional tracking for camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Return error code. +// return slReturnCode; +// } + +// /****************************************************************************** +// * @brief Disable to positional tracking funcionality of the camera. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-26 +// ******************************************************************************/ +// void ZEDCam::DisablePositionalTracking() +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Disable pose tracking. +// m_slCamera.disablePositionalTracking(); +// } + +// /****************************************************************************** +// * @brief Sets the pose of the positional tracking of the camera. XYZ will point +// * in their respective directions according to ZED_COORD_SYSTEM defined in +// * AutonomyConstants.h. +// * +// * Warning: This method is slow and should not be called in a loop. Setting the pose +// * will temporarily block the entire camera from grabbed or copying frames to +// * new threads. This method should only be called occasionally when absolutely needed. +// * +// * @param dX - The X position of the camera in ZED_MEASURE_UNITS. +// * @param dY - The Y position of the camera in ZED_MEASURE_UNITS. +// * @param dZ - The Z position of the camera in ZED_MEASURE_UNITS. +// * @param dXO - The tilt of the camera around the X axis in degrees. +// * @param dYO - The tilt of the camera around the Y axis in degrees. +// * @param dZO - The tilt of the camera around the Z axis in degrees. +// * @return sl::ERROR_CODE - Whether or not the pose was set successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) +// { +// // Create new translation to set position back to user given values. +// sl::Translation slZeroTranslation(dX, dY, dZ); +// // This will reset position and coordinate frame. +// sl::Rotation slZeroRotation; +// slZeroRotation.setEulerAngles(sl::float3(dXO, dYO, dZO), false); + +// // Store new translation and rotation in a tranform object. +// sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); + +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Reset the positional tracking location of the camera. +// return m_slCamera.resetPositionalTracking(slZeroTransform); +// } + +// /****************************************************************************** +// * @brief Enabled the spatial mapping feature of the camera. Pose tracking will be +// * enabled if it is not already. +// * +// * @param fTimeoutSeconds - The timeout used to wait for pose tracking to be on the OK state. Default is 10 seconds. +// * @return sl::ERROR_CODE - Whether or not spatial mapping was successfully enabled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::EnableSpatialMapping(const int nTimeoutSeconds) +// { +// // Create instance variables. +// auto tmStartTime = std::chrono::steady_clock::now(); +// sl::Pose slCameraPose; +// sl::ERROR_CODE slReturnCode; + +// // Check if positional tracking is enabled. +// if (!m_slCamera.isPositionalTrackingEnabled()) +// { +// // Enable positional tracking. +// this->EnablePositionalTracking(); +// } + +// // Wait for positional tracking state to be OK. Defualt Timeout of 10 seconds. +// while (m_slCamera.getPosition(slCameraPose) != sl::POSITIONAL_TRACKING_STATE::OK && +// std::chrono::steady_clock::now() - tmStartTime <= std::chrono::seconds(nTimeoutSeconds)) +// { +// // Sleep for one millisecond. +// std::this_thread::sleep_for(std::chrono::milliseconds(100)); +// } + +// // Final check if positional tracking was successfully enabled. +// if (m_slCamera.getPosition(slCameraPose) == sl::POSITIONAL_TRACKING_STATE::OK) +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Enable spatial mapping. +// slReturnCode = m_slCamera.enableSpatialMapping(m_slSpatialMappingParams); +// // Release lock. +// lkSharedLock.unlock(); + +// // Check if positional tracking was enabled properly. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Failed to enabled spatial mapping for camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } +// } +// else +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Failed to enabled spatial mapping for camera {} ({}) because positional tracking could not be enabled! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Return error code. +// return slReturnCode; +// } + +// /****************************************************************************** +// * @brief Disabled the spatial mapping feature of the camera. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// void ZEDCam::DisableSpatialMapping() +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Disable spatial mapping. +// m_slCamera.disableSpatialMapping(); +// } + +// /****************************************************************************** +// * @brief Enables the object detection and tracking feature of the camera. +// * +// * @return sl::ERROR_CODE - Whether or not object detection/tracking was successfully enabled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// sl::ERROR_CODE ZEDCam::EnableObjectDetection(const bool bEnableBatching) +// { +// // Check if batching should be turned on. +// bEnableBatching ? m_slObjectDetectionBatchParams.enable = true : m_slObjectDetectionBatchParams.enable = false; +// // Give batch params to detection params. +// m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; + +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Enable object detection. +// sl::ERROR_CODE slReturnCode = m_slCamera.enableObjectDetection(m_slObjectDetectionParams); +// // Release lock. +// lkSharedLock.unlock(); + +// // Check if positional tracking was enabled properly. +// if (slReturnCode != sl::ERROR_CODE::SUCCESS) +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Failed to enabled object detection for camera {} ({})! sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), +// m_slCamera.getCameraInformation().serial_number, +// sl::toString(slReturnCode).get()); +// } + +// // Return error code. +// return slReturnCode; +// } + +// /****************************************************************************** +// * @brief Disables the object detection and tracking feature of the camera. +// * +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// void ZEDCam::DisableObjectDetection() +// { +// // Acquire write lock. +// std::unique_lock lkSharedLock(m_muCameraMutex); +// // Disable object detection and tracking. +// m_slCamera.disableObjectDetection(); +// } + +// /****************************************************************************** +// * @brief Accessor for the current status of the camera. +// * +// * @return true - Camera is currently opened and functional. +// * @return false - Camera is not opened and/or connected. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetCameraIsOpen() +// { +// return m_slCamera.isOpened(); +// } + +// /****************************************************************************** +// * @brief Accessor for if this ZED is storing it's frames in GPU memory. +// * +// * @return true - Using GPU memory for mats. +// * @return false - Using CPU memory for mats. +// * +// * @author ClayJay3 (claytonraycowen@gmail.com) +// * @date 2023-09-09 +// ******************************************************************************/ +// bool ZEDCam::GetUsingGPUMem() const +// { +// // Check if we are using GPU memory. +// if (m_slMemoryType == sl::MEM::GPU) +// { +// // Using GPU memory. +// return true; +// } +// else +// { +// // Not using GPU memory. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Accessor for the model enum from the ZEDSDK and represents the camera model as a string. +// * +// * @return std::string - The model of the zed camera. +// * Possible values: ZED, ZED_MINI, ZED_2, ZED_2i, ZED_X, ZED_X_MINI, UNDEFINED_UNKNOWN +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// std::string ZEDCam::GetCameraModel() +// { +// // Declare instance variables. +// std::string szCameraModel; + +// // Check if the camera is opened. +// if (m_slCamera.isOpened()) +// { +// // Convert camera model to a string. +// szCameraModel = sl::toString(m_slCamera.getCameraInformation().camera_model).get(); +// } +// else +// { +// // Set the model string to show camera isn't opened. +// szCameraModel = "NOT_OPENED"; +// } + +// // Return model of camera represented as string. +// return szCameraModel; +// } + +// /****************************************************************************** +// * @brief Accessor for the camera's serial number. +// * +// * @return unsigned int - +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// unsigned int ZEDCam::GetCameraSerial() +// { +// // Return connected camera serial number. +// return m_slCamera.getCameraInformation().serial_number; +// } + +// /****************************************************************************** +// * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. +// * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. +// * +// * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. +// * @return true - Pose was successfully retrieved and copied. +// * @return false - Pose was not successfully retrieved and/or copied. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetPositionalPose(sl::Pose& slPose) +// { +// // Check if positional tracking has been enabled. +// if (m_slCamera.isPositionalTrackingEnabled()) +// { +// // Assemble the data container. +// containers::DataFetchContainer stContainer(slPose); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qPoseCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving sl::Pose from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled!"); +// // Return unsuccessful. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Accessor for if the positional tracking functionality of the camera has been enabled. +// * +// * @return true - Positional tracking is enabled. +// * @return false - Positional tracking is not enabled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetPositionalTrackingEnabled() +// { +// return m_slCamera.isPositionalTrackingEnabled(); +// } + +// /****************************************************************************** +// * @brief Gets the IMU data from the ZED camera. If getting the data fails, the +// * last successfully retrieved value is returned. +// * +// * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. +// * @return true - The IMU vals were copied and stored successfully. +// * @return false - The IMU vals were copied and stored successfully. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetIMUData(std::vector& vIMUValues) +// { +// // Assemble the data container. +// containers::DataFetchContainer&> stContainer(vIMUValues); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qIMUDataCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving IMU data from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief Accessor for the current state of the camera's spatial mapping feature. +// * +// * @return sl::SPATIAL_MAPPING_STATE - The enum value of the spatial mapping state. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// sl::SPATIAL_MAPPING_STATE ZEDCam::GetSpatialMappingState() +// { +// // Return the current spatial mapping state of the camera. +// return m_slCamera.getSpatialMappingState(); +// } + +// /****************************************************************************** +// * @brief Retrieve the built spatial map from the camera. Spatial mapping must be enabled. +// * This method takes in an std::future to eventually store the map in. +// * It returns a enum code representing the successful scheduling of building the map. +// * Any code other than SPATIAL_MAPPING_STATE::OK means the future will never be filled. +// * +// * @param std::future - The future to eventually store the map in. +// * @return sl::SPATIAL_MAPPING_STATE - Whether or not the building of the map was successfully scheduled. +// * Anything other than OK means the future will never be filled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// sl::SPATIAL_MAPPING_STATE ZEDCam::ExtractSpatialMapAsync(std::future& fuMeshFuture) +// { +// // Get and store current state of spatial mapping. +// sl::SPATIAL_MAPPING_STATE slReturnState = m_slCamera.getSpatialMappingState(); + +// // Check if spatial mapping has been enabled and ready +// if (slReturnState == sl::SPATIAL_MAPPING_STATE::OK) +// { +// // Request that the ZEDSDK begin processing the spatial map for export. +// m_slCamera.requestSpatialMapAsync(); + +// // Start an async thread to wait for spatial map processing to finish. Return resultant future object. +// fuMeshFuture = std::async(std::launch::async, +// [this]() +// { +// // Create instance variables. +// sl::Mesh slSpatialMap; + +// // Loop until map is finished generating. +// while (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) +// { +// // Sleep for 10ms. +// std::this_thread::sleep_for(std::chrono::milliseconds(10)); +// } + +// // Check if the spatial map was exported successfully. +// if (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::SUCCESS) +// { +// // Get and store the spatial map. +// m_slCamera.retrieveSpatialMapAsync(slSpatialMap); + +// // Return spatial map. +// return slSpatialMap; +// } +// else +// { +// // Submit logger message. +// LOG_ERROR(g_qSharedLogger, +// "Failed to extract ZED spatial map. sl::ERROR_CODE is: {}", +// sl::toString(m_slCamera.getSpatialMapRequestStatusAsync()).get()); + +// // Return empty point cloud. +// return sl::Mesh(); +// } +// }); +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "ZED spatial mapping was never enabled, can't extract spatial map!"); +// } + +// // Return current spatial mapping state. +// return slReturnState; +// } + +// /****************************************************************************** +// * @brief Accessor for if the cameras object detection and tracking feature is enabled. +// * +// * @return true - Object detection and tracking is enabled. +// * @return false - Object detection and tracking is not enabled. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetObjectDetectionEnabled() +// { +// return m_slCamera.isObjectDetectionEnabled(); +// } + +// /****************************************************************************** +// * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the +// * given sl::ObjectData vector. +// * +// * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. +// * @return true - The object data was successfully copied. +// * @return false - The object data was not successfully copied. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-27 +// ******************************************************************************/ +// bool ZEDCam::GetObjects(std::vector& vObjectData) +// { +// // Check if object detection has been enabled. +// if (m_slCamera.isObjectDetectionEnabled()) +// { +// // Assemble the data container. +// containers::DataFetchContainer&> stContainer(vObjectData); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qObjectDataCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving object data from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED object data but object detection/tracking is not enabled!"); +// // Return unsuccessful. +// return false; +// } +// } + +// /****************************************************************************** +// * @brief If batching is enabled, this retrieves the normal objects and passes them to +// * the the iternal batching queue of the zed api. This performs short-term re-identification +// * with deep learning and trajectories filtering. Batching must have been set to enabled when +// * EnableObjectDetection() was called. Most of the time the vector will be empty and will be +// * filled every ZED_OBJDETECTION_BATCH_LATENCY. +// * +// * @param vBatchedObjectData - A vector containing objects of sl::ObjectsBatch object that will +// * have object data copied to. +// * @return true - The batched objects data was successfully copied. +// * @return false - The batched objects data was not successfully copied. +// * +// * @author clayjay3 (claytonraycowen@gmail.com) +// * @date 2023-08-30 +// ******************************************************************************/ +// bool ZEDCam::GetBatchedObjects(std::vector& vBatchedObjectData) +// { +// // Check if object detection and batching has been enabled. +// if (m_slCamera.isObjectDetectionEnabled() && m_slObjectDetectionBatchParams.enable) +// { +// // Assemble the data container. +// containers::DataFetchContainer&> stContainer(vBatchedObjectData); + +// // Acquire lock on frame copy queue. +// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); +// // Append frame fetch container to the schedule queue. +// m_qObjectBatchedDataCopySchedule.push(stContainer); +// // Release lock on the frame schedule queue. +// lkSchedulers.unlock(); + +// // Create lock variable to be used by condition variable. CV unlocks this during wait(). +// std::unique_lock lkConditionLock(stContainer.muConditionMutex); +// // Wait up to 10 seconds for the condition variable to be notified. +// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); +// // Release lock. +// lkConditionLock.unlock(); + +// // Check condition variable status and return. +// if (cdStatus == std::cv_status::no_timeout) +// { +// // Image was successfully written to the given cv::Mat reference. +// return true; +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving batched object data from threadpool queue."); +// // Image was not written successfully. +// return false; +// } +// } +// else +// { +// // Submit logger message. +// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED batched object data but object detection/tracking is not enabled!"); +// // Return unsuccessful. +// return false; +// } +// } diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index 279641d7..de4fe608 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -1,132 +1,132 @@ -/****************************************************************************** - * @brief Defines the ZEDCam class. - * - * @file ZEDCam.h - * @author ClayJay3 (claytonraycowen@gmail.com) - * @date 2023-08-25 - * - * @copyright Copyright MRDT 2024 - All Rights Reserved - ******************************************************************************/ +// /****************************************************************************** +// * @brief Defines the ZEDCam class. +// * +// * @file ZEDCam.h +// * @author ClayJay3 (claytonraycowen@gmail.com) +// * @date 2023-08-25 +// * +// * @copyright Copyright MRDT 2024 - All Rights Reserved +// ******************************************************************************/ -#ifndef ZEDCAM_H -#define ZEDCAM_H +// #ifndef ZEDCAM_H +// #define ZEDCAM_H -#include -#include -#include -#include +// #include +// #include +// #include +// #include -#include "../../AutonomyConstants.h" -#include "../../interfaces/AutonomyThread.hpp" -#include "../../interfaces/Camera.hpp" -#include "../../util/vision/FetchContainers.hpp" +// #include "../../AutonomyConstants.h" +// #include "../../interfaces/AutonomyThread.hpp" +// #include "../../interfaces/Camera.hpp" +// #include "../../util/vision/FetchContainers.hpp" -class ZEDCam : public Camera, public AutonomyThread -{ - public: - ///////////////////////////////////////// - // Declare public structs that are specific to and used within this class. - ///////////////////////////////////////// - struct ZedObjectData; +// class ZEDCam : public Camera, public AutonomyThread +// { +// public: +// ///////////////////////////////////////// +// // Declare public structs that are specific to and used within this class. +// ///////////////////////////////////////// +// struct ZedObjectData; - ///////////////////////////////////////// - // Declare public methods and member variables. - ///////////////////////////////////////// - ZEDCam(const int nPropResolutionX, - const int nPropResolutionY, - const int nPropFramesPerSecond, - const double dPropHorizontalFOV, - const double dPropVerticalFOV, - const float fMinSenseDistance = constants::ZED_DEFAULT_MINIMUM_DISTANCE, - const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, - const bool bMemTypeGPU = false, - const bool bUseHalfDepthPrecision = false, - const unsigned int unCameraSerialNumber = 0); - ~ZEDCam(); - bool GrabFrame(cv::Mat& cvFrame) override; - bool GrabFrame(cv::cuda::GpuMat& cvGPUFrame); - bool GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); - bool GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); - bool GrabPointCloud(cv::Mat& cvPointCloud); - bool GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud); - sl::ERROR_CODE ResetPositionalTracking(); - sl::ERROR_CODE TrackCustomBoxObjects(std::vector& vCustomObjects); - sl::ERROR_CODE RebootCamera(); +// ///////////////////////////////////////// +// // Declare public methods and member variables. +// ///////////////////////////////////////// +// ZEDCam(const int nPropResolutionX, +// const int nPropResolutionY, +// const int nPropFramesPerSecond, +// const double dPropHorizontalFOV, +// const double dPropVerticalFOV, +// const float fMinSenseDistance = constants::ZED_DEFAULT_MINIMUM_DISTANCE, +// const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, +// const bool bMemTypeGPU = false, +// const bool bUseHalfDepthPrecision = false, +// const unsigned int unCameraSerialNumber = 0); +// ~ZEDCam(); +// bool GrabFrame(cv::Mat& cvFrame) override; +// bool GrabFrame(cv::cuda::GpuMat& cvGPUFrame); +// bool GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); +// bool GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); +// bool GrabPointCloud(cv::Mat& cvPointCloud); +// bool GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud); +// sl::ERROR_CODE ResetPositionalTracking(); +// sl::ERROR_CODE TrackCustomBoxObjects(std::vector& vCustomObjects); +// sl::ERROR_CODE RebootCamera(); - ///////////////////////////////////////// - // Setters for class member variables. - ///////////////////////////////////////// - sl::ERROR_CODE EnablePositionalTracking(); - void DisablePositionalTracking(); - sl::ERROR_CODE SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO); - sl::ERROR_CODE EnableSpatialMapping(const int nTimeoutSeconds = 10); - void DisableSpatialMapping(); - sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false); - void DisableObjectDetection(); +// ///////////////////////////////////////// +// // Setters for class member variables. +// ///////////////////////////////////////// +// sl::ERROR_CODE EnablePositionalTracking(); +// void DisablePositionalTracking(); +// sl::ERROR_CODE SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO); +// sl::ERROR_CODE EnableSpatialMapping(const int nTimeoutSeconds = 10); +// void DisableSpatialMapping(); +// sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false); +// void DisableObjectDetection(); - ///////////////////////////////////////// - // Accessors for class member variables. - ///////////////////////////////////////// - bool GetCameraIsOpen() override; - bool GetUsingGPUMem() const; - std::string GetCameraModel(); - unsigned int GetCameraSerial(); - bool GetPositionalPose(sl::Pose& slPose); - bool GetPositionalTrackingEnabled(); - bool GetIMUData(std::vector& vIMUValues); - sl::SPATIAL_MAPPING_STATE GetSpatialMappingState(); - sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future& fuMeshFuture); - bool GetObjectDetectionEnabled(); - bool GetObjects(std::vector& vObjectData); - bool GetBatchedObjects(std::vector& vBatchedObjectData); +// ///////////////////////////////////////// +// // Accessors for class member variables. +// ///////////////////////////////////////// +// bool GetCameraIsOpen() override; +// bool GetUsingGPUMem() const; +// std::string GetCameraModel(); +// unsigned int GetCameraSerial(); +// bool GetPositionalPose(sl::Pose& slPose); +// bool GetPositionalTrackingEnabled(); +// bool GetIMUData(std::vector& vIMUValues); +// sl::SPATIAL_MAPPING_STATE GetSpatialMappingState(); +// sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future& fuMeshFuture); +// bool GetObjectDetectionEnabled(); +// bool GetObjects(std::vector& vObjectData); +// bool GetBatchedObjects(std::vector& vBatchedObjectData); - private: - ///////////////////////////////////////// - // Declare private member variables. - ///////////////////////////////////////// - // ZED Camera specific. - sl::Camera m_slCamera; - std::shared_mutex m_muCameraMutex; - sl::InitParameters m_slCameraParams; - sl::RuntimeParameters m_slRuntimeParams; - sl::MEASURE m_slDepthMeasureType; - sl::SensorsData m_slSensorData; - sl::PositionalTrackingParameters m_slPoseTrackingParams; - sl::Pose m_slCameraPose; - sl::SpatialMappingParameters m_slSpatialMappingParams; - sl::ObjectDetectionParameters m_slObjectDetectionParams; - sl::BatchParameters m_slObjectDetectionBatchParams; - sl::Objects m_slDetectedObjects; - std::vector m_slDetectedObjectsBatched; - sl::MEM m_slMemoryType; - unsigned int m_unCameraSerialNumber; +// private: +// ///////////////////////////////////////// +// // Declare private member variables. +// ///////////////////////////////////////// +// // ZED Camera specific. +// sl::Camera m_slCamera; +// std::shared_mutex m_muCameraMutex; +// sl::InitParameters m_slCameraParams; +// sl::RuntimeParameters m_slRuntimeParams; +// sl::MEASURE m_slDepthMeasureType; +// sl::SensorsData m_slSensorData; +// sl::PositionalTrackingParameters m_slPoseTrackingParams; +// sl::Pose m_slCameraPose; +// sl::SpatialMappingParameters m_slSpatialMappingParams; +// sl::ObjectDetectionParameters m_slObjectDetectionParams; +// sl::BatchParameters m_slObjectDetectionBatchParams; +// sl::Objects m_slDetectedObjects; +// std::vector m_slDetectedObjectsBatched; +// sl::MEM m_slMemoryType; +// unsigned int m_unCameraSerialNumber; - // Mats for storing frames and measures. - sl::Mat m_slFrame; - sl::Mat m_slDepthImage; - sl::Mat m_slDepthMeasure; - sl::Mat m_slPointCloud; +// // Mats for storing frames and measures. +// sl::Mat m_slFrame; +// sl::Mat m_slDepthImage; +// sl::Mat m_slDepthMeasure; +// sl::Mat m_slPointCloud; - // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue>> m_qFrameCopySchedule; - std::queue>> m_qGPUFrameCopySchedule; - std::queue&>>> m_qCustomBoxInjestSchedule; - std::queue>> m_qPoseCopySchedule; - std::queue&>>> m_qIMUDataCopySchedule; - std::queue&>>> m_qObjectDataCopySchedule; - std::queue&>>> m_qObjectBatchedDataCopySchedule; - std::shared_mutex m_muPoolScheduleMutex; - std::mutex m_muFrameCopyMutex; - std::mutex m_muCustomBoxInjestMutex; - std::mutex m_muPoseCopyMutex; - std::mutex m_muIMUDataCopyMutex; - std::mutex m_muObjectDataCopyMutex; - std::mutex m_muObjectBatchedDataCopyMutex; +// // Queues and mutexes for scheduling and copying camera frames and data to other threads. +// std::queue>> m_qFrameCopySchedule; +// std::queue>> m_qGPUFrameCopySchedule; +// std::queue&>>> m_qCustomBoxInjestSchedule; +// std::queue>> m_qPoseCopySchedule; +// std::queue&>>> m_qIMUDataCopySchedule; +// std::queue&>>> m_qObjectDataCopySchedule; +// std::queue&>>> m_qObjectBatchedDataCopySchedule; +// std::shared_mutex m_muPoolScheduleMutex; +// std::mutex m_muFrameCopyMutex; +// std::mutex m_muCustomBoxInjestMutex; +// std::mutex m_muPoseCopyMutex; +// std::mutex m_muIMUDataCopyMutex; +// std::mutex m_muObjectDataCopyMutex; +// std::mutex m_muObjectBatchedDataCopyMutex; - ///////////////////////////////////////// - // Declare private methods. - ///////////////////////////////////////// - void ThreadedContinuousCode() override; - void PooledLinearCode() override; -}; -#endif +// ///////////////////////////////////////// +// // Declare private methods. +// ///////////////////////////////////////// +// void ThreadedContinuousCode() override; +// void PooledLinearCode() override; +// }; +// #endif From 987f5e18d3ba543f60c102688c3153354aabdfa5 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Wed, 27 Sep 2023 00:48:32 +0000 Subject: [PATCH 02/11] Frame copy futures now work for BasicCam and are implemented for ZEDCam. --- examples/vision/OpenBasicCam.hpp | 32 +- examples/vision/OpenZEDCam.hpp | 3 +- src/interfaces/Camera.hpp | 4 +- src/util/vision/FetchContainers.hpp | 100 +- src/vision/cameras/BasicCam.cpp | 10 +- src/vision/cameras/BasicCam.h | 2 +- src/vision/cameras/ZEDCam.cpp | 3020 +++++++++++++-------------- src/vision/cameras/ZEDCam.h | 242 +-- 8 files changed, 1660 insertions(+), 1753 deletions(-) diff --git a/examples/vision/OpenBasicCam.hpp b/examples/vision/OpenBasicCam.hpp index 67763191..56a5f674 100644 --- a/examples/vision/OpenBasicCam.hpp +++ b/examples/vision/OpenBasicCam.hpp @@ -29,14 +29,20 @@ void RunExample() BasicCam* TestCamera1 = g_pCameraHandler->GetBasicCam(CameraHandlerThread::eHeadLeftArucoEye); // Declare mats to store images in. cv::Mat cvNormalFrame1; + cv::Mat cvNormalFrame2; + + // Declare FPS counter. + IPS FPS = IPS(); // Loop forever, or until user hits ESC. while (true) { // Grab normal frame from camera. - std::future fuTest = TestCamera1->GrabFrame(cvNormalFrame1); - cvNormalFrame1 = fuTest.get(); - if (!cvNormalFrame1.empty()) + std::future fuCopyStatus1 = TestCamera1->RequestFrameCopy(cvNormalFrame1); + std::future fuCopyStatus2 = TestCamera1->RequestFrameCopy(cvNormalFrame2); + + // Show first frame copy. + if (fuCopyStatus1.get() && !cvNormalFrame1.empty()) { // Print info. LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); @@ -45,9 +51,27 @@ void RunExample() cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Display frame. - cv::imshow("BasicCamExample", cvNormalFrame1); + cv::imshow("BasicCamExample Frame1", cvNormalFrame1); + } + + // Show second frame copy. + if (fuCopyStatus2.get() && !cvNormalFrame2.empty()) + { + // Print info. + LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); + + // Put FPS on normal frame. + cv::putText(cvNormalFrame2, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + + // Display frame. + cv::imshow("BasicCamExample Frame2", cvNormalFrame2); } + // Tick FPS counter. + FPS.Tick(); + // Print FPS of main loop. + LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetAverageIPS()); + char chKey = cv::waitKey(1); if (chKey == 27) // Press 'Esc' key to exit break; diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index 25759bc3..e45565ad 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/examples/vision/OpenZEDCam.hpp @@ -77,7 +77,6 @@ void RunExample() // Print info. LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); - LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetExactIPS()); LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z); LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]); // Check if spatial mapping is enabled. @@ -89,6 +88,8 @@ void RunExample() // Tick FPS counter. FPS.Tick(); + // Print FPS of main loop. + LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetAverageIPS()); char chKey = cv::waitKey(1); if (chKey == 27) // Press 'Esc' key to exit diff --git a/src/interfaces/Camera.hpp b/src/interfaces/Camera.hpp index c80820ca..ec874c13 100644 --- a/src/interfaces/Camera.hpp +++ b/src/interfaces/Camera.hpp @@ -68,8 +68,8 @@ class Camera double m_dPropVerticalFOV; // Declare interface class pure virtual functions. (These must be overriden by inheritor.) - virtual std::future GrabFrame(T& tFrame) = 0; // This is where the code to retrieve an image from the camera is put. - virtual bool GetCameraIsOpen() = 0; // This is where the code to check if the camera is current open goes. + virtual std::future RequestFrameCopy(T& tFrame) = 0; // This is where the code to retrieve an image from the camera is put. + virtual bool GetCameraIsOpen() = 0; // This is where the code to check if the camera is current open goes. // Declare protected object pointers. IPS m_IPS = IPS(); diff --git a/src/util/vision/FetchContainers.hpp b/src/util/vision/FetchContainers.hpp index 198cb80b..ae3939dd 100644 --- a/src/util/vision/FetchContainers.hpp +++ b/src/util/vision/FetchContainers.hpp @@ -29,9 +29,16 @@ namespace containers { /****************************************************************************** - * @brief This struct is used to contain camera frames for scheduling and copying. - * It also stores a condition variable and mutex for signaling when the copy - * has been completed. + * @brief This struct is used to carry references to camera frames for scheduling and copying. + * It is constructed so that a reference to a frame is passed into the container and the pointer + * to that reference is stored internally. Then a shared_pointer to a new std::promise is created, + * which allows the programmer to return a future from whatever method created this container. + * The future can then be waited on before the passed in frame is used. The future will return a true or false. + * + * The idea is that any threads that call a grab/retrieve method of a camera object (which also runs in a different thread) + * will have the empty frame that they passed in be put into the camera queues and then the grab/retrieve + * method will immediately return. This allows BOTH threads to continue processing other things or request + * multiple frame copies non-sequentially/serially. * * @tparam T - The mat type that the struct will be containing. * @@ -43,9 +50,9 @@ namespace containers { public: // Declare and define public struct member variables. - std::shared_ptr pFrame; + T* pFrame; PIXEL_FORMATS eFrameType; - std::shared_ptr> pCopiedFrame; + std::shared_ptr> pCopiedFrameStatus; /****************************************************************************** * @brief Construct a new Frame Fetch Container object. @@ -57,23 +64,39 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : - pFrame(std::make_shared(tFrame)), eFrameType(eFrameType), pCopiedFrame(std::make_shared>()) + FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : pFrame(&tFrame), eFrameType(eFrameType), pCopiedFrameStatus(std::make_shared>()) {} + /****************************************************************************** + * @brief Copy Construct a new Frame Fetch Container object. + * + * @param stOtherFrameContainer - FrameFetchContainer to copy pointers and values from. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ FrameFetchContainer(const FrameFetchContainer& stOtherFrameContainer) : - pFrame(stOtherFrameContainer.pFrame), eFrameType(stOtherFrameContainer.eFrameType), pCopiedFrame(stOtherFrameContainer.pCopiedFrame) + pFrame(stOtherFrameContainer.pFrame), eFrameType(stOtherFrameContainer.eFrameType), pCopiedFrameStatus(stOtherFrameContainer.pCopiedFrameStatus) {} + /****************************************************************************** + * @brief Operator equals for FrameFetchContainer. Shallow Copy. + * + * @param stOtherFrameContainer - FrameFetchContainer to copy pointers and values from. + * @return FrameFetchContainer& - A reference to this object. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ FrameFetchContainer& operator=(const FrameFetchContainer& stOtherFrameContainer) { // Check if the passed in container is the same as this one. if (this != &stOtherFrameContainer) { // Copy struct attributes. - this->pFrame = stOtherFrameContainer.pFrame; - this->eFrameType = stOtherFrameContainer.eFrameType; - this->pCopiedFrame = stOtherFrameContainer.pCopiedFrame; + this->pFrame = stOtherFrameContainer.pFrame; + this->eFrameType = stOtherFrameContainer.eFrameType; + this->pCopiedFrameStatus = stOtherFrameContainer.pCopiedFrameStatus; } // Return pointer to this object which now contains the copied values. @@ -82,9 +105,16 @@ namespace containers }; /****************************************************************************** - * @brief The struct is used to contain any datatype for scheduling and copying. - * It also stores a condition variable and mutex for signaling when the copy - * has been completed. + * @brief This struct is used to carry references to any datatype for scheduling and copying. + * It is constructed so that a reference to the data object is passed into the container and the pointer + * to that reference is stored internally. Then a shared_pointer to a new std::promise is created, + * which allows the programmer to return a future from whatever method created this container. + * The future can then be waited on before the data is used. The future will return a true or false. + * + * The idea is that any threads that call a grab/retrieve method of an object (which also runs in a different thread) + * will have the data reference that they passed in be put into the called objects queues and then the grab/retrieve + * method will immediately return. This allows BOTH threads to continue processing other things or request + * multiple data copies non-sequentially/serially. * * @tparam T - The type of data object that this struct will be containing. * @@ -96,9 +126,8 @@ namespace containers { public: // Declare and define public struct member variables. - std::condition_variable cdMatWriteSuccess; - std::mutex muConditionMutex; - T& tData; + T* pData; + std::shared_ptr> pCopiedDataStatus; /****************************************************************************** * @brief Construct a new Frame Fetch Container object. @@ -108,7 +137,42 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - DataFetchContainer(T& tData) : tData(tData) {} + DataFetchContainer(T& tData) : pData(&tData) {} + + /****************************************************************************** + * @brief Copy Construct a new Frame Fetch Container object. + * + * @param stOtherDataContainer - DataFetchContainer to copy pointers and values from. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + DataFetchContainer(const DataFetchContainer& stOtherDataContainer) : + pData(stOtherDataContainer.pData), pCopiedDataStatus(stOtherDataContainer.pCopiedDataStatus) + {} + + /****************************************************************************** + * @brief Operator equals for FrameFetchContainer. Shallow Copy. + * + * @param stOtherDataContainer - DataFetchContainer to copy pointers and values from. + * @return DataFetchContainer& - A reference to this object. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + DataFetchContainer& operator=(const DataFetchContainer& stOtherDataContainer) + { + // Check if the passed in container is the same as this one. + if (this != &stOtherDataContainer) + { + // Copy struct attributes. + this->pData = stOtherDataContainer.pData; + this->pCopiedDataStatus = stOtherDataContainer.pCopiedDataStatus; + } + + // Return pointer to this object which now contains the copied values. + return *this; + } }; } // namespace containers diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index 0f45f9c9..84feb2f1 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -204,6 +204,8 @@ void BasicCam::PooledLinearCode() // Copy frame to data container. *(stContainer.pFrame) = m_cvFrame.clone(); + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); } } @@ -213,13 +215,13 @@ void BasicCam::PooledLinearCode() * class/thread calls it. * * @param cvFrame - A reference to the cv::Mat to store the frame in. - * @return true - Frame successfully retrieved and stored. - * @return false - Frame was not stored successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ -std::future BasicCam::GrabFrame(cv::Mat& cvFrame) +std::future BasicCam::RequestFrameCopy(cv::Mat& cvFrame) { // Assemble the FrameFetchContainer. containers::FrameFetchContainer stContainer(cvFrame, m_ePropPixelFormat); @@ -232,7 +234,7 @@ std::future BasicCam::GrabFrame(cv::Mat& cvFrame) lkScheduler.unlock(); // Return the future from the promise stored in the container. - return stContainer.pCopiedFrame->get_future(); + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** diff --git a/src/vision/cameras/BasicCam.h b/src/vision/cameras/BasicCam.h index b8ef99c1..113df6b7 100644 --- a/src/vision/cameras/BasicCam.h +++ b/src/vision/cameras/BasicCam.h @@ -39,7 +39,7 @@ class BasicCam : public Camera, public AutonomyThread const double dPropHorizontalFOV, const double dPropVerticalFOV); ~BasicCam(); - std::future GrabFrame(cv::Mat& cvFrame) override; + std::future RequestFrameCopy(cv::Mat& cvFrame) override; ///////////////////////////////////////// // Getters. diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 5f93b4c1..ba55caab 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -1,1602 +1,1418 @@ -// /****************************************************************************** -// * @brief Implements the ZEDCam class. -// * -// * @file ZEDCam.cpp -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// * -// * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved -// ******************************************************************************/ - -// #include "ZEDCam.h" -// #include "../../AutonomyLogging.h" -// #include "../../util/vision/ImageOperations.hpp" - -// /****************************************************************************** -// * @brief This struct is part of the ZEDCam class and is used as a container for all -// * bounding box data that is going to be passed to the zed api via the ZEDCam's -// * TrackCustomBoxObjects() method. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-29 -// ******************************************************************************/ -// struct ZEDCam::ZedObjectData - -// { -// private: -// // Declare and define private struct member variables. -// std::string szObjectUUID = sl::generate_unique_id().get(); // This will automatically generate a guaranteed unique id so the object is traceable. - -// // Declare a private struct for holding point data. -// /****************************************************************************** -// * @brief This struct is internal to the ZedObjectData struct is used to store -// * an X and Y value for the corners of a bounding box. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-29 -// ******************************************************************************/ -// struct Corner -// { -// public: -// // Declare public struct member variables. -// unsigned int nX; -// unsigned int nY; -// }; - -// public: -// // Declare and define public struct member variables. -// struct Corner CornerTL; // The top left corner of the bounding box. -// struct Corner CornerTR; // The top right corner of the bounding box. -// struct Corner CornerBL; // The bottom left corner of the bounding box. -// struct Corner CornerBR; // The bottom right corner of bounding box. -// int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the -// same. float fConfidence; // This info is passed through from your detection algorithm and will help improve tracking by throwing out bad detections. -// // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class -// number -// // to avoid issues. -// bool bObjectRemainsOnFloorPlane = false; - -// // Declare and define public struct getters. -// std::string GetObjectUUID() { return szObjectUUID; }; -// }; - -// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -// /****************************************************************************** -// * @brief Construct a new Zed Cam:: Zed Cam object. -// * -// * @param nPropResolutionX - X res of camera. Must be smaller than ZED_BASE_RESOLUTION. -// * @param nPropResolutionY - Y res of camera. Must be smaller than ZED_BASE_RESOLUTION. -// * @param nPropFramesPerSecond - FPS camera is running at. -// * @param dPropHorizontalFOV - The horizontal field of view. -// * @param dPropVerticalFOV - The vertical field of view. -// * @param fMinSenseDistance - The minimum distance to include in depth measures. -// * @param fMaxSenseDistance - The maximim distance to include in depth measures. -// * @param bMemTypeGPU - Whether or not to use the GPU memory for operations. -// * @param unCameraSerialNumber - The serial number of the camera to open. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// ZEDCam::ZEDCam(const int nPropResolutionX, -// const int nPropResolutionY, -// const int nPropFramesPerSecond, -// const double dPropHorizontalFOV, -// const double dPropVerticalFOV, -// const float fMinSenseDistance, -// const float fMaxSenseDistance, -// const bool bMemTypeGPU, -// const bool bUseHalfDepthPrecision, -// const unsigned int unCameraSerialNumber) : -// Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) -// { -// // Assign member variables. -// bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; -// bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; -// m_unCameraSerialNumber = unCameraSerialNumber; - -// // Setup camera params. -// m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; -// m_slCameraParams.camera_fps = nPropFramesPerSecond; -// m_slCameraParams.coordinate_units = constants::ZED_MEASURE_UNITS; -// m_slCameraParams.coordinate_system = constants::ZED_COORD_SYSTEM; -// m_slCameraParams.depth_mode = constants::ZED_DEPTH_MODE; -// m_slCameraParams.depth_minimum_distance = fMinSenseDistance; -// m_slCameraParams.depth_maximum_distance = fMaxSenseDistance; -// m_slCameraParams.depth_stabilization = constants::ZED_DEPTH_STABILIZATION; -// // Only set serial number if necessary. -// if (unCameraSerialNumber != static_cast(0)) -// { -// m_slCameraParams.input.setFromSerialNumber(unCameraSerialNumber); -// } - -// // Setup camera runtime params. -// m_slRuntimeParams.enable_fill_mode = constants::ZED_SENSING_FILL; - -// // Setup positional tracking parameters. -// m_slPoseTrackingParams.mode = constants::ZED_POSETRACK_MODE; -// m_slPoseTrackingParams.enable_area_memory = constants::ZED_POSETRACK_AREA_MEMORY; -// m_slPoseTrackingParams.enable_pose_smoothing = constants::ZED_POSETRACK_POSE_SMOOTHING; -// m_slPoseTrackingParams.set_floor_as_origin = constants::ZED_POSETRACK_FLOOR_IS_ORIGIN; -// m_slPoseTrackingParams.enable_imu_fusion = constants::ZED_POSETRACK_ENABLE_IMU_FUSION; -// m_slPoseTrackingParams.depth_min_range = constants::ZED_POSETRACK_USABLE_DEPTH_MIN; -// m_slPoseTrackingParams.set_gravity_as_origin = constants::ZED_POSETRACK_USE_GRAVITY_ORIGIN; - -// // Setup spatial mapping parameters. -// m_slSpatialMappingParams.map_type = constants::ZED_MAPPING_TYPE; -// m_slSpatialMappingParams.resolution_meter = constants::ZED_MAPPING_RESOLUTION_METER; -// m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); -// m_slSpatialMappingParams.save_texture = true; -// m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; -// m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; - -// // Setup object detection/tracking parameters. -// m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; -// m_slObjectDetectionParams.image_sync = constants::ZED_OBJDETECTION_IMG_SYNC; -// m_slObjectDetectionParams.enable_tracking = constants::ZED_OBJDETECTION_TRACK_OBJ; -// m_slObjectDetectionParams.enable_segmentation = constants::ZED_OBJDETECTION_SEGMENTATION; -// m_slObjectDetectionParams.filtering_mode = constants::ZED_OBJDETECTION_FILTERING; -// m_slObjectDetectionParams.prediction_timeout_s = constants::ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT; -// // Setup object detection/tracking batch parameters. -// m_slObjectDetectionBatchParams.enable = false; -// m_slObjectDetectionBatchParams.id_retention_time = constants::ZED_OBJDETECTION_BATCH_RETENTION_TIME; -// m_slObjectDetectionBatchParams.latency = constants::ZED_OBJDETECTION_BATCH_LATENCY; -// m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; - -// // Attempt to open camera. -// sl::ERROR_CODE slReturnCode = m_slCamera.open(m_slCameraParams); -// // Check if the camera was successfully opened. -// if (m_slCamera.isOpened()) -// { -// // Submit logger message. -// LOG_DEBUG(g_qSharedLogger, -// "{} stereo camera with serial number {} has been succsessfully opened.", -// this->GetCameraModel(), -// m_slCamera.getCameraInformation().serial_number); -// } -// else -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Unable to open stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } -// } - -// /****************************************************************************** -// * @brief Destroy the Zed Cam:: Zed Cam object. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// ZEDCam::~ZEDCam() -// { -// // Stop threaded code. -// this->RequestStop(); -// this->Join(); - -// // Close the ZEDCam. -// m_slCamera.close(); -// } - -// /****************************************************************************** -// * @brief The code inside this private method runs in a seperate thread, but still -// * has access to this*. This method continuously calls the grab() function of -// * the ZEDSDK, which updates all frames (RGB, depth, cloud) and all other data -// * such as positional and spatial mapping. Then a thread pool is started and joined -// * once per iteration to mass copy the frames and/or measure to any other thread -// * waiting in the queues. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-09-01 -// ******************************************************************************/ -// void ZEDCam::ThreadedContinuousCode() -// { -// // Check if camera is opened. -// if (!m_slCamera.isOpened()) -// { -// // Shutdown threads for this ZEDCam. -// this->RequestStop(); -// // Submit logger message. -// LOG_CRITICAL(g_qSharedLogger, -// "Camera start was attempted for camera at {}, but camera never properly opened or it has been closed/rebooted!", -// m_unCameraSerialNumber); -// } -// else -// { -// // Acquire write lock for camera object. -// std::unique_lock lkSharedCameraLock(m_muCameraMutex); -// // Call generalized update method of zed api. -// sl::ERROR_CODE slReturnCode = m_slCamera.grab(m_slRuntimeParams); -// // Check if new frame was computed successfully. -// if (slReturnCode == sl::ERROR_CODE::SUCCESS) -// { -// // Grab regular image and store it in member variable. -// slReturnCode = m_slCamera.retrieveImage(m_slFrame, constants::ZED_RETRIEVE_VIEW, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new frame image for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Grab depth measure and store it in member variable. -// slReturnCode = m_slCamera.retrieveMeasure(m_slDepthMeasure, m_slDepthMeasureType, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new depth measure for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Grab depth grayscale image and store it in member variable. -// slReturnCode = m_slCamera.retrieveImage(m_slDepthImage, sl::VIEW::DEPTH, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new depth image for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Grab regular resized image and store it in member variable. -// slReturnCode = m_slCamera.retrieveMeasure(m_slPointCloud, sl::MEASURE::XYZ, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new point cloud for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Get and store the SensorData object from the camera. Get data from the most recent image grab. -// // Using TIME_REFERENCE::CURRENT requires high rate polling and can introduce error as the most recent -// // IMU data could be in the future of the camera image. -// slReturnCode = m_slCamera.getSensorsData(m_slSensorData, sl::TIME_REFERENCE::IMAGE); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new sensors data for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Check if positional tracking is enabled. -// if (m_slCamera.isPositionalTrackingEnabled()) -// { -// // Get the current pose of the camera. -// sl::POSITIONAL_TRACKING_STATE slPoseTrackReturnCode = m_slCamera.getPosition(m_slCameraPose, sl::REFERENCE_FRAME::WORLD); -// // Check that the regular frame was retrieved successfully. -// if (slPoseTrackReturnCode != sl::POSITIONAL_TRACKING_STATE::OK) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new positional tracking pose for stereo camera {} ({})! sl::POSITIONAL_TRACKING_STATE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slPoseTrackReturnCode).get()); -// } -// } -// // Check if object detection is enabled. -// if (m_slCamera.isObjectDetectionEnabled()) -// { -// // Get updated objects from camera. -// slReturnCode = m_slCamera.retrieveObjects(m_slDetectedObjects); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Check if batched object data is enabled. -// if (m_slObjectDetectionBatchParams.enable) -// { -// // Get updated batched objects from camera. -// slReturnCode = m_slCamera.getObjectsBatch(m_slDetectedObjectsBatched); -// // Check that the regular frame was retrieved successfully. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Unable to retrieve new batched object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } -// } -// } -// // Release camera lock. -// lkSharedCameraLock.unlock(); - -// // Call FPS tick. -// m_IPS.Tick(); -// } -// else -// { -// // Release camera lock. -// lkSharedCameraLock.unlock(); - -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Unable to update stereo camera {} ({}) frames, measurements, and sensors! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Acquire a shared_lock on the frame copy queue. -// std::shared_lock lkSchedulers(m_muPoolScheduleMutex); -// // Check if the frame copy queue is empty. -// if (!m_qFrameCopySchedule.empty() || !m_qGPUFrameCopySchedule.empty() || !m_qCustomBoxInjestSchedule.empty() || !m_qPoseCopySchedule.empty() || -// !m_qIMUDataCopySchedule.empty() || !m_qObjectDataCopySchedule.empty() || !m_qObjectBatchedDataCopySchedule.empty()) -// { -// // Find the queue with the longest length. -// size_t siMaxQueueLength = std::max({m_qFrameCopySchedule.size(), -// m_qGPUFrameCopySchedule.size(), -// m_qCustomBoxInjestSchedule.size(), -// m_qPoseCopySchedule.size(), -// m_qIMUDataCopySchedule.size(), -// m_qObjectDataCopySchedule.size(), -// m_qObjectBatchedDataCopySchedule.size()}); - -// // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. -// this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); -// // Wait for thread pool to finish. -// this->JoinPool(); -// // Release lock on frame copy queue. -// lkSchedulers.unlock(); -// } -// } -// } - -// /****************************************************************************** -// * @brief This method holds the code that is ran in the thread pool started by -// * the ThreadedLinearCode() method. It copies the data from the different -// * data objects to references of the same type stored in a vector queued up by the -// * Grab methods. -// * -// * -// * @author ClayJay3 (claytonraycowen@gmail.com) -// * @date 2023-09-08 -// ******************************************************************************/ -// void ZEDCam::PooledLinearCode() -// { -// ///////////////////////////// -// // Frame queue. -// ///////////////////////////// -// // Check if we are using CPU or GPU mats. -// if (m_slMemoryType == sl::MEM::CPU) -// { -// // Aqcuire mutex for getting frames out of the queue. -// std::unique_lock lkFrameQueue(m_muFrameCopyMutex); -// // Check if the queue is empty. -// if (!m_qFrameCopySchedule.empty()) -// { -// // Get frame container out of queue. -// containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); -// // Pop out of queue. -// m_qFrameCopySchedule.pop(); -// // Release lock. -// lkFrameQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); - -// // Determine which frame should be copied. -// switch (stContainer.eFrameType) -// { -// case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; -// case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; -// case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; -// case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; -// default: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; -// } - -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } -// } -// // Use GPU mat. -// else -// { -// // Check if the queue is empty. -// if (!m_qGPUFrameCopySchedule.empty()) -// { -// // Aqcuire mutex for getting frames out of the queue. -// std::unique_lock lkFrameQueue(m_muFrameCopyMutex); -// // Get frame container out of queue. -// containers::FrameFetchContainer& stContainer = m_qGPUFrameCopySchedule.front(); -// // Pop out of queue. -// m_qGPUFrameCopySchedule.pop(); -// // Release lock. -// lkFrameQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); - -// // Determine which frame should be copied. -// switch (stContainer.eFrameType) -// { -// case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; -// case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; -// case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; -// case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; -// default: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; -// } - -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } -// } - -// ///////////////////////////// -// // Pose queue. -// ///////////////////////////// -// // Aqcuire mutex for getting frames out of the pose queue. -// std::unique_lock lkPoseQueue(m_muPoseCopyMutex); -// // Check if the queue is empty. -// if (!m_qPoseCopySchedule.empty()) -// { -// // Get frame container out of queue. -// containers::DataFetchContainer& stContainer = m_qPoseCopySchedule.front(); -// // Pop out of queue. -// m_qPoseCopySchedule.pop(); -// // Release lock. -// lkPoseQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Copy pose. -// stContainer.tData = sl::Pose(m_slCameraPose); -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } - -// ///////////////////////////// -// // IMU data queue. -// ///////////////////////////// -// // Aqcuire mutex for getting frames out of the pose queue. -// std::unique_lock lkIMUQueue(m_muIMUDataCopyMutex); -// // Check if the queue is empty. -// if (!m_qIMUDataCopySchedule.empty()) -// { -// // Get frame container out of queue. -// containers::DataFetchContainer&>& stContainer = m_qIMUDataCopySchedule.front(); -// // Pop out of queue. -// m_qIMUDataCopySchedule.pop(); -// // Release lock. -// lkIMUQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); - -// // Get IMU orientation in degrees. -// sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); -// // Get IMU linear acceleration. -// sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; -// // Repackage angles and accels into vector. -// stContainer.tData.emplace_back(slAngles.x); -// stContainer.tData.emplace_back(slAngles.y); -// stContainer.tData.emplace_back(slAngles.z); -// stContainer.tData.emplace_back(slLinearAccels.x); -// stContainer.tData.emplace_back(slLinearAccels.y); -// stContainer.tData.emplace_back(slLinearAccels.z); - -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } - -// ///////////////////////////// -// // ObjectData queue. -// ///////////////////////////// -// // Aqcuire mutex for getting frames out of the pose queue. -// std::unique_lock lkObjectDataQueue(m_muObjectDataCopyMutex); -// // Check if the queue is empty. -// if (!m_qObjectDataCopySchedule.empty()) -// { -// // Get frame container out of queue. -// containers::DataFetchContainer&>& stContainer = m_qObjectDataCopySchedule.front(); -// // Pop out of queue. -// m_qObjectDataCopySchedule.pop(); -// // Release lock. -// lkIMUQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) -// stContainer.tData = m_slDetectedObjects.object_list; -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } - -// ///////////////////////////// -// // ObjectData Batched queue. -// ///////////////////////////// -// // Aqcuire mutex for getting frames out of the pose queue. -// std::unique_lock lkObjectBatchedDataQueue(m_muObjectBatchedDataCopyMutex); -// // Check if the queue is empty. -// if (!m_qObjectBatchedDataCopySchedule.empty()) -// { -// // Get frame container out of queue. -// containers::DataFetchContainer&>& stContainer = m_qObjectBatchedDataCopySchedule.front(); -// // Pop out of queue. -// m_qObjectBatchedDataCopySchedule.pop(); -// // Release lock. -// lkIMUQueue.unlock(); - -// // Acquire unique lock on container. -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) -// stContainer.tData = m_slDetectedObjectsBatched; -// // Release lock. -// lkConditionLock.unlock(); -// // Use the condition variable to notify other waiting threads that this thread is finished. -// stContainer.cdMatWriteSuccess.notify_all(); -// } -// } - -// /****************************************************************************** -// * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. -// * Remember this code will be ran in whatever class/thread calls it. -// * -// * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author ClayJay3 (claytonraycowen@gmail.com) -// * @date 2023-09-09 -// ******************************************************************************/ -// bool ZEDCam::GrabFrame(cv::Mat& cvFrame) -// { -// // Assemble the FrameFetchContainer. -// containers::FrameFetchContainer stContainer(cvFrame, eBGRA); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and -// * stores it in a GPU mat. -// * Remember this code will be ran in whatever class/thread calls it. -// * -// * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author ClayJay3 (claytonraycowen@gmail.com) -// * @date 2023-09-09 -// ******************************************************************************/ -// bool ZEDCam::GrabFrame(cv::cuda::GpuMat& cvGPUFrame) -// { -// // Assemble the FrameFetchContainer. -// containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qGPUFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Grabs a depth measure or image from the camera. This image has the same shape as -// * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in -// * AutonomyConstants.h. -// * -// * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. -// * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image -// * purposes other than displaying depth. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) -// { -// // Create instance variables. -// PIXEL_FORMATS eFrameType; - -// // Check if the container should be set to retrieve an image or a measure. -// bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; -// // Assemble container. -// containers::FrameFetchContainer stContainer(cvDepth, eFrameType); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as -// * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in -// * AutonomyConstants.h. -// * -// * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. -// * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image -// * purposes other than displaying depth. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) -// { -// // Create instance variables. -// PIXEL_FORMATS eFrameType; - -// // Check if the container should be set to retrieve an image or a measure. -// bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; -// // Assemble container. -// containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qGPUFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal -// * image but with three XYZ values replacing the old color values in the 3rd dimension. -// * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM -// * constants set in AutonomyConstants.h. -// * -// * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) -// { -// // Assemble the FrameFetchContainer. -// containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal -// * image but with three XYZ values replacing the old color values in the 3rd dimension. -// * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM -// * constants set in AutonomyConstants.h. -// * -// * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. -// * @return true - The frame was successfully copied. -// * @return false - The frame was not copied successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// bool ZEDCam::GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud) -// { -// // Assemble the FrameFetchContainer. -// containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qGPUFrameCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Resets the cameras X,Y,Z translation and Roll,Pitch,Yaw orientation back -// * to 0. THINK CAREFULLY! Do you actually want to reset this? It will also realign -// * the coordinate system to whichever way the camera happens to be facing. -// * -// * @return sl::ERROR_CODE - Status of the positional tracking reset. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::ResetPositionalTracking() -// { -// // Create new translation to set position back to zero. -// sl::Translation slZeroTranslation(0.0, 0.0, 0.0); -// // This will reset position and coordinate frame. -// sl::Rotation slZeroRotation; -// slZeroRotation.setEulerAngles(sl::float3(0.0, 0.0, 0.0), false); - -// // Store new translation and rotation in a tranform object. -// sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); - -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Reset the positional tracking location of the camera. -// return m_slCamera.resetPositionalTracking(slZeroTransform); -// } - -// /****************************************************************************** -// * @brief A vector containing CustomBoxObjectData objects. These objects simply store -// * information about your detected objects from an external object detection model. -// * You will need to take your inference results and package them into a sl::CustomBoxObjectData -// * so the the ZEDSDK can properly interpret your detections. -// * -// * Giving the bounding boxes of your detected objects to the ZEDSDK will enable positional -// * tracking and velocity estimation for each object. Even when not in view. The IDs of objects -// * will also become persistent. -// * -// * @param vCustomObjects - A vector of sl::CustomBoxObjectData objects. -// * @return sl::ERROR_CODE - The return status of ingestion. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::TrackCustomBoxObjects(std::vector& vCustomObjects) -// { -// // Create instance varables. -// std::vector vCustomBoxData; - -// // Repack detection data into sl specific object. -// for (ZedObjectData stObjectData : vCustomObjects) -// { -// // Create new sl CustomBoxObjectData struct. -// sl::CustomBoxObjectData slCustomBox; -// std::vector vCorners; - -// // Assign simple attributes. -// slCustomBox.unique_object_id = sl::String(stObjectData.GetObjectUUID().c_str()); -// slCustomBox.label = stObjectData.nClassNumber; -// slCustomBox.probability = stObjectData.fConfidence; -// slCustomBox.is_grounded = stObjectData.bObjectRemainsOnFloorPlane; -// // Repackage object corner data. -// vCorners.emplace_back(sl::uint2(stObjectData.CornerTL.nX, stObjectData.CornerTL.nY)); -// vCorners.emplace_back(sl::uint2(stObjectData.CornerTR.nX, stObjectData.CornerTR.nY)); -// vCorners.emplace_back(sl::uint2(stObjectData.CornerBL.nX, stObjectData.CornerBL.nY)); -// vCorners.emplace_back(sl::uint2(stObjectData.CornerBR.nX, stObjectData.CornerBR.nY)); -// slCustomBox.bounding_box_2d = vCorners; - -// // Append repackaged object to vector. -// vCustomBoxData.emplace_back(slCustomBox); -// } - -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Give the custom box data to the zed api. -// sl::ERROR_CODE slReturnCode = m_slCamera.ingestCustomBoxObjects(vCustomBoxData); -// // Release lock. -// lkSharedLock.unlock(); - -// // Check if successful. -// if (slReturnCode == sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, -// "Failed to ingest new objects for camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Return error code. -// return slReturnCode; -// } - -// /****************************************************************************** -// * @brief Performs a hardware reset of the ZED2 or ZED2i camera. -// * -// * @return sl::ERROR_CODE - Whether or not the camera reboot was successful. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::RebootCamera() -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Reboot this camera and return the status code. -// return sl::Camera::reboot(m_slCamera.getCameraInformation().serial_number); -// } - -// /****************************************************************************** -// * @brief Enable the positional tracking functionality of the camera. -// * -// * @return sl::ERROR_CODE - Whether or not positional tracking was successfully enabled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::EnablePositionalTracking() -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Enable pose tracking and store return code. -// sl::ERROR_CODE slReturnCode = m_slCamera.enablePositionalTracking(m_slPoseTrackingParams); -// // Release lock. -// lkSharedLock.unlock(); - -// // Check if positional tracking was enabled properly. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Failed to enabled positional tracking for camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Return error code. -// return slReturnCode; -// } - -// /****************************************************************************** -// * @brief Disable to positional tracking funcionality of the camera. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-26 -// ******************************************************************************/ -// void ZEDCam::DisablePositionalTracking() -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Disable pose tracking. -// m_slCamera.disablePositionalTracking(); -// } - -// /****************************************************************************** -// * @brief Sets the pose of the positional tracking of the camera. XYZ will point -// * in their respective directions according to ZED_COORD_SYSTEM defined in -// * AutonomyConstants.h. -// * -// * Warning: This method is slow and should not be called in a loop. Setting the pose -// * will temporarily block the entire camera from grabbed or copying frames to -// * new threads. This method should only be called occasionally when absolutely needed. -// * -// * @param dX - The X position of the camera in ZED_MEASURE_UNITS. -// * @param dY - The Y position of the camera in ZED_MEASURE_UNITS. -// * @param dZ - The Z position of the camera in ZED_MEASURE_UNITS. -// * @param dXO - The tilt of the camera around the X axis in degrees. -// * @param dYO - The tilt of the camera around the Y axis in degrees. -// * @param dZO - The tilt of the camera around the Z axis in degrees. -// * @return sl::ERROR_CODE - Whether or not the pose was set successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) -// { -// // Create new translation to set position back to user given values. -// sl::Translation slZeroTranslation(dX, dY, dZ); -// // This will reset position and coordinate frame. -// sl::Rotation slZeroRotation; -// slZeroRotation.setEulerAngles(sl::float3(dXO, dYO, dZO), false); - -// // Store new translation and rotation in a tranform object. -// sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); - -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Reset the positional tracking location of the camera. -// return m_slCamera.resetPositionalTracking(slZeroTransform); -// } - -// /****************************************************************************** -// * @brief Enabled the spatial mapping feature of the camera. Pose tracking will be -// * enabled if it is not already. -// * -// * @param fTimeoutSeconds - The timeout used to wait for pose tracking to be on the OK state. Default is 10 seconds. -// * @return sl::ERROR_CODE - Whether or not spatial mapping was successfully enabled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::EnableSpatialMapping(const int nTimeoutSeconds) -// { -// // Create instance variables. -// auto tmStartTime = std::chrono::steady_clock::now(); -// sl::Pose slCameraPose; -// sl::ERROR_CODE slReturnCode; - -// // Check if positional tracking is enabled. -// if (!m_slCamera.isPositionalTrackingEnabled()) -// { -// // Enable positional tracking. -// this->EnablePositionalTracking(); -// } - -// // Wait for positional tracking state to be OK. Defualt Timeout of 10 seconds. -// while (m_slCamera.getPosition(slCameraPose) != sl::POSITIONAL_TRACKING_STATE::OK && -// std::chrono::steady_clock::now() - tmStartTime <= std::chrono::seconds(nTimeoutSeconds)) -// { -// // Sleep for one millisecond. -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// } - -// // Final check if positional tracking was successfully enabled. -// if (m_slCamera.getPosition(slCameraPose) == sl::POSITIONAL_TRACKING_STATE::OK) -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Enable spatial mapping. -// slReturnCode = m_slCamera.enableSpatialMapping(m_slSpatialMappingParams); -// // Release lock. -// lkSharedLock.unlock(); - -// // Check if positional tracking was enabled properly. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Failed to enabled spatial mapping for camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } -// } -// else -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Failed to enabled spatial mapping for camera {} ({}) because positional tracking could not be enabled! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Return error code. -// return slReturnCode; -// } - -// /****************************************************************************** -// * @brief Disabled the spatial mapping feature of the camera. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// void ZEDCam::DisableSpatialMapping() -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Disable spatial mapping. -// m_slCamera.disableSpatialMapping(); -// } - -// /****************************************************************************** -// * @brief Enables the object detection and tracking feature of the camera. -// * -// * @return sl::ERROR_CODE - Whether or not object detection/tracking was successfully enabled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// sl::ERROR_CODE ZEDCam::EnableObjectDetection(const bool bEnableBatching) -// { -// // Check if batching should be turned on. -// bEnableBatching ? m_slObjectDetectionBatchParams.enable = true : m_slObjectDetectionBatchParams.enable = false; -// // Give batch params to detection params. -// m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; - -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Enable object detection. -// sl::ERROR_CODE slReturnCode = m_slCamera.enableObjectDetection(m_slObjectDetectionParams); -// // Release lock. -// lkSharedLock.unlock(); - -// // Check if positional tracking was enabled properly. -// if (slReturnCode != sl::ERROR_CODE::SUCCESS) -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Failed to enabled object detection for camera {} ({})! sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getCameraInformation().camera_model).get(), -// m_slCamera.getCameraInformation().serial_number, -// sl::toString(slReturnCode).get()); -// } - -// // Return error code. -// return slReturnCode; -// } - -// /****************************************************************************** -// * @brief Disables the object detection and tracking feature of the camera. -// * -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// void ZEDCam::DisableObjectDetection() -// { -// // Acquire write lock. -// std::unique_lock lkSharedLock(m_muCameraMutex); -// // Disable object detection and tracking. -// m_slCamera.disableObjectDetection(); -// } - -// /****************************************************************************** -// * @brief Accessor for the current status of the camera. -// * -// * @return true - Camera is currently opened and functional. -// * @return false - Camera is not opened and/or connected. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetCameraIsOpen() -// { -// return m_slCamera.isOpened(); -// } - -// /****************************************************************************** -// * @brief Accessor for if this ZED is storing it's frames in GPU memory. -// * -// * @return true - Using GPU memory for mats. -// * @return false - Using CPU memory for mats. -// * -// * @author ClayJay3 (claytonraycowen@gmail.com) -// * @date 2023-09-09 -// ******************************************************************************/ -// bool ZEDCam::GetUsingGPUMem() const -// { -// // Check if we are using GPU memory. -// if (m_slMemoryType == sl::MEM::GPU) -// { -// // Using GPU memory. -// return true; -// } -// else -// { -// // Not using GPU memory. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Accessor for the model enum from the ZEDSDK and represents the camera model as a string. -// * -// * @return std::string - The model of the zed camera. -// * Possible values: ZED, ZED_MINI, ZED_2, ZED_2i, ZED_X, ZED_X_MINI, UNDEFINED_UNKNOWN -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// std::string ZEDCam::GetCameraModel() -// { -// // Declare instance variables. -// std::string szCameraModel; - -// // Check if the camera is opened. -// if (m_slCamera.isOpened()) -// { -// // Convert camera model to a string. -// szCameraModel = sl::toString(m_slCamera.getCameraInformation().camera_model).get(); -// } -// else -// { -// // Set the model string to show camera isn't opened. -// szCameraModel = "NOT_OPENED"; -// } - -// // Return model of camera represented as string. -// return szCameraModel; -// } - -// /****************************************************************************** -// * @brief Accessor for the camera's serial number. -// * -// * @return unsigned int - -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// unsigned int ZEDCam::GetCameraSerial() -// { -// // Return connected camera serial number. -// return m_slCamera.getCameraInformation().serial_number; -// } - -// /****************************************************************************** -// * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. -// * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. -// * -// * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. -// * @return true - Pose was successfully retrieved and copied. -// * @return false - Pose was not successfully retrieved and/or copied. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetPositionalPose(sl::Pose& slPose) -// { -// // Check if positional tracking has been enabled. -// if (m_slCamera.isPositionalTrackingEnabled()) -// { -// // Assemble the data container. -// containers::DataFetchContainer stContainer(slPose); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qPoseCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving sl::Pose from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled!"); -// // Return unsuccessful. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Accessor for if the positional tracking functionality of the camera has been enabled. -// * -// * @return true - Positional tracking is enabled. -// * @return false - Positional tracking is not enabled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetPositionalTrackingEnabled() -// { -// return m_slCamera.isPositionalTrackingEnabled(); -// } - -// /****************************************************************************** -// * @brief Gets the IMU data from the ZED camera. If getting the data fails, the -// * last successfully retrieved value is returned. -// * -// * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. -// * @return true - The IMU vals were copied and stored successfully. -// * @return false - The IMU vals were copied and stored successfully. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetIMUData(std::vector& vIMUValues) -// { -// // Assemble the data container. -// containers::DataFetchContainer&> stContainer(vIMUValues); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qIMUDataCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving IMU data from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief Accessor for the current state of the camera's spatial mapping feature. -// * -// * @return sl::SPATIAL_MAPPING_STATE - The enum value of the spatial mapping state. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// sl::SPATIAL_MAPPING_STATE ZEDCam::GetSpatialMappingState() -// { -// // Return the current spatial mapping state of the camera. -// return m_slCamera.getSpatialMappingState(); -// } - -// /****************************************************************************** -// * @brief Retrieve the built spatial map from the camera. Spatial mapping must be enabled. -// * This method takes in an std::future to eventually store the map in. -// * It returns a enum code representing the successful scheduling of building the map. -// * Any code other than SPATIAL_MAPPING_STATE::OK means the future will never be filled. -// * -// * @param std::future - The future to eventually store the map in. -// * @return sl::SPATIAL_MAPPING_STATE - Whether or not the building of the map was successfully scheduled. -// * Anything other than OK means the future will never be filled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// sl::SPATIAL_MAPPING_STATE ZEDCam::ExtractSpatialMapAsync(std::future& fuMeshFuture) -// { -// // Get and store current state of spatial mapping. -// sl::SPATIAL_MAPPING_STATE slReturnState = m_slCamera.getSpatialMappingState(); - -// // Check if spatial mapping has been enabled and ready -// if (slReturnState == sl::SPATIAL_MAPPING_STATE::OK) -// { -// // Request that the ZEDSDK begin processing the spatial map for export. -// m_slCamera.requestSpatialMapAsync(); - -// // Start an async thread to wait for spatial map processing to finish. Return resultant future object. -// fuMeshFuture = std::async(std::launch::async, -// [this]() -// { -// // Create instance variables. -// sl::Mesh slSpatialMap; - -// // Loop until map is finished generating. -// while (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) -// { -// // Sleep for 10ms. -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// } - -// // Check if the spatial map was exported successfully. -// if (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::SUCCESS) -// { -// // Get and store the spatial map. -// m_slCamera.retrieveSpatialMapAsync(slSpatialMap); - -// // Return spatial map. -// return slSpatialMap; -// } -// else -// { -// // Submit logger message. -// LOG_ERROR(g_qSharedLogger, -// "Failed to extract ZED spatial map. sl::ERROR_CODE is: {}", -// sl::toString(m_slCamera.getSpatialMapRequestStatusAsync()).get()); - -// // Return empty point cloud. -// return sl::Mesh(); -// } -// }); -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "ZED spatial mapping was never enabled, can't extract spatial map!"); -// } - -// // Return current spatial mapping state. -// return slReturnState; -// } - -// /****************************************************************************** -// * @brief Accessor for if the cameras object detection and tracking feature is enabled. -// * -// * @return true - Object detection and tracking is enabled. -// * @return false - Object detection and tracking is not enabled. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetObjectDetectionEnabled() -// { -// return m_slCamera.isObjectDetectionEnabled(); -// } - -// /****************************************************************************** -// * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the -// * given sl::ObjectData vector. -// * -// * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. -// * @return true - The object data was successfully copied. -// * @return false - The object data was not successfully copied. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-27 -// ******************************************************************************/ -// bool ZEDCam::GetObjects(std::vector& vObjectData) -// { -// // Check if object detection has been enabled. -// if (m_slCamera.isObjectDetectionEnabled()) -// { -// // Assemble the data container. -// containers::DataFetchContainer&> stContainer(vObjectData); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qObjectDataCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving object data from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED object data but object detection/tracking is not enabled!"); -// // Return unsuccessful. -// return false; -// } -// } - -// /****************************************************************************** -// * @brief If batching is enabled, this retrieves the normal objects and passes them to -// * the the iternal batching queue of the zed api. This performs short-term re-identification -// * with deep learning and trajectories filtering. Batching must have been set to enabled when -// * EnableObjectDetection() was called. Most of the time the vector will be empty and will be -// * filled every ZED_OBJDETECTION_BATCH_LATENCY. -// * -// * @param vBatchedObjectData - A vector containing objects of sl::ObjectsBatch object that will -// * have object data copied to. -// * @return true - The batched objects data was successfully copied. -// * @return false - The batched objects data was not successfully copied. -// * -// * @author clayjay3 (claytonraycowen@gmail.com) -// * @date 2023-08-30 -// ******************************************************************************/ -// bool ZEDCam::GetBatchedObjects(std::vector& vBatchedObjectData) -// { -// // Check if object detection and batching has been enabled. -// if (m_slCamera.isObjectDetectionEnabled() && m_slObjectDetectionBatchParams.enable) -// { -// // Assemble the data container. -// containers::DataFetchContainer&> stContainer(vBatchedObjectData); - -// // Acquire lock on frame copy queue. -// std::unique_lock lkSchedulers(m_muPoolScheduleMutex); -// // Append frame fetch container to the schedule queue. -// m_qObjectBatchedDataCopySchedule.push(stContainer); -// // Release lock on the frame schedule queue. -// lkSchedulers.unlock(); - -// // Create lock variable to be used by condition variable. CV unlocks this during wait(). -// std::unique_lock lkConditionLock(stContainer.muConditionMutex); -// // Wait up to 10 seconds for the condition variable to be notified. -// std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); -// // Release lock. -// lkConditionLock.unlock(); - -// // Check condition variable status and return. -// if (cdStatus == std::cv_status::no_timeout) -// { -// // Image was successfully written to the given cv::Mat reference. -// return true; -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving batched object data from threadpool queue."); -// // Image was not written successfully. -// return false; -// } -// } -// else -// { -// // Submit logger message. -// LOG_WARNING(g_qSharedLogger, "Attempted to get ZED batched object data but object detection/tracking is not enabled!"); -// // Return unsuccessful. -// return false; -// } -// } +/****************************************************************************** + * @brief Implements the ZEDCam class. + * + * @file ZEDCam.cpp + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + * + * @copyright Copyright Mars Rover Design Team 2023 - All Rights Reserved + ******************************************************************************/ + +#include "ZEDCam.h" +#include "../../AutonomyLogging.h" +#include "../../util/vision/ImageOperations.hpp" + +/****************************************************************************** + * @brief This struct is part of the ZEDCam class and is used as a container for all + * bounding box data that is going to be passed to the zed api via the ZEDCam's + * TrackCustomBoxObjects() method. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-29 + ******************************************************************************/ +struct ZEDCam::ZedObjectData + +{ + private: + // Declare and define private struct member variables. + std::string szObjectUUID = sl::generate_unique_id().get(); // This will automatically generate a guaranteed unique id so the object is traceable. + + // Declare a private struct for holding point data. + /****************************************************************************** + * @brief This struct is internal to the ZedObjectData struct is used to store + * an X and Y value for the corners of a bounding box. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-29 + ******************************************************************************/ + struct Corner + { + public: + // Declare public struct member variables. + unsigned int nX; + unsigned int nY; + }; + + public: + // Declare and define public struct member variables. + struct Corner CornerTL; // The top left corner of the bounding box. + struct Corner CornerTR; // The top right corner of the bounding box. + struct Corner CornerBL; // The bottom left corner of the bounding box. + struct Corner CornerBR; // The bottom right corner of bounding box. + int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the + float fConfidence; // This info is passed through from your detection algorithm and will help improve tracking by throwing out bad detections. + // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class + // to avoid issues. + bool bObjectRemainsOnFloorPlane = false; + + // Declare and define public struct getters. + std::string GetObjectUUID() { return szObjectUUID; }; +}; + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/****************************************************************************** + * @brief Construct a new Zed Cam:: Zed Cam object. + * + * @param nPropResolutionX - X res of camera. Must be smaller than ZED_BASE_RESOLUTION. + * @param nPropResolutionY - Y res of camera. Must be smaller than ZED_BASE_RESOLUTION. + * @param nPropFramesPerSecond - FPS camera is running at. + * @param dPropHorizontalFOV - The horizontal field of view. + * @param dPropVerticalFOV - The vertical field of view. + * @param fMinSenseDistance - The minimum distance to include in depth measures. + * @param fMaxSenseDistance - The maximim distance to include in depth measures. + * @param bMemTypeGPU - Whether or not to use the GPU memory for operations. + * @param unCameraSerialNumber - The serial number of the camera to open. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +ZEDCam::ZEDCam(const int nPropResolutionX, + const int nPropResolutionY, + const int nPropFramesPerSecond, + const double dPropHorizontalFOV, + const double dPropVerticalFOV, + const float fMinSenseDistance, + const float fMaxSenseDistance, + const bool bMemTypeGPU, + const bool bUseHalfDepthPrecision, + const unsigned int unCameraSerialNumber) : + Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) +{ + // Assign member variables. + bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; + bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; + m_unCameraSerialNumber = unCameraSerialNumber; + + // Setup camera params. + m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; + m_slCameraParams.camera_fps = nPropFramesPerSecond; + m_slCameraParams.coordinate_units = constants::ZED_MEASURE_UNITS; + m_slCameraParams.coordinate_system = constants::ZED_COORD_SYSTEM; + m_slCameraParams.depth_mode = constants::ZED_DEPTH_MODE; + m_slCameraParams.depth_minimum_distance = fMinSenseDistance; + m_slCameraParams.depth_maximum_distance = fMaxSenseDistance; + m_slCameraParams.depth_stabilization = constants::ZED_DEPTH_STABILIZATION; + // Only set serial number if necessary. + if (unCameraSerialNumber != static_cast(0)) + { + m_slCameraParams.input.setFromSerialNumber(unCameraSerialNumber); + } + + // Setup camera runtime params. + m_slRuntimeParams.enable_fill_mode = constants::ZED_SENSING_FILL; + + // Setup positional tracking parameters. + m_slPoseTrackingParams.mode = constants::ZED_POSETRACK_MODE; + m_slPoseTrackingParams.enable_area_memory = constants::ZED_POSETRACK_AREA_MEMORY; + m_slPoseTrackingParams.enable_pose_smoothing = constants::ZED_POSETRACK_POSE_SMOOTHING; + m_slPoseTrackingParams.set_floor_as_origin = constants::ZED_POSETRACK_FLOOR_IS_ORIGIN; + m_slPoseTrackingParams.enable_imu_fusion = constants::ZED_POSETRACK_ENABLE_IMU_FUSION; + m_slPoseTrackingParams.depth_min_range = constants::ZED_POSETRACK_USABLE_DEPTH_MIN; + m_slPoseTrackingParams.set_gravity_as_origin = constants::ZED_POSETRACK_USE_GRAVITY_ORIGIN; + + // Setup spatial mapping parameters. + m_slSpatialMappingParams.map_type = constants::ZED_MAPPING_TYPE; + m_slSpatialMappingParams.resolution_meter = constants::ZED_MAPPING_RESOLUTION_METER; + m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); + m_slSpatialMappingParams.save_texture = true; + m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; + m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; + + // Setup object detection/tracking parameters. + m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; + m_slObjectDetectionParams.image_sync = constants::ZED_OBJDETECTION_IMG_SYNC; + m_slObjectDetectionParams.enable_tracking = constants::ZED_OBJDETECTION_TRACK_OBJ; + m_slObjectDetectionParams.enable_segmentation = constants::ZED_OBJDETECTION_SEGMENTATION; + m_slObjectDetectionParams.filtering_mode = constants::ZED_OBJDETECTION_FILTERING; + m_slObjectDetectionParams.prediction_timeout_s = constants::ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT; + // Setup object detection/tracking batch parameters. + m_slObjectDetectionBatchParams.enable = false; + m_slObjectDetectionBatchParams.id_retention_time = constants::ZED_OBJDETECTION_BATCH_RETENTION_TIME; + m_slObjectDetectionBatchParams.latency = constants::ZED_OBJDETECTION_BATCH_LATENCY; + m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; + + // Attempt to open camera. + sl::ERROR_CODE slReturnCode = m_slCamera.open(m_slCameraParams); + // Check if the camera was successfully opened. + if (m_slCamera.isOpened()) + { + // Submit logger message. + LOG_DEBUG(g_qSharedLogger, + "{} stereo camera with serial number {} has been succsessfully opened.", + this->GetCameraModel(), + m_slCamera.getCameraInformation().serial_number); + } + else + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Unable to open stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } +} + +/****************************************************************************** + * @brief Destroy the Zed Cam:: Zed Cam object. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +ZEDCam::~ZEDCam() +{ + // Stop threaded code. + this->RequestStop(); + this->Join(); + + // Close the ZEDCam. + m_slCamera.close(); +} + +/****************************************************************************** + * @brief The code inside this private method runs in a seperate thread, but still + * has access to this*. This method continuously calls the grab() function of + * the ZEDSDK, which updates all frames (RGB, depth, cloud) and all other data + * such as positional and spatial mapping. Then a thread pool is started and joined + * once per iteration to mass copy the frames and/or measure to any other thread + * waiting in the queues. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-01 + ******************************************************************************/ +void ZEDCam::ThreadedContinuousCode() +{ + // Check if camera is opened. + if (!m_slCamera.isOpened()) + { + // Shutdown threads for this ZEDCam. + this->RequestStop(); + // Submit logger message. + LOG_CRITICAL(g_qSharedLogger, + "Camera start was attempted for camera at {}, but camera never properly opened or it has been closed/rebooted!", + m_unCameraSerialNumber); + } + else + { + // Acquire write lock for camera object. + std::unique_lock lkSharedCameraLock(m_muCameraMutex); + // Call generalized update method of zed api. + sl::ERROR_CODE slReturnCode = m_slCamera.grab(m_slRuntimeParams); + // Check if new frame was computed successfully. + if (slReturnCode == sl::ERROR_CODE::SUCCESS) + { + // Grab regular image and store it in member variable. + slReturnCode = m_slCamera.retrieveImage(m_slFrame, constants::ZED_RETRIEVE_VIEW, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new frame image for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Grab depth measure and store it in member variable. + slReturnCode = m_slCamera.retrieveMeasure(m_slDepthMeasure, m_slDepthMeasureType, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new depth measure for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Grab depth grayscale image and store it in member variable. + slReturnCode = m_slCamera.retrieveImage(m_slDepthImage, sl::VIEW::DEPTH, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new depth image for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Grab regular resized image and store it in member variable. + slReturnCode = m_slCamera.retrieveMeasure(m_slPointCloud, sl::MEASURE::XYZ, m_slMemoryType, sl::Resolution(m_nPropResolutionX, m_nPropResolutionY)); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new point cloud for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Get and store the SensorData object from the camera. Get data from the most recent image grab. + // Using TIME_REFERENCE::CURRENT requires high rate polling and can introduce error as the most recent + // IMU data could be in the future of the camera image. + slReturnCode = m_slCamera.getSensorsData(m_slSensorData, sl::TIME_REFERENCE::IMAGE); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new sensors data for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Check if positional tracking is enabled. + if (m_slCamera.isPositionalTrackingEnabled()) + { + // Get the current pose of the camera. + sl::POSITIONAL_TRACKING_STATE slPoseTrackReturnCode = m_slCamera.getPosition(m_slCameraPose, sl::REFERENCE_FRAME::WORLD); + // Check that the regular frame was retrieved successfully. + if (slPoseTrackReturnCode != sl::POSITIONAL_TRACKING_STATE::OK) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new positional tracking pose for stereo camera {} ({})! sl::POSITIONAL_TRACKING_STATE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slPoseTrackReturnCode).get()); + } + } + // Check if object detection is enabled. + if (m_slCamera.isObjectDetectionEnabled()) + { + // Get updated objects from camera. + slReturnCode = m_slCamera.retrieveObjects(m_slDetectedObjects); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Check if batched object data is enabled. + if (m_slObjectDetectionBatchParams.enable) + { + // Get updated batched objects from camera. + slReturnCode = m_slCamera.getObjectsBatch(m_slDetectedObjectsBatched); + // Check that the regular frame was retrieved successfully. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Unable to retrieve new batched object data for stereo camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + } + } + // Release camera lock. + lkSharedCameraLock.unlock(); + + // Call FPS tick. + m_IPS.Tick(); + } + else + { + // Release camera lock. + lkSharedCameraLock.unlock(); + + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Unable to update stereo camera {} ({}) frames, measurements, and sensors! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Acquire a shared_lock on the frame copy queue. + std::shared_lock lkSchedulers(m_muPoolScheduleMutex); + // Check if the frame copy queue is empty. + if (!m_qFrameCopySchedule.empty() || !m_qGPUFrameCopySchedule.empty() || !m_qCustomBoxInjestSchedule.empty() || !m_qPoseCopySchedule.empty() || + !m_qIMUDataCopySchedule.empty() || !m_qObjectDataCopySchedule.empty() || !m_qObjectBatchedDataCopySchedule.empty()) + { + // Find the queue with the longest length. + size_t siMaxQueueLength = std::max({m_qFrameCopySchedule.size(), + m_qGPUFrameCopySchedule.size(), + m_qCustomBoxInjestSchedule.size(), + m_qPoseCopySchedule.size(), + m_qIMUDataCopySchedule.size(), + m_qObjectDataCopySchedule.size(), + m_qObjectBatchedDataCopySchedule.size()}); + + // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. + this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); + // Wait for thread pool to finish. + this->JoinPool(); + // Release lock on frame copy queue. + lkSchedulers.unlock(); + } + } +} + +/****************************************************************************** + * @brief This method holds the code that is ran in the thread pool started by + * the ThreadedLinearCode() method. It copies the data from the different + * data objects to references of the same type stored in a vector queued up by the + * Grab methods. + * + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-09-08 + ******************************************************************************/ +void ZEDCam::PooledLinearCode() +{ + ///////////////////////////// + // Frame queue. + ///////////////////////////// + // Check if we are using CPU or GPU mats. + if (m_slMemoryType == sl::MEM::CPU) + { + // Aqcuire mutex for getting frames out of the queue. + std::unique_lock lkFrameQueue(m_muFrameCopyMutex); + // Check if the queue is empty. + if (!m_qFrameCopySchedule.empty()) + { + // Get frame container out of queue. + containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); + // Pop out of queue. + m_qFrameCopySchedule.pop(); + // Release lock. + lkFrameQueue.unlock(); + + // Determine which frame should be copied. + switch (stContainer.eFrameType) + { + case eBGRA: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slFrame); break; + case eDepthMeasure: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; + case eDepthImage: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; + case eXYZ: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; + default: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slFrame); break; + } + + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); + } + } + // Use GPU mat. + else + { + // Check if the queue is empty. + if (!m_qGPUFrameCopySchedule.empty()) + { + // Aqcuire mutex for getting frames out of the queue. + std::unique_lock lkFrameQueue(m_muFrameCopyMutex); + // Get frame container out of queue. + containers::FrameFetchContainer stContainer = m_qGPUFrameCopySchedule.front(); + // Pop out of queue. + m_qGPUFrameCopySchedule.pop(); + // Release lock. + lkFrameQueue.unlock(); + + // Determine which frame should be copied. + switch (stContainer.eFrameType) + { + case eBGRA: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slFrame); break; + case eDepthMeasure: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; + case eDepthImage: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; + case eXYZ: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; + default: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slFrame); break; + } + + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); + } + } + + ///////////////////////////// + // Pose queue. + ///////////////////////////// + // Aqcuire mutex for getting frames out of the pose queue. + std::unique_lock lkPoseQueue(m_muPoseCopyMutex); + // Check if the queue is empty. + if (!m_qPoseCopySchedule.empty()) + { + // Get frame container out of queue. + containers::DataFetchContainer stContainer = m_qPoseCopySchedule.front(); + // Pop out of queue. + m_qPoseCopySchedule.pop(); + // Release lock. + lkPoseQueue.unlock(); + + // Copy pose. + *(stContainer.pData) = sl::Pose(m_slCameraPose); + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); + } + + ///////////////////////////// + // IMU data queue. + ///////////////////////////// + // Aqcuire mutex for getting frames out of the pose queue. + std::unique_lock lkIMUQueue(m_muIMUDataCopyMutex); + // Check if the queue is empty. + if (!m_qIMUDataCopySchedule.empty()) + { + // Get frame container out of queue. + containers::DataFetchContainer> stContainer = m_qIMUDataCopySchedule.front(); + // Pop out of queue. + m_qIMUDataCopySchedule.pop(); + // Release lock. + lkIMUQueue.unlock(); + + // Get IMU orientation in degrees. + sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); + // Get IMU linear acceleration. + sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; + // Repackage angles and accels into vector. + stContainer.pData->emplace_back(slAngles.x); + stContainer.pData->emplace_back(slAngles.y); + stContainer.pData->emplace_back(slAngles.z); + stContainer.pData->emplace_back(slLinearAccels.x); + stContainer.pData->emplace_back(slLinearAccels.y); + stContainer.pData->emplace_back(slLinearAccels.z); + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); + } + + ///////////////////////////// + // ObjectData queue. + ///////////////////////////// + // Aqcuire mutex for getting frames out of the pose queue. + std::unique_lock lkObjectDataQueue(m_muObjectDataCopyMutex); + // Check if the queue is empty. + if (!m_qObjectDataCopySchedule.empty()) + { + // Get frame container out of queue. + containers::DataFetchContainer> stContainer = m_qObjectDataCopySchedule.front(); + // Pop out of queue. + m_qObjectDataCopySchedule.pop(); + // Release lock. + lkIMUQueue.unlock(); + + // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) + *(stContainer.pData) = m_slDetectedObjects.object_list; + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); + } + + ///////////////////////////// + // ObjectData Batched queue. + ///////////////////////////// + // Aqcuire mutex for getting frames out of the pose queue. + std::unique_lock lkObjectBatchedDataQueue(m_muObjectBatchedDataCopyMutex); + // Check if the queue is empty. + if (!m_qObjectBatchedDataCopySchedule.empty()) + { + // Get frame container out of queue. + containers::DataFetchContainer> stContainer = m_qObjectBatchedDataCopySchedule.front(); + // Pop out of queue. + m_qObjectBatchedDataCopySchedule.pop(); + // Release lock. + lkIMUQueue.unlock(); + + // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) + *(stContainer.pData) = m_slDetectedObjectsBatched; + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); + } +} + +/****************************************************************************** + * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. + * Remember this code will be ran in whatever class/thread calls it. + * + * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-09-09 + ******************************************************************************/ +std::future ZEDCam::RequestFrameCopy(cv::Mat& cvFrame) +{ + // Assemble the FrameFetchContainer. + containers::FrameFetchContainer stContainer(cvFrame, eBGRA); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and + * stores it in a GPU mat. + * Remember this code will be ran in whatever class/thread calls it. + * + * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-09-09 + ******************************************************************************/ +std::future ZEDCam::RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame) +{ + // Assemble the FrameFetchContainer. + containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qGPUFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Grabs a depth measure or image from the camera. This image has the same shape as + * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in + * AutonomyConstants.h. + * + * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. + * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image + * purposes other than displaying depth. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +std::future ZEDCam::RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure) +{ + // Create instance variables. + PIXEL_FORMATS eFrameType; + + // Check if the container should be set to retrieve an image or a measure. + bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; + // Assemble container. + containers::FrameFetchContainer stContainer(cvDepth, eFrameType); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as + * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in + * AutonomyConstants.h. + * + * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. + * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image + * purposes other than displaying depth. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +std::future ZEDCam::RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) +{ + // Create instance variables. + PIXEL_FORMATS eFrameType; + + // Check if the container should be set to retrieve an image or a measure. + bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; + // Assemble container. + containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qGPUFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal + * image but with three XYZ values replacing the old color values in the 3rd dimension. + * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM + * constants set in AutonomyConstants.h. + * + * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +std::future ZEDCam::RequestPointCloudCopy(cv::Mat& cvPointCloud) +{ + // Assemble the FrameFetchContainer. + containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal + * image but with three XYZ values replacing the old color values in the 3rd dimension. + * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM + * constants set in AutonomyConstants.h. + * + * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +std::future ZEDCam::RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud) +{ + // Assemble the FrameFetchContainer. + containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qGPUFrameCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); +} + +/****************************************************************************** + * @brief Resets the cameras X,Y,Z translation and Roll,Pitch,Yaw orientation back + * to 0. THINK CAREFULLY! Do you actually want to reset this? It will also realign + * the coordinate system to whichever way the camera happens to be facing. + * + * @return sl::ERROR_CODE - Status of the positional tracking reset. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::ResetPositionalTracking() +{ + // Create new translation to set position back to zero. + sl::Translation slZeroTranslation(0.0, 0.0, 0.0); + // This will reset position and coordinate frame. + sl::Rotation slZeroRotation; + slZeroRotation.setEulerAngles(sl::float3(0.0, 0.0, 0.0), false); + + // Store new translation and rotation in a tranform object. + sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); + + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Reset the positional tracking location of the camera. + return m_slCamera.resetPositionalTracking(slZeroTransform); +} + +/****************************************************************************** + * @brief A vector containing CustomBoxObjectData objects. These objects simply store + * information about your detected objects from an external object detection model. + * You will need to take your inference results and package them into a sl::CustomBoxObjectData + * so the the ZEDSDK can properly interpret your detections. + * + * Giving the bounding boxes of your detected objects to the ZEDSDK will enable positional + * tracking and velocity estimation for each object. Even when not in view. The IDs of objects + * will also become persistent. + * + * @param vCustomObjects - A vector of sl::CustomBoxObjectData objects. + * @return sl::ERROR_CODE - The return status of ingestion. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::TrackCustomBoxObjects(std::vector& vCustomObjects) +{ + // Create instance varables. + std::vector vCustomBoxData; + + // Repack detection data into sl specific object. + for (ZedObjectData stObjectData : vCustomObjects) + { + // Create new sl CustomBoxObjectData struct. + sl::CustomBoxObjectData slCustomBox; + std::vector vCorners; + + // Assign simple attributes. + slCustomBox.unique_object_id = sl::String(stObjectData.GetObjectUUID().c_str()); + slCustomBox.label = stObjectData.nClassNumber; + slCustomBox.probability = stObjectData.fConfidence; + slCustomBox.is_grounded = stObjectData.bObjectRemainsOnFloorPlane; + // Repackage object corner data. + vCorners.emplace_back(sl::uint2(stObjectData.CornerTL.nX, stObjectData.CornerTL.nY)); + vCorners.emplace_back(sl::uint2(stObjectData.CornerTR.nX, stObjectData.CornerTR.nY)); + vCorners.emplace_back(sl::uint2(stObjectData.CornerBL.nX, stObjectData.CornerBL.nY)); + vCorners.emplace_back(sl::uint2(stObjectData.CornerBR.nX, stObjectData.CornerBR.nY)); + slCustomBox.bounding_box_2d = vCorners; + + // Append repackaged object to vector. + vCustomBoxData.emplace_back(slCustomBox); + } + + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Give the custom box data to the zed api. + sl::ERROR_CODE slReturnCode = m_slCamera.ingestCustomBoxObjects(vCustomBoxData); + // Release lock. + lkSharedLock.unlock(); + + // Check if successful. + if (slReturnCode == sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, + "Failed to ingest new objects for camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Return error code. + return slReturnCode; +} + +/****************************************************************************** + * @brief Performs a hardware reset of the ZED2 or ZED2i camera. + * + * @return sl::ERROR_CODE - Whether or not the camera reboot was successful. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::RebootCamera() +{ + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Reboot this camera and return the status code. + return sl::Camera::reboot(m_slCamera.getCameraInformation().serial_number); +} + +/****************************************************************************** + * @brief Enable the positional tracking functionality of the camera. + * + * @return sl::ERROR_CODE - Whether or not positional tracking was successfully enabled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::EnablePositionalTracking() +{ + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Enable pose tracking and store return code. + sl::ERROR_CODE slReturnCode = m_slCamera.enablePositionalTracking(m_slPoseTrackingParams); + // Release lock. + lkSharedLock.unlock(); + + // Check if positional tracking was enabled properly. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Failed to enabled positional tracking for camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Return error code. + return slReturnCode; +} + +/****************************************************************************** + * @brief Disable to positional tracking funcionality of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-26 + ******************************************************************************/ +void ZEDCam::DisablePositionalTracking() +{ + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Disable pose tracking. + m_slCamera.disablePositionalTracking(); +} + +/****************************************************************************** + * @brief Sets the pose of the positional tracking of the camera. XYZ will point + * in their respective directions according to ZED_COORD_SYSTEM defined in + * AutonomyConstants.h. + * + * Warning: This method is slow and should not be called in a loop. Setting the pose + * will temporarily block the entire camera from grabbed or copying frames to + * new threads. This method should only be called occasionally when absolutely needed. + * + * @param dX - The X position of the camera in ZED_MEASURE_UNITS. + * @param dY - The Y position of the camera in ZED_MEASURE_UNITS. + * @param dZ - The Z position of the camera in ZED_MEASURE_UNITS. + * @param dXO - The tilt of the camera around the X axis in degrees. + * @param dYO - The tilt of the camera around the Y axis in degrees. + * @param dZO - The tilt of the camera around the Z axis in degrees. + * @return sl::ERROR_CODE - Whether or not the pose was set successfully. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) +{ + // Create new translation to set position back to user given values. + sl::Translation slZeroTranslation(dX, dY, dZ); + // This will reset position and coordinate frame. + sl::Rotation slZeroRotation; + slZeroRotation.setEulerAngles(sl::float3(dXO, dYO, dZO), false); + + // Store new translation and rotation in a tranform object. + sl::Transform slZeroTransform(slZeroRotation, slZeroTranslation); + + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Reset the positional tracking location of the camera. + return m_slCamera.resetPositionalTracking(slZeroTransform); +} + +/****************************************************************************** + * @brief Enabled the spatial mapping feature of the camera. Pose tracking will be + * enabled if it is not already. + * + * @param fTimeoutSeconds - The timeout used to wait for pose tracking to be on the OK state. Default is 10 seconds. + * @return sl::ERROR_CODE - Whether or not spatial mapping was successfully enabled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::EnableSpatialMapping(const int nTimeoutSeconds) +{ + // Create instance variables. + auto tmStartTime = std::chrono::steady_clock::now(); + sl::Pose slCameraPose; + sl::ERROR_CODE slReturnCode; + + // Check if positional tracking is enabled. + if (!m_slCamera.isPositionalTrackingEnabled()) + { + // Enable positional tracking. + this->EnablePositionalTracking(); + } + + // Wait for positional tracking state to be OK. Defualt Timeout of 10 seconds. + while (m_slCamera.getPosition(slCameraPose) != sl::POSITIONAL_TRACKING_STATE::OK && + std::chrono::steady_clock::now() - tmStartTime <= std::chrono::seconds(nTimeoutSeconds)) + { + // Sleep for one millisecond. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Final check if positional tracking was successfully enabled. + if (m_slCamera.getPosition(slCameraPose) == sl::POSITIONAL_TRACKING_STATE::OK) + { + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Enable spatial mapping. + slReturnCode = m_slCamera.enableSpatialMapping(m_slSpatialMappingParams); + // Release lock. + lkSharedLock.unlock(); + + // Check if positional tracking was enabled properly. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Failed to enabled spatial mapping for camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + } + else + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Failed to enabled spatial mapping for camera {} ({}) because positional tracking could not be enabled! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Return error code. + return slReturnCode; +} + +/****************************************************************************** + * @brief Disabled the spatial mapping feature of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +void ZEDCam::DisableSpatialMapping() +{ + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Disable spatial mapping. + m_slCamera.disableSpatialMapping(); +} + +/****************************************************************************** + * @brief Enables the object detection and tracking feature of the camera. + * + * @return sl::ERROR_CODE - Whether or not object detection/tracking was successfully enabled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +sl::ERROR_CODE ZEDCam::EnableObjectDetection(const bool bEnableBatching) +{ + // Check if batching should be turned on. + bEnableBatching ? m_slObjectDetectionBatchParams.enable = true : m_slObjectDetectionBatchParams.enable = false; + // Give batch params to detection params. + m_slObjectDetectionParams.batch_parameters = m_slObjectDetectionBatchParams; + + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Enable object detection. + sl::ERROR_CODE slReturnCode = m_slCamera.enableObjectDetection(m_slObjectDetectionParams); + // Release lock. + lkSharedLock.unlock(); + + // Check if positional tracking was enabled properly. + if (slReturnCode != sl::ERROR_CODE::SUCCESS) + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Failed to enabled object detection for camera {} ({})! sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getCameraInformation().camera_model).get(), + m_slCamera.getCameraInformation().serial_number, + sl::toString(slReturnCode).get()); + } + + // Return error code. + return slReturnCode; +} + +/****************************************************************************** + * @brief Disables the object detection and tracking feature of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +void ZEDCam::DisableObjectDetection() +{ + // Acquire write lock. + std::unique_lock lkSharedLock(m_muCameraMutex); + // Disable object detection and tracking. + m_slCamera.disableObjectDetection(); +} + +/****************************************************************************** + * @brief Accessor for the current status of the camera. + * + * @return true - Camera is currently opened and functional. + * @return false - Camera is not opened and/or connected. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +bool ZEDCam::GetCameraIsOpen() +{ + return m_slCamera.isOpened(); +} + +/****************************************************************************** + * @brief Accessor for if this ZED is storing it's frames in GPU memory. + * + * @return true - Using GPU memory for mats. + * @return false - Using CPU memory for mats. + * + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-09-09 + ******************************************************************************/ +bool ZEDCam::GetUsingGPUMem() const +{ + // Check if we are using GPU memory. + if (m_slMemoryType == sl::MEM::GPU) + { + // Using GPU memory. + return true; + } + else + { + // Not using GPU memory. + return false; + } +} + +/****************************************************************************** + * @brief Accessor for the model enum from the ZEDSDK and represents the camera model as a string. + * + * @return std::string - The model of the zed camera. + * Possible values: ZED, ZED_MINI, ZED_2, ZED_2i, ZED_X, ZED_X_MINI, UNDEFINED_UNKNOWN + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +std::string ZEDCam::GetCameraModel() +{ + // Declare instance variables. + std::string szCameraModel; + + // Check if the camera is opened. + if (m_slCamera.isOpened()) + { + // Convert camera model to a string. + szCameraModel = sl::toString(m_slCamera.getCameraInformation().camera_model).get(); + } + else + { + // Set the model string to show camera isn't opened. + szCameraModel = "NOT_OPENED"; + } + + // Return model of camera represented as string. + return szCameraModel; +} + +/****************************************************************************** + * @brief Accessor for the camera's serial number. + * + * @return unsigned int - + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +unsigned int ZEDCam::GetCameraSerial() +{ + // Return connected camera serial number. + return m_slCamera.getCameraInformation().serial_number; +} + +/****************************************************************************** + * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. + * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. + * + * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. + * @return std::future - A future that should be waited on before the passed in sl::Pose is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +std::future ZEDCam::RequestPositionalPoseCopy(sl::Pose& slPose) +{ + // Check if positional tracking has been enabled. + if (m_slCamera.isPositionalTrackingEnabled()) + { + // Assemble the data container. + containers::DataFetchContainer stContainer(slPose); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qPoseCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); + } + else + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + + // Return unsuccessful. + return fuDummyFuture; + } +} + +/****************************************************************************** + * @brief Accessor for if the positional tracking functionality of the camera has been enabled. + * + * @return true - Positional tracking is enabled. + * @return false - Positional tracking is not enabled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +bool ZEDCam::GetPositionalTrackingEnabled() +{ + return m_slCamera.isPositionalTrackingEnabled(); +} + +/****************************************************************************** + * @brief Gets the IMU data from the ZED camera. If getting the data fails, the + * last successfully retrieved value is returned. + * + * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +std::future ZEDCam::RequestIMUDataCopy(std::vector& vIMUValues) +{ + // Assemble the data container. + containers::DataFetchContainer> stContainer(vIMUValues); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qIMUDataCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); +} + +/****************************************************************************** + * @brief Accessor for the current state of the camera's spatial mapping feature. + * + * @return sl::SPATIAL_MAPPING_STATE - The enum value of the spatial mapping state. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +sl::SPATIAL_MAPPING_STATE ZEDCam::GetSpatialMappingState() +{ + // Return the current spatial mapping state of the camera. + return m_slCamera.getSpatialMappingState(); +} + +/****************************************************************************** + * @brief Retrieve the built spatial map from the camera. Spatial mapping must be enabled. + * This method takes in an std::future to eventually store the map in. + * It returns a enum code representing the successful scheduling of building the map. + * Any code other than SPATIAL_MAPPING_STATE::OK means the future will never be filled. + * + * @param std::future - The future to eventually store the map in. + * @return sl::SPATIAL_MAPPING_STATE - Whether or not the building of the map was successfully scheduled. + * Anything other than OK means the future will never be filled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +sl::SPATIAL_MAPPING_STATE ZEDCam::ExtractSpatialMapAsync(std::future& fuMeshFuture) +{ + // Get and store current state of spatial mapping. + sl::SPATIAL_MAPPING_STATE slReturnState = m_slCamera.getSpatialMappingState(); + + // Check if spatial mapping has been enabled and ready + if (slReturnState == sl::SPATIAL_MAPPING_STATE::OK) + { + // Request that the ZEDSDK begin processing the spatial map for export. + m_slCamera.requestSpatialMapAsync(); + + // Start an async thread to wait for spatial map processing to finish. Return resultant future object. + fuMeshFuture = std::async(std::launch::async, + [this]() + { + // Create instance variables. + sl::Mesh slSpatialMap; + + // Loop until map is finished generating. + while (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) + { + // Sleep for 10ms. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Check if the spatial map was exported successfully. + if (m_slCamera.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::SUCCESS) + { + // Get and store the spatial map. + m_slCamera.retrieveSpatialMapAsync(slSpatialMap); + + // Return spatial map. + return slSpatialMap; + } + else + { + // Submit logger message. + LOG_ERROR(g_qSharedLogger, + "Failed to extract ZED spatial map. sl::ERROR_CODE is: {}", + sl::toString(m_slCamera.getSpatialMapRequestStatusAsync()).get()); + + // Return empty point cloud. + return sl::Mesh(); + } + }); + } + else + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, "ZED spatial mapping was never enabled, can't extract spatial map!"); + } + + // Return current spatial mapping state. + return slReturnState; +} + +/****************************************************************************** + * @brief Accessor for if the cameras object detection and tracking feature is enabled. + * + * @return true - Object detection and tracking is enabled. + * @return false - Object detection and tracking is not enabled. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +bool ZEDCam::GetObjectDetectionEnabled() +{ + return m_slCamera.isObjectDetectionEnabled(); +} + +/****************************************************************************** + * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the + * given sl::ObjectData vector. + * + * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-27 + ******************************************************************************/ +std::future ZEDCam::RequestObjectsCopy(std::vector& vObjectData) +{ + // Check if object detection has been enabled. + if (m_slCamera.isObjectDetectionEnabled()) + { + // Assemble the data container. + containers::DataFetchContainer> stContainer(vObjectData); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qObjectDataCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); + } + else + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, "Attempted to get ZED object data but object detection/tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + + // Return unsuccessful. + return fuDummyFuture; + } +} + +/****************************************************************************** + * @brief If batching is enabled, this retrieves the normal objects and passes them to + * the the iternal batching queue of the zed api. This performs short-term re-identification + * with deep learning and trajectories filtering. Batching must have been set to enabled when + * EnableObjectDetection() was called. Most of the time the vector will be empty and will be + * filled every ZED_OBJDETECTION_BATCH_LATENCY. + * + * @param vBatchedObjectData - A vector containing objects of sl::ObjectsBatch object that will + * have object data copied to. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-08-30 + ******************************************************************************/ +std::future ZEDCam::RequestBatchedObjectsCopy(std::vector& vBatchedObjectData) +{ + // Check if object detection and batching has been enabled. + if (m_slCamera.isObjectDetectionEnabled() && m_slObjectDetectionBatchParams.enable) + { + // Assemble the data container. + containers::DataFetchContainer> stContainer(vBatchedObjectData); + + // Acquire lock on frame copy queue. + std::unique_lock lkSchedulers(m_muPoolScheduleMutex); + // Append frame fetch container to the schedule queue. + m_qObjectBatchedDataCopySchedule.push(stContainer); + // Release lock on the frame schedule queue. + lkSchedulers.unlock(); + + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); + } + else + { + // Submit logger message. + LOG_WARNING(g_qSharedLogger, "Attempted to get ZED batched object data but object detection/tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + + // Return unsuccessful. + return fuDummyFuture; + } +} diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index de4fe608..5eb93d4c 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -1,132 +1,132 @@ -// /****************************************************************************** -// * @brief Defines the ZEDCam class. -// * -// * @file ZEDCam.h -// * @author ClayJay3 (claytonraycowen@gmail.com) -// * @date 2023-08-25 -// * -// * @copyright Copyright MRDT 2024 - All Rights Reserved -// ******************************************************************************/ +/****************************************************************************** + * @brief Defines the ZEDCam class. + * + * @file ZEDCam.h + * @author ClayJay3 (claytonraycowen@gmail.com) + * @date 2023-08-25 + * + * @copyright Copyright MRDT 2024 - All Rights Reserved + ******************************************************************************/ -// #ifndef ZEDCAM_H -// #define ZEDCAM_H +#ifndef ZEDCAM_H +#define ZEDCAM_H -// #include -// #include -// #include -// #include +#include +#include +#include +#include -// #include "../../AutonomyConstants.h" -// #include "../../interfaces/AutonomyThread.hpp" -// #include "../../interfaces/Camera.hpp" -// #include "../../util/vision/FetchContainers.hpp" +#include "../../AutonomyConstants.h" +#include "../../interfaces/AutonomyThread.hpp" +#include "../../interfaces/Camera.hpp" +#include "../../util/vision/FetchContainers.hpp" -// class ZEDCam : public Camera, public AutonomyThread -// { -// public: -// ///////////////////////////////////////// -// // Declare public structs that are specific to and used within this class. -// ///////////////////////////////////////// -// struct ZedObjectData; +class ZEDCam : public Camera, public AutonomyThread +{ + public: + ///////////////////////////////////////// + // Declare public structs that are specific to and used within this class. + ///////////////////////////////////////// + struct ZedObjectData; -// ///////////////////////////////////////// -// // Declare public methods and member variables. -// ///////////////////////////////////////// -// ZEDCam(const int nPropResolutionX, -// const int nPropResolutionY, -// const int nPropFramesPerSecond, -// const double dPropHorizontalFOV, -// const double dPropVerticalFOV, -// const float fMinSenseDistance = constants::ZED_DEFAULT_MINIMUM_DISTANCE, -// const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, -// const bool bMemTypeGPU = false, -// const bool bUseHalfDepthPrecision = false, -// const unsigned int unCameraSerialNumber = 0); -// ~ZEDCam(); -// bool GrabFrame(cv::Mat& cvFrame) override; -// bool GrabFrame(cv::cuda::GpuMat& cvGPUFrame); -// bool GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); -// bool GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); -// bool GrabPointCloud(cv::Mat& cvPointCloud); -// bool GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud); -// sl::ERROR_CODE ResetPositionalTracking(); -// sl::ERROR_CODE TrackCustomBoxObjects(std::vector& vCustomObjects); -// sl::ERROR_CODE RebootCamera(); + ///////////////////////////////////////// + // Declare public methods and member variables. + ///////////////////////////////////////// + ZEDCam(const int nPropResolutionX, + const int nPropResolutionY, + const int nPropFramesPerSecond, + const double dPropHorizontalFOV, + const double dPropVerticalFOV, + const float fMinSenseDistance = constants::ZED_DEFAULT_MINIMUM_DISTANCE, + const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, + const bool bMemTypeGPU = false, + const bool bUseHalfDepthPrecision = false, + const unsigned int unCameraSerialNumber = 0); + ~ZEDCam(); + std::future RequestFrameCopy(cv::Mat& cvFrame) override; + std::future RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame); + std::future RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); + std::future RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); + std::future RequestPointCloudCopy(cv::Mat& cvPointCloud); + std::future RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud); + sl::ERROR_CODE ResetPositionalTracking(); + sl::ERROR_CODE TrackCustomBoxObjects(std::vector& vCustomObjects); + sl::ERROR_CODE RebootCamera(); -// ///////////////////////////////////////// -// // Setters for class member variables. -// ///////////////////////////////////////// -// sl::ERROR_CODE EnablePositionalTracking(); -// void DisablePositionalTracking(); -// sl::ERROR_CODE SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO); -// sl::ERROR_CODE EnableSpatialMapping(const int nTimeoutSeconds = 10); -// void DisableSpatialMapping(); -// sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false); -// void DisableObjectDetection(); + ///////////////////////////////////////// + // Setters for class member variables. + ///////////////////////////////////////// + sl::ERROR_CODE EnablePositionalTracking(); + void DisablePositionalTracking(); + sl::ERROR_CODE SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO); + sl::ERROR_CODE EnableSpatialMapping(const int nTimeoutSeconds = 10); + void DisableSpatialMapping(); + sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false); + void DisableObjectDetection(); -// ///////////////////////////////////////// -// // Accessors for class member variables. -// ///////////////////////////////////////// -// bool GetCameraIsOpen() override; -// bool GetUsingGPUMem() const; -// std::string GetCameraModel(); -// unsigned int GetCameraSerial(); -// bool GetPositionalPose(sl::Pose& slPose); -// bool GetPositionalTrackingEnabled(); -// bool GetIMUData(std::vector& vIMUValues); -// sl::SPATIAL_MAPPING_STATE GetSpatialMappingState(); -// sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future& fuMeshFuture); -// bool GetObjectDetectionEnabled(); -// bool GetObjects(std::vector& vObjectData); -// bool GetBatchedObjects(std::vector& vBatchedObjectData); + ///////////////////////////////////////// + // Accessors for class member variables. + ///////////////////////////////////////// + bool GetCameraIsOpen() override; + bool GetUsingGPUMem() const; + std::string GetCameraModel(); + unsigned int GetCameraSerial(); + std::future RequestPositionalPoseCopy(sl::Pose& slPose); + bool GetPositionalTrackingEnabled(); + std::future RequestIMUDataCopy(std::vector& vIMUValues); + sl::SPATIAL_MAPPING_STATE GetSpatialMappingState(); + sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future& fuMeshFuture); + bool GetObjectDetectionEnabled(); + std::future RequestObjectsCopy(std::vector& vObjectData); + std::future RequestBatchedObjectsCopy(std::vector& vBatchedObjectData); -// private: -// ///////////////////////////////////////// -// // Declare private member variables. -// ///////////////////////////////////////// -// // ZED Camera specific. -// sl::Camera m_slCamera; -// std::shared_mutex m_muCameraMutex; -// sl::InitParameters m_slCameraParams; -// sl::RuntimeParameters m_slRuntimeParams; -// sl::MEASURE m_slDepthMeasureType; -// sl::SensorsData m_slSensorData; -// sl::PositionalTrackingParameters m_slPoseTrackingParams; -// sl::Pose m_slCameraPose; -// sl::SpatialMappingParameters m_slSpatialMappingParams; -// sl::ObjectDetectionParameters m_slObjectDetectionParams; -// sl::BatchParameters m_slObjectDetectionBatchParams; -// sl::Objects m_slDetectedObjects; -// std::vector m_slDetectedObjectsBatched; -// sl::MEM m_slMemoryType; -// unsigned int m_unCameraSerialNumber; + private: + ///////////////////////////////////////// + // Declare private member variables. + ///////////////////////////////////////// + // ZED Camera specific. + sl::Camera m_slCamera; + std::shared_mutex m_muCameraMutex; + sl::InitParameters m_slCameraParams; + sl::RuntimeParameters m_slRuntimeParams; + sl::MEASURE m_slDepthMeasureType; + sl::SensorsData m_slSensorData; + sl::PositionalTrackingParameters m_slPoseTrackingParams; + sl::Pose m_slCameraPose; + sl::SpatialMappingParameters m_slSpatialMappingParams; + sl::ObjectDetectionParameters m_slObjectDetectionParams; + sl::BatchParameters m_slObjectDetectionBatchParams; + sl::Objects m_slDetectedObjects; + std::vector m_slDetectedObjectsBatched; + sl::MEM m_slMemoryType; + unsigned int m_unCameraSerialNumber; -// // Mats for storing frames and measures. -// sl::Mat m_slFrame; -// sl::Mat m_slDepthImage; -// sl::Mat m_slDepthMeasure; -// sl::Mat m_slPointCloud; + // Mats for storing frames and measures. + sl::Mat m_slFrame; + sl::Mat m_slDepthImage; + sl::Mat m_slDepthMeasure; + sl::Mat m_slPointCloud; -// // Queues and mutexes for scheduling and copying camera frames and data to other threads. -// std::queue>> m_qFrameCopySchedule; -// std::queue>> m_qGPUFrameCopySchedule; -// std::queue&>>> m_qCustomBoxInjestSchedule; -// std::queue>> m_qPoseCopySchedule; -// std::queue&>>> m_qIMUDataCopySchedule; -// std::queue&>>> m_qObjectDataCopySchedule; -// std::queue&>>> m_qObjectBatchedDataCopySchedule; -// std::shared_mutex m_muPoolScheduleMutex; -// std::mutex m_muFrameCopyMutex; -// std::mutex m_muCustomBoxInjestMutex; -// std::mutex m_muPoseCopyMutex; -// std::mutex m_muIMUDataCopyMutex; -// std::mutex m_muObjectDataCopyMutex; -// std::mutex m_muObjectBatchedDataCopyMutex; + // Queues and mutexes for scheduling and copying camera frames and data to other threads. + std::queue>> m_qFrameCopySchedule; + std::queue>> m_qGPUFrameCopySchedule; + std::queue>>> m_qCustomBoxInjestSchedule; + std::queue>> m_qPoseCopySchedule; + std::queue>>> m_qIMUDataCopySchedule; + std::queue>>> m_qObjectDataCopySchedule; + std::queue>>> m_qObjectBatchedDataCopySchedule; + std::shared_mutex m_muPoolScheduleMutex; + std::mutex m_muFrameCopyMutex; + std::mutex m_muCustomBoxInjestMutex; + std::mutex m_muPoseCopyMutex; + std::mutex m_muIMUDataCopyMutex; + std::mutex m_muObjectDataCopyMutex; + std::mutex m_muObjectBatchedDataCopyMutex; -// ///////////////////////////////////////// -// // Declare private methods. -// ///////////////////////////////////////// -// void ThreadedContinuousCode() override; -// void PooledLinearCode() override; -// }; -// #endif + ///////////////////////////////////////// + // Declare private methods. + ///////////////////////////////////////// + void ThreadedContinuousCode() override; + void PooledLinearCode() override; +}; +#endif From 9906fc1e0dfb7cd1b46dc46339289c045b8c7252 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Wed, 27 Sep 2023 01:03:57 +0000 Subject: [PATCH 03/11] Finished implementing future and code cleanup. --- src/threads/CameraHandlerThread.cpp | 52 +++++++++++++++-------------- src/threads/CameraHandlerThread.h | 7 ++-- src/vision/cameras/BasicCam.cpp | 18 ++++++---- src/vision/cameras/BasicCam.h | 17 ++++++++-- src/vision/cameras/ZEDCam.cpp | 4 ++- src/vision/cameras/ZEDCam.h | 12 +++++++ 6 files changed, 71 insertions(+), 39 deletions(-) diff --git a/src/threads/CameraHandlerThread.cpp b/src/threads/CameraHandlerThread.cpp index 3dca036e..0cc60838 100644 --- a/src/threads/CameraHandlerThread.cpp +++ b/src/threads/CameraHandlerThread.cpp @@ -21,16 +21,17 @@ CameraHandlerThread::CameraHandlerThread() { // Initialize main ZED camera. - // m_pMainCam = new ZEDCam(constants::ZED_MAINCAM_RESOLUTIONX, - // constants::ZED_MAINCAM_RESOLUTIONY, - // constants::ZED_MAINCAM_FPS, - // constants::ZED_MAINCAM_HORIZONTAL_FOV, - // constants::ZED_MAINCAM_VERTICAL_FOV, - // constants::ZED_DEFAULT_MINIMUM_DISTANCE, - // constants::ZED_DEFAULT_MAXIMUM_DISTANCE, - // constants::ZED_MAINCAM_USE_GPU_MAT, - // constants::ZED_MAINCAM_USE_HALF_PRECISION_DEPTH, - // constants::ZED_MAINCAN_SERIAL); + m_pMainCam = new ZEDCam(constants::ZED_MAINCAM_RESOLUTIONX, + constants::ZED_MAINCAM_RESOLUTIONY, + constants::ZED_MAINCAM_FPS, + constants::ZED_MAINCAM_HORIZONTAL_FOV, + constants::ZED_MAINCAM_VERTICAL_FOV, + constants::ZED_DEFAULT_MINIMUM_DISTANCE, + constants::ZED_DEFAULT_MAXIMUM_DISTANCE, + constants::ZED_MAINCAM_USE_GPU_MAT, + constants::ZED_MAINCAM_USE_HALF_PRECISION_DEPTH, + constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS, + constants::ZED_MAINCAN_SERIAL); // Initialize Left acruco eye. m_pLeftCam = new BasicCam(constants::BASIC_LEFTCAM_INDEX, @@ -39,7 +40,8 @@ CameraHandlerThread::CameraHandlerThread() constants::BASIC_LEFTCAM_FPS, constants::BASIC_LEFTCAM_PIXELTYPE, constants::BASIC_LEFTCAM_HORIZONTAL_FOV, - constants::BASIC_LEFTCAM_VERTICAL_FOV); + constants::BASIC_LEFTCAM_VERTICAL_FOV, + constants::BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS); } /****************************************************************************** @@ -52,17 +54,17 @@ CameraHandlerThread::CameraHandlerThread() CameraHandlerThread::~CameraHandlerThread() { // Signal and wait for cameras to stop. - // m_pMainCam->RequestStop(); + m_pMainCam->RequestStop(); m_pLeftCam->RequestStop(); - // m_pMainCam->Join(); + m_pMainCam->Join(); m_pLeftCam->Join(); // Delete dynamic memory. - // delete m_pMainCam; + delete m_pMainCam; delete m_pLeftCam; // Set dangling pointers to nullptr. - // m_pMainCam = nullptr; + m_pMainCam = nullptr; m_pLeftCam = nullptr; } @@ -76,7 +78,7 @@ CameraHandlerThread::~CameraHandlerThread() void CameraHandlerThread::StartAllCameras() { // Start ZED cams. - // m_pMainCam->Start(); + m_pMainCam->Start(); // Start basic cams. m_pLeftCam->Start(); @@ -91,15 +93,15 @@ void CameraHandlerThread::StartAllCameras() * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-09-01 ******************************************************************************/ -// ZEDCam* CameraHandlerThread::GetZED(ZEDCamName eCameraName) -// { -// // Determine which camera should be returned. -// switch (eCameraName) -// { -// case eHeadMainCam: return m_pMainCam; -// default: return m_pMainCam; -// } -// } +ZEDCam* CameraHandlerThread::GetZED(ZEDCamName eCameraName) +{ + // Determine which camera should be returned. + switch (eCameraName) + { + case eHeadMainCam: return m_pMainCam; + default: return m_pMainCam; + } +} /****************************************************************************** * @brief Accessor for Basic cameras. diff --git a/src/threads/CameraHandlerThread.h b/src/threads/CameraHandlerThread.h index 6e3d06da..4a30ce90 100644 --- a/src/threads/CameraHandlerThread.h +++ b/src/threads/CameraHandlerThread.h @@ -15,8 +15,7 @@ #include "../interfaces/AutonomyThread.hpp" #include "../vision/cameras/BasicCam.h" - -// #include "../vision/cameras/ZEDCam.h" +#include "../vision/cameras/ZEDCam.h" /****************************************************************************** * @brief The CameraHandlerThread class is responsible for managing all of the @@ -32,7 +31,7 @@ class CameraHandlerThread { private: // Declare private class member variables. - // ZEDCam* m_pMainCam; + ZEDCam* m_pMainCam; BasicCam* m_pLeftCam; public: @@ -54,7 +53,7 @@ class CameraHandlerThread void StartAllCameras(); // Accessors. - // ZEDCam* GetZED(ZEDCamName eCameraName); + ZEDCam* GetZED(ZEDCamName eCameraName); BasicCam* GetBasicCam(BasicCamName eCameraName); }; diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index 84feb2f1..8e0ceb04 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -32,12 +32,14 @@ BasicCam::BasicCam(const std::string szCameraPath, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV) : + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, ePropPixelFormat, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. - m_szCameraPath = szCameraPath; - m_nCameraIndex = -1; + m_szCameraPath = szCameraPath; + m_nCameraIndex = -1; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; // Set flag specifying that the camera is located at a dev/video index. m_bCameraIsConnectedOnVideoIndex = false; @@ -76,12 +78,14 @@ BasicCam::BasicCam(const int nCameraIndex, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV) : + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, ePropPixelFormat, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. - m_nCameraIndex = nCameraIndex; - m_szCameraPath = ""; + m_nCameraIndex = nCameraIndex; + m_szCameraPath = ""; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; // Set flag specifying that the camera is located at a dev/video index. m_bCameraIsConnectedOnVideoIndex = true; @@ -169,7 +173,7 @@ void BasicCam::ThreadedContinuousCode() if (!m_qFrameCopySchedule.empty()) { // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. - this->RunDetachedPool(m_qFrameCopySchedule.size(), constants::BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS); + this->RunDetachedPool(m_qFrameCopySchedule.size(), m_nNumFrameRetrievalThreads); // Wait for thread pool to finish. this->JoinPool(); // Release lock on frame copy queue. diff --git a/src/vision/cameras/BasicCam.h b/src/vision/cameras/BasicCam.h index 113df6b7..a7814ca7 100644 --- a/src/vision/cameras/BasicCam.h +++ b/src/vision/cameras/BasicCam.h @@ -18,6 +18,16 @@ #include "../../interfaces/Camera.hpp" #include "../../util/vision/FetchContainers.hpp" +/****************************************************************************** + * @brief This class implements and interfaces with the most common USB cameras + * and features. It is designed in such a way that multiple other classes/threads + * can safely call any method of an object of this class withing resource corruption + * or slowdown of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-21 + ******************************************************************************/ class BasicCam : public Camera, public AutonomyThread { public: @@ -30,14 +40,16 @@ class BasicCam : public Camera, public AutonomyThread const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV); + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads = 10); BasicCam(const int nCameraIndex, const int nPropResolutionX, const int nPropResolutionY, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV); + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads = 10); ~BasicCam(); std::future RequestFrameCopy(cv::Mat& cvFrame) override; @@ -57,6 +69,7 @@ class BasicCam : public Camera, public AutonomyThread std::string m_szCameraPath; bool m_bCameraIsConnectedOnVideoIndex; int m_nCameraIndex; + int m_nNumFrameRetrievalThreads; // Mats for storing frames. cv::Mat m_cvFrame; diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index ba55caab..58d526b7 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -88,13 +88,15 @@ ZEDCam::ZEDCam(const int nPropResolutionX, const float fMaxSenseDistance, const bool bMemTypeGPU, const bool bUseHalfDepthPrecision, + const int nNumFrameRetrievalThreads, const unsigned int unCameraSerialNumber) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; - m_unCameraSerialNumber = unCameraSerialNumber; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; + m_unCameraSerialNumber = unCameraSerialNumber; // Setup camera params. m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index 5eb93d4c..1340b24f 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -21,6 +21,16 @@ #include "../../interfaces/Camera.hpp" #include "../../util/vision/FetchContainers.hpp" +/****************************************************************************** + * @brief This class implements and interfaces with the most common ZEDSDK cameras + * and features. It is designed in such a way that multiple other classes/threads + * can safely call any method of an object of this class withing resource corruption + * or slowdown of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-21 + ******************************************************************************/ class ZEDCam : public Camera, public AutonomyThread { public: @@ -41,6 +51,7 @@ class ZEDCam : public Camera, public AutonomyThread const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, const bool bMemTypeGPU = false, const bool bUseHalfDepthPrecision = false, + const int nNumFrameRetrievalThreads = 10, const unsigned int unCameraSerialNumber = 0); ~ZEDCam(); std::future RequestFrameCopy(cv::Mat& cvFrame) override; @@ -99,6 +110,7 @@ class ZEDCam : public Camera, public AutonomyThread sl::Objects m_slDetectedObjects; std::vector m_slDetectedObjectsBatched; sl::MEM m_slMemoryType; + int m_nNumFrameRetrievalThreads; unsigned int m_unCameraSerialNumber; // Mats for storing frames and measures. From fa888b1a15365d133ae86f02c61d98e53420f7e2 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Wed, 27 Sep 2023 17:59:14 +0000 Subject: [PATCH 04/11] Possibly fixed broken promise with ZEDCam, will test later. --- examples/vision/OpenZEDCam.hpp | 34 ++++++++++++++++++++++------------ src/main.cpp | 2 +- src/vision/cameras/ZEDCam.cpp | 10 ++++++---- src/vision/cameras/ZEDCam.h | 14 +++++++------- 4 files changed, 36 insertions(+), 24 deletions(-) diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index e45565ad..bdddf0ab 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/examples/vision/OpenZEDCam.hpp @@ -41,6 +41,7 @@ void RunExample() } // Declare mats to store images in. + sl::Pose slPose; cv::Mat cvNormalFrame1; cv::Mat cvDepthFrame1; cv::cuda::GpuMat cvGPUNormalFrame1; @@ -52,33 +53,42 @@ void RunExample() // Loop forever, or until user hits ESC. while (true) { - // Grab normal frame from camera. - if (TestCamera1->GrabFrame(cvGPUNormalFrame1) && TestCamera1->GrabDepth(cvGPUDepthFrame1, false)) + // Grab frames from camera. + std::future fuFrameCopyStatus = TestCamera1->RequestFrameCopy(cvGPUNormalFrame1); + // std::future fuDepthCopyStatus = TestCamera1->RequestDepthCopy(cvGPUDepthFrame1); + // Grab other info from camera. + // std::future fuPoseCopyStatus = TestCamera1->RequestPositionalPoseCopy(slPose); + + // Wait for the frames to be copied. + if (fuFrameCopyStatus.get()) { // Download memory from gpu mats if necessary. cvGPUNormalFrame1.download(cvNormalFrame1); - cvGPUDepthFrame1.download(cvDepthFrame1); + // cvGPUDepthFrame1.download(cvDepthFrame1); // Put FPS on normal frame. cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Put FPS on depth frame. - cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + // cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Display frame. cv::imshow("TEST1", cvNormalFrame1); - cv::imshow("DEPTH1", cvDepthFrame1); + // cv::imshow("DEPTH1", cvDepthFrame1); - // Get info about position. - sl::Pose slPose; - TestCamera1->GetPositionalPose(slPose); - sl::Translation slTranslation = slPose.getTranslation(); - sl::float3 slEulerAngles = slPose.getEulerAngles(false); + // // Wait for the other info to be copied. + // if (fuPoseCopyStatus.get()) + // { + // // Wait for the + // sl::Translation slTranslation = slPose.getTranslation(); + // sl::float3 slEulerAngles = slPose.getEulerAngles(false); + + // LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z); + // LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]); + // } // Print info. LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); - LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z); - LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]); // Check if spatial mapping is enabled. if (ENABLE_SPATIAL_MAPPING) { diff --git a/src/main.cpp b/src/main.cpp index 507ac33c..26d79db3 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,7 +13,7 @@ #include #include -#include "../examples/vision/OpenBasicCam.hpp" +#include "../examples/vision/OpenZEDCam.hpp" #include "./AutonomyGlobals.h" #include "./AutonomyLogging.h" #include "./interfaces/StateMachine.hpp" diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 58d526b7..63c9f05e 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -369,7 +369,7 @@ void ZEDCam::ThreadedContinuousCode() m_qObjectBatchedDataCopySchedule.size()}); // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. - this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); + this->RunDetachedPool(siMaxQueueLength, m_nNumFrameRetrievalThreads); // Wait for thread pool to finish. this->JoinPool(); // Release lock on frame copy queue. @@ -401,6 +401,7 @@ void ZEDCam::PooledLinearCode() // Check if the queue is empty. if (!m_qFrameCopySchedule.empty()) { + // TODO: Check if the frame is actually a Mat. // Get frame container out of queue. containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); // Pop out of queue. @@ -425,11 +426,12 @@ void ZEDCam::PooledLinearCode() // Use GPU mat. else { + // Aqcuire mutex for getting frames out of the queue. + std::unique_lock lkFrameQueue(m_muFrameCopyMutex); // Check if the queue is empty. if (!m_qGPUFrameCopySchedule.empty()) { - // Aqcuire mutex for getting frames out of the queue. - std::unique_lock lkFrameQueue(m_muFrameCopyMutex); + // TODO: Check if the frame is actually a GpuMat // Get frame container out of queue. containers::FrameFetchContainer stContainer = m_qGPUFrameCopySchedule.front(); // Pop out of queue. @@ -1180,7 +1182,7 @@ std::future ZEDCam::RequestPositionalPoseCopy(sl::Pose& slPose) std::promise pmDummyPromise; std::future fuDummyFuture = pmDummyPromise.get_future(); // Set future value. - pmDummyPromise.set_value(false); + pmDummyPromise.set_value(true); // Return unsuccessful. return fuDummyFuture; diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index 1340b24f..eea9137c 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -120,13 +120,13 @@ class ZEDCam : public Camera, public AutonomyThread sl::Mat m_slPointCloud; // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue>> m_qFrameCopySchedule; - std::queue>> m_qGPUFrameCopySchedule; - std::queue>>> m_qCustomBoxInjestSchedule; - std::queue>> m_qPoseCopySchedule; - std::queue>>> m_qIMUDataCopySchedule; - std::queue>>> m_qObjectDataCopySchedule; - std::queue>>> m_qObjectBatchedDataCopySchedule; + std::queue> m_qFrameCopySchedule; + std::queue> m_qGPUFrameCopySchedule; + std::queue>> m_qCustomBoxInjestSchedule; + std::queue> m_qPoseCopySchedule; + std::queue>> m_qIMUDataCopySchedule; + std::queue>> m_qObjectDataCopySchedule; + std::queue>> m_qObjectBatchedDataCopySchedule; std::shared_mutex m_muPoolScheduleMutex; std::mutex m_muFrameCopyMutex; std::mutex m_muCustomBoxInjestMutex; From 9776bc18df37e98b22f523c8e84617e924415a0e Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Wed, 27 Sep 2023 23:57:54 +0000 Subject: [PATCH 05/11] ZEDCam now works with futures. Small improvements to camer handler and constants. --- examples/vision/OpenZEDCam.hpp | 41 +++++++++++++++-------------- src/AutonomyConstants.h | 21 +++++++++------ src/threads/CameraHandlerThread.cpp | 16 +++++------ src/threads/CameraHandlerThread.h | 12 +++++++++ src/util/vision/FetchContainers.hpp | 2 +- src/util/vision/ImageOperations.hpp | 2 ++ src/vision/cameras/BasicCam.cpp | 3 +++ src/vision/cameras/ZEDCam.cpp | 35 +++++++++++++++++++++--- tools/valgrind/run_valgrind.sh | 2 +- 9 files changed, 93 insertions(+), 41 deletions(-) diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index bdddf0ab..676308b6 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/examples/vision/OpenZEDCam.hpp @@ -13,7 +13,7 @@ #include "../../src/vision/cameras/ZEDCam.h" // Declare file constants. -const bool ENABLE_SPATIAL_MAPPING = false; +const bool ENABLE_SPATIAL_MAPPING = true; /****************************************************************************** * @brief Main example method. @@ -41,11 +41,12 @@ void RunExample() } // Declare mats to store images in. - sl::Pose slPose; cv::Mat cvNormalFrame1; cv::Mat cvDepthFrame1; cv::cuda::GpuMat cvGPUNormalFrame1; cv::cuda::GpuMat cvGPUDepthFrame1; + // Declare other data types to store data in. + sl::Pose slPose; // Declare FPS counter. IPS FPS = IPS(); @@ -55,37 +56,37 @@ void RunExample() { // Grab frames from camera. std::future fuFrameCopyStatus = TestCamera1->RequestFrameCopy(cvGPUNormalFrame1); - // std::future fuDepthCopyStatus = TestCamera1->RequestDepthCopy(cvGPUDepthFrame1); + std::future fuDepthCopyStatus = TestCamera1->RequestDepthCopy(cvGPUDepthFrame1, false); // Grab other info from camera. - // std::future fuPoseCopyStatus = TestCamera1->RequestPositionalPoseCopy(slPose); + std::future fuPoseCopyStatus = TestCamera1->RequestPositionalPoseCopy(slPose); // Wait for the frames to be copied. - if (fuFrameCopyStatus.get()) + if (fuFrameCopyStatus.get() && fuDepthCopyStatus.get()) { // Download memory from gpu mats if necessary. cvGPUNormalFrame1.download(cvNormalFrame1); - // cvGPUDepthFrame1.download(cvDepthFrame1); + cvGPUDepthFrame1.download(cvDepthFrame1); // Put FPS on normal frame. cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Put FPS on depth frame. - // cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Display frame. - cv::imshow("TEST1", cvNormalFrame1); - // cv::imshow("DEPTH1", cvDepthFrame1); - - // // Wait for the other info to be copied. - // if (fuPoseCopyStatus.get()) - // { - // // Wait for the - // sl::Translation slTranslation = slPose.getTranslation(); - // sl::float3 slEulerAngles = slPose.getEulerAngles(false); - - // LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z); - // LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]); - // } + cv::imshow("FRAME1", cvNormalFrame1); + cv::imshow("DEPTH1", cvDepthFrame1); + + // Wait for the other info to be copied. + if (fuPoseCopyStatus.get()) + { + // Wait for the + sl::Translation slTranslation = slPose.getTranslation(); + sl::float3 slEulerAngles = slPose.getEulerAngles(false); + + LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z); + LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]); + } // Print info. LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h index 30758279..6da00a96 100644 --- a/src/AutonomyConstants.h +++ b/src/AutonomyConstants.h @@ -11,6 +11,7 @@ #ifndef CONSTS_H #define CONSTS_H +#include #include #include "./interfaces/Camera.hpp" @@ -55,6 +56,7 @@ namespace constants const float ZED_POSETRACK_USE_GRAVITY_ORIGIN = true; // Override 2 of the 3 rotations from initial_world_transform using the IMU. // ZedCam Spatial Mapping Config. const sl::SpatialMappingParameters::SPATIAL_MAP_TYPE ZED_MAPPING_TYPE = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::MESH; // Mesh or point cloud output. + const float ZED_MAPPING_RANGE_METER = 20.0; // The max range in meters that the ZED cameras should use for mapping. 0 = auto. const float ZED_MAPPING_RESOLUTION_METER = 0.01; // The approx goal precision for spatial mapping in METERS. Higher = Faster. const int ZED_MAPPING_MAX_MEMORY = 4096; // The max amount of CPU RAM (MB) that can be allocated for spatial mapping. const bool ZED_MAPPING_USE_CHUNK_ONLY = true; // Only update chunks that have probably changed or have new data. Faster, less accurate. @@ -67,6 +69,9 @@ namespace constants const float ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT = 0.5; // 0-1 second. Timeout to keep guessing object position when not in sight. const float ZED_OBJDETECTION_BATCH_RETENTION_TIME = 240; // The time in seconds to search for an object UUID before expiring the object. const float ZED_OBJDETECTION_BATCH_LATENCY = 2; // Short latency will limit the search for previously seen object IDs but will be closer to real time output. + + // BasicCam Basic Config. + const cv::InterpolationFlags BASICCAM_RESIZE_INTERPOLATION_METHOD = cv::InterpolationFlags::INTER_LINEAR; // The algorithm used to fill in pixels when resizing. /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// @@ -85,14 +90,14 @@ namespace constants const int ZED_MAINCAN_SERIAL = 31237348; // The serial number of the camera. // Left Side Cam. - const int BASIC_LEFTCAM_RESOLUTIONX = 1280; // The horizontal pixel resolution to resize the maincam images to. - const int BASIC_LEFTCAM_RESOLUTIONY = 720; // The vertical pixel resolution to resize the maincam images to. - const int BASIC_LEFTCAM_FPS = 30; // The FPS to use for the maincam. - const int BASIC_LEFTCAM_HORIZONTAL_FOV = 110; // The horizontal FOV of the camera. Useful for future calculations. - const int BASIC_LEFTCAM_VERTICAL_FOV = 70; // The vertical FOV of the camera. Useful for future calculations. - const int BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS = 10; // The number of threads allocated to the threadpool for performing frame copies to other threads. - const int BASIC_LEFTCAM_INDEX = 0; // The /dev/video index of the camera. - const PIXEL_FORMATS BASIC_LEFTCAM_PIXELTYPE = PIXEL_FORMATS::eBGR; // The pixel layout of the camera. + const int BASICCAM_LEFTCAM_RESOLUTIONX = 1280; // The horizontal pixel resolution to resize the maincam images to. + const int BASICCAM_LEFTCAM_RESOLUTIONY = 720; // The vertical pixel resolution to resize the maincam images to. + const int BASICCAM_LEFTCAM_FPS = 30; // The FPS to use for the maincam. + const int BASICCAM_LEFTCAM_HORIZONTAL_FOV = 110; // The horizontal FOV of the camera. Useful for future calculations. + const int BASICCAM_LEFTCAM_VERTICAL_FOV = 70; // The vertical FOV of the camera. Useful for future calculations. + const int BASICCAM_LEFTCAM_FRAME_RETRIEVAL_THREADS = 10; // The number of threads allocated to the threadpool for performing frame copies to other threads. + const int BASICCAM_LEFTCAM_INDEX = 0; // The /dev/video index of the camera. + const PIXEL_FORMATS BASICCAM_LEFTCAM_PIXELTYPE = PIXEL_FORMATS::eBGR; // The pixel layout of the camera. /////////////////////////////////////////////////////////////////////////// } // namespace constants diff --git a/src/threads/CameraHandlerThread.cpp b/src/threads/CameraHandlerThread.cpp index 0cc60838..161a0f22 100644 --- a/src/threads/CameraHandlerThread.cpp +++ b/src/threads/CameraHandlerThread.cpp @@ -34,14 +34,14 @@ CameraHandlerThread::CameraHandlerThread() constants::ZED_MAINCAN_SERIAL); // Initialize Left acruco eye. - m_pLeftCam = new BasicCam(constants::BASIC_LEFTCAM_INDEX, - constants::BASIC_LEFTCAM_RESOLUTIONX, - constants::BASIC_LEFTCAM_RESOLUTIONY, - constants::BASIC_LEFTCAM_FPS, - constants::BASIC_LEFTCAM_PIXELTYPE, - constants::BASIC_LEFTCAM_HORIZONTAL_FOV, - constants::BASIC_LEFTCAM_VERTICAL_FOV, - constants::BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS); + m_pLeftCam = new BasicCam(constants::BASICCAM_LEFTCAM_INDEX, + constants::BASICCAM_LEFTCAM_RESOLUTIONX, + constants::BASICCAM_LEFTCAM_RESOLUTIONY, + constants::BASICCAM_LEFTCAM_FPS, + constants::BASICCAM_LEFTCAM_PIXELTYPE, + constants::BASICCAM_LEFTCAM_HORIZONTAL_FOV, + constants::BASICCAM_LEFTCAM_VERTICAL_FOV, + constants::BASICCAM_LEFTCAM_FRAME_RETRIEVAL_THREADS); } /****************************************************************************** diff --git a/src/threads/CameraHandlerThread.h b/src/threads/CameraHandlerThread.h index 4a30ce90..d96cc776 100644 --- a/src/threads/CameraHandlerThread.h +++ b/src/threads/CameraHandlerThread.h @@ -30,12 +30,18 @@ class CameraHandlerThread { private: + ///////////////////////////////////////// // Declare private class member variables. + ///////////////////////////////////////// + ZEDCam* m_pMainCam; BasicCam* m_pLeftCam; public: + ///////////////////////////////////////// // Define public enumerators specific to this class. + ///////////////////////////////////////// + enum ZEDCamName // Enum for different zed cameras. { eHeadMainCam @@ -47,12 +53,18 @@ class CameraHandlerThread eHeadRightAcuroEye }; + ///////////////////////////////////////// // Declare public class methods and variables. + ///////////////////////////////////////// + CameraHandlerThread(); ~CameraHandlerThread(); void StartAllCameras(); + ///////////////////////////////////////// // Accessors. + ///////////////////////////////////////// + ZEDCam* GetZED(ZEDCamName eCameraName); BasicCam* GetBasicCam(BasicCamName eCameraName); }; diff --git a/src/util/vision/FetchContainers.hpp b/src/util/vision/FetchContainers.hpp index ae3939dd..77bdff51 100644 --- a/src/util/vision/FetchContainers.hpp +++ b/src/util/vision/FetchContainers.hpp @@ -137,7 +137,7 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - DataFetchContainer(T& tData) : pData(&tData) {} + DataFetchContainer(T& tData) : pData(&tData), pCopiedDataStatus(std::make_shared>()) {} /****************************************************************************** * @brief Copy Construct a new Frame Fetch Container object. diff --git a/src/util/vision/ImageOperations.hpp b/src/util/vision/ImageOperations.hpp index 97ebb139..cb175652 100644 --- a/src/util/vision/ImageOperations.hpp +++ b/src/util/vision/ImageOperations.hpp @@ -46,6 +46,7 @@ namespace imgops case sl::MAT_TYPE::F32_C2: nOpenCVType = CV_32FC2; break; case sl::MAT_TYPE::F32_C3: nOpenCVType = CV_32FC3; break; case sl::MAT_TYPE::F32_C4: nOpenCVType = CV_32FC4; break; + case sl::MAT_TYPE::U16_C1: nOpenCVType = CV_16SC1; break; case sl::MAT_TYPE::U8_C1: nOpenCVType = CV_8UC1; break; case sl::MAT_TYPE::U8_C2: nOpenCVType = CV_8UC2; break; case sl::MAT_TYPE::U8_C3: nOpenCVType = CV_8UC3; break; @@ -78,6 +79,7 @@ namespace imgops case CV_32FC2: slMatType = sl::MAT_TYPE::F32_C2; break; case CV_32FC3: slMatType = sl::MAT_TYPE::F32_C3; break; case CV_32FC4: slMatType = sl::MAT_TYPE::F32_C4; break; + case CV_16SC1: slMatType = sl::MAT_TYPE::U16_C1; break; case CV_8UC1: slMatType = sl::MAT_TYPE::U8_C1; break; case CV_8UC2: slMatType = sl::MAT_TYPE::U8_C2; break; case CV_8UC3: slMatType = sl::MAT_TYPE::U8_C3; break; diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index 8e0ceb04..7505088a 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -158,6 +158,9 @@ void BasicCam::ThreadedContinuousCode() // Check if new frame was computed successfully. if (m_cvCamera.read(m_cvFrame)) { + // Resize the frame. + cv::resize(m_cvFrame, m_cvFrame, cv::Size(m_nPropResolutionX, m_nPropResolutionY), 0.0, 0.0, constants::BASICCAM_RESIZE_INTERPOLATION_METHOD); + // Call FPS tick. m_IPS.Tick(); } diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 63c9f05e..68180aac 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -132,6 +132,17 @@ ZEDCam::ZEDCam(const int nPropResolutionX, m_slSpatialMappingParams.save_texture = true; m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; + // Set or auto-set max depth range for mapping. + if (constants::ZED_MAPPING_RANGE_METER >= 0) + { + // Automatically guess the best mapping depth range. + m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); + } + else + { + // Manually set. + m_slSpatialMappingParams.range_meter = constants::ZED_MAPPING_RANGE_METER; + } // Setup object detection/tracking parameters. m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; @@ -401,7 +412,6 @@ void ZEDCam::PooledLinearCode() // Check if the queue is empty. if (!m_qFrameCopySchedule.empty()) { - // TODO: Check if the frame is actually a Mat. // Get frame container out of queue. containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); // Pop out of queue. @@ -422,6 +432,16 @@ void ZEDCam::PooledLinearCode() // Signal future that the frame has been successfully retrieved. stContainer.pCopiedFrameStatus->set_value(true); } + + // Check if anything has been added to the GPU queue. + if (!m_qGPUFrameCopySchedule.empty()) + { + // Submit logger error. + LOG_ERROR(g_qSharedLogger, + "ZEDCam ({}) is in CPU sl::Mat mode but a GPU mat has been added to the copy queue! Whichever thread queued the frame will now appear frozen if " + "future.get() is called. Either switch the camera to GPU Mat mode in AutonomyConstants.h or stop queueing frames of type cv::Mat.", + m_unCameraSerialNumber); + } } // Use GPU mat. else @@ -431,7 +451,6 @@ void ZEDCam::PooledLinearCode() // Check if the queue is empty. if (!m_qGPUFrameCopySchedule.empty()) { - // TODO: Check if the frame is actually a GpuMat // Get frame container out of queue. containers::FrameFetchContainer stContainer = m_qGPUFrameCopySchedule.front(); // Pop out of queue. @@ -452,6 +471,16 @@ void ZEDCam::PooledLinearCode() // Signal future that the frame has been successfully retrieved. stContainer.pCopiedFrameStatus->set_value(true); } + + // Check if anything has been added to the GPU queue. + if (!m_qFrameCopySchedule.empty()) + { + // Submit logger error. + LOG_ERROR(g_qSharedLogger, + "ZEDCam ({}) is in GPU sl::Mat mode but a CPU mat has been added to the copy queue! Whichever thread queued the frame will now appear frozen if " + "future.get() is called. Either switch the camera to GPU Mat mode in AutonomyConstants.h or stop queueing frames of type cv::cuda::GpuMat.", + m_unCameraSerialNumber); + } } ///////////////////////////// @@ -1182,7 +1211,7 @@ std::future ZEDCam::RequestPositionalPoseCopy(sl::Pose& slPose) std::promise pmDummyPromise; std::future fuDummyFuture = pmDummyPromise.get_future(); // Set future value. - pmDummyPromise.set_value(true); + pmDummyPromise.set_value(false); // Return unsuccessful. return fuDummyFuture; diff --git a/tools/valgrind/run_valgrind.sh b/tools/valgrind/run_valgrind.sh index 29934a96..4eb29821 100755 --- a/tools/valgrind/run_valgrind.sh +++ b/tools/valgrind/run_valgrind.sh @@ -23,7 +23,7 @@ cp /usr/local/share/opencv*/*.supp $script_dir supp_files=$(find "$script_dir" -maxdepth 1 -type f -name "*.supp") # Construct the Valgrind command -valgrind_cmd="valgrind -s --leak-check=yes --show-leak-kinds=all " +valgrind_cmd="valgrind -s --leak-check=yes --show-leak-kinds=all --track-origins=yes " if [ "$1" = "GitHub-Action" ]; then valgrind_cmd+=" --log-file=/opt/Autonomy_Software/tools/valgrind/valgrind.rpt" fi From cac1fedcefc4b16baa60d39a44f4020987681644 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Wed, 27 Sep 2023 23:58:12 +0000 Subject: [PATCH 06/11] Make clang format break long string literals. --- .clang-format | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.clang-format b/.clang-format index a7147bf0..c80deb35 100644 --- a/.clang-format +++ b/.clang-format @@ -48,7 +48,7 @@ BreakBeforeBinaryOperators: None BreakBeforeTernaryOperators: true BreakConstructorInitializers: AfterColon BreakInheritanceList: AfterColon -BreakStringLiterals: false +BreakStringLiterals: true ColumnLimit: 170 CompactNamespaces: false ContinuationIndentWidth: 4 From 793e0aca6533accb4a1ce3a43448b5ecc903dcd4 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 28 Sep 2023 00:21:04 +0000 Subject: [PATCH 07/11] Moved to some member vars to Camera.hpp interface to hopefully inspire future programmers to use them. --- examples/vision/OpenZEDCam.hpp | 2 +- src/interfaces/Camera.hpp | 38 ++++++----------------------- src/util/vision/FetchContainers.hpp | 32 ++++++++++++++++++++++++ src/vision/cameras/BasicCam.h | 7 ------ src/vision/cameras/ZEDCam.h | 5 ---- 5 files changed, 41 insertions(+), 43 deletions(-) diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index 676308b6..aeaa44a4 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/examples/vision/OpenZEDCam.hpp @@ -13,7 +13,7 @@ #include "../../src/vision/cameras/ZEDCam.h" // Declare file constants. -const bool ENABLE_SPATIAL_MAPPING = true; +const bool ENABLE_SPATIAL_MAPPING = false; /****************************************************************************** * @brief Main example method. diff --git a/src/interfaces/Camera.hpp b/src/interfaces/Camera.hpp index ec874c13..54dcc9ab 100644 --- a/src/interfaces/Camera.hpp +++ b/src/interfaces/Camera.hpp @@ -12,37 +12,10 @@ #define CAMERA_HPP #include "../util/IPS.hpp" -#include - -// Declare global/file-scope enumerator. -enum PIXEL_FORMATS -{ - eRGB, - eBGR, - eRGBA, - eBGRA, - eARGB, - eABGR, - eRGBE, - eXYZ, - eRGBXZY, - eRGBAXYZ, - eZED, - eGrayscale, - eDepthImage, - eDepthMeasure, - eCMYK, - eYUV, - eYUYV, - eYUVJ, - eHSV, - eHSL, - eSRGB, - eLAB, - eUNKNOWN -}; +#include "../util/vision/FetchContainers.hpp" -/////////////////////////////////////////////////////////////////////////////// +#include +#include /****************************************************************************** * @brief This interface class serves as a base for all other classes that will @@ -67,6 +40,11 @@ class Camera double m_dPropHorizontalFOV; double m_dPropVerticalFOV; + // Queues and mutexes for scheduling and copying camera frames and data to other threads. + std::queue> m_qFrameCopySchedule; + std::shared_mutex m_muPoolScheduleMutex; + std::mutex m_muFrameCopyMutex; + // Declare interface class pure virtual functions. (These must be overriden by inheritor.) virtual std::future RequestFrameCopy(T& tFrame) = 0; // This is where the code to retrieve an image from the camera is put. virtual bool GetCameraIsOpen() = 0; // This is where the code to check if the camera is current open goes. diff --git a/src/util/vision/FetchContainers.hpp b/src/util/vision/FetchContainers.hpp index 77bdff51..5f7670f2 100644 --- a/src/util/vision/FetchContainers.hpp +++ b/src/util/vision/FetchContainers.hpp @@ -14,10 +14,42 @@ #ifndef FETCH_CONTAINERS_HPP #define FETCH_CONTAINERS_HPP +#include "../../interfaces/Camera.hpp" + #include #include #include +// Declare global/file-scope enumerator. +enum PIXEL_FORMATS +{ + eRGB, + eBGR, + eRGBA, + eBGRA, + eARGB, + eABGR, + eRGBE, + eXYZ, + eRGBXZY, + eRGBAXYZ, + eZED, + eGrayscale, + eDepthImage, + eDepthMeasure, + eCMYK, + eYUV, + eYUYV, + eYUVJ, + eHSV, + eHSL, + eSRGB, + eLAB, + eUNKNOWN +}; + +/////////////////////////////////////////////////////////////////////////////// + /****************************************************************************** * @brief Namespace containing functions or objects/struct used to aid in data * storage and passage between threads. diff --git a/src/vision/cameras/BasicCam.h b/src/vision/cameras/BasicCam.h index a7814ca7..d47c5ebb 100644 --- a/src/vision/cameras/BasicCam.h +++ b/src/vision/cameras/BasicCam.h @@ -12,11 +12,9 @@ #define BASICCAM_H #include -#include #include "../../interfaces/AutonomyThread.hpp" #include "../../interfaces/Camera.hpp" -#include "../../util/vision/FetchContainers.hpp" /****************************************************************************** * @brief This class implements and interfaces with the most common USB cameras @@ -74,11 +72,6 @@ class BasicCam : public Camera, public AutonomyThread // Mats for storing frames. cv::Mat m_cvFrame; - // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue> m_qFrameCopySchedule; - std::shared_mutex m_muPoolScheduleMutex; - std::mutex m_muFrameCopyMutex; - ///////////////////////////////////////// // Declare private methods. ///////////////////////////////////////// diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index eea9137c..3aac8a9a 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -13,13 +13,11 @@ #include #include -#include #include #include "../../AutonomyConstants.h" #include "../../interfaces/AutonomyThread.hpp" #include "../../interfaces/Camera.hpp" -#include "../../util/vision/FetchContainers.hpp" /****************************************************************************** * @brief This class implements and interfaces with the most common ZEDSDK cameras @@ -120,15 +118,12 @@ class ZEDCam : public Camera, public AutonomyThread sl::Mat m_slPointCloud; // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue> m_qFrameCopySchedule; std::queue> m_qGPUFrameCopySchedule; std::queue>> m_qCustomBoxInjestSchedule; std::queue> m_qPoseCopySchedule; std::queue>> m_qIMUDataCopySchedule; std::queue>> m_qObjectDataCopySchedule; std::queue>> m_qObjectBatchedDataCopySchedule; - std::shared_mutex m_muPoolScheduleMutex; - std::mutex m_muFrameCopyMutex; std::mutex m_muCustomBoxInjestMutex; std::mutex m_muPoseCopyMutex; std::mutex m_muIMUDataCopyMutex; From 204ee2ad1f5c9e7517aa030e9a28bbbfc674cc3d Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 28 Sep 2023 00:39:43 +0000 Subject: [PATCH 08/11] Cleaned up examples. Changed default example to BasicCam. --- examples/vision/OpenBasicCam.hpp | 13 +++++- examples/vision/OpenZEDCam.hpp | 72 +++++++++++++++++++++++++------- src/main.cpp | 2 +- 3 files changed, 69 insertions(+), 18 deletions(-) diff --git a/examples/vision/OpenBasicCam.hpp b/examples/vision/OpenBasicCam.hpp index 56a5f674..0678a06f 100644 --- a/examples/vision/OpenBasicCam.hpp +++ b/examples/vision/OpenBasicCam.hpp @@ -13,8 +13,19 @@ #include "../../src/vision/cameras/BasicCam.h" /****************************************************************************** - * @brief Main example method. + * @brief This example demonstrates the proper way to interact with the CameraHandler. + * A pointer to a BasicCam is retrieved and then a couple of local cv::Mat are created + * for storing frames. Then, the frames are passed to the RequestFrameCopy function of the + * camera and a future is IMMEDIATELY returned. The method call doesn't wait for frame to be + * retrieved/copied before returning. This allows you to request multiple frames/data from the + * camera non-sequentially. * + * Inside the camera thread, the cv::Mat pointer that points to the cv::Mat within THIS class + * is written to and an std::promise is set to TRUE. The future that was return now contains this + * TRUE value. When the get() method is called on the returned future, the code will block until + * the promise is fullfilled (set to TRUE). Once the get() method returns, the cv::Mat within + * this class now contains a complete frame and can be display or used in other computer vision + * things. * * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-07-22 diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index aeaa44a4..d6a0e3c3 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/examples/vision/OpenZEDCam.hpp @@ -7,6 +7,7 @@ * @date 2023-09-16 ******************************************************************************/ +#include "../../src/AutonomyConstants.h" #include "../../src/AutonomyGlobals.h" #include "../../src/AutonomyLogging.h" #include "../../src/util/ExampleChecker.h" @@ -16,7 +17,23 @@ const bool ENABLE_SPATIAL_MAPPING = false; /****************************************************************************** - * @brief Main example method. + * @brief This example demonstrates the proper way to interact with the CameraHandler. + * A pointer to a ZEDCam is retrieved and then a couple of local cv::Mat are created + * for storing frames. Then, the frames are passed to the RequestFrameCopy function of the + * camera and a future is IMMEDIATELY returned. The method call doesn't wait for frame to be + * retrieved/copied before returning. This allows you to request multiple frames/data from the + * camera non-sequentially. + * + * Inside the camera thread, the cv::Mat pointer that points to the cv::Mat within THIS class + * is written to and an std::promise is set to TRUE. The future that was return now contains this + * TRUE value. When the get() method is called on the returned future, the code will block until + * the promise is fullfilled (set to TRUE). Once the get() method returns, the cv::Mat within + * this class now contains a complete frame and can be display or used in other computer vision + * things. + * + * The same exact process happens for the positional tracking pose that is retrieved from the camera. + * Multiple other methods of the ZEDCam class work this way as it allows this thread and other threads + * to get multiple pieces of from the camera without slowing it down to an unusable speed. * * * @author ClayJay3 (claytonraycowen@gmail.com) @@ -29,15 +46,15 @@ void RunExample() g_pCameraHandler->StartAllCameras(); // Get reference to camera. - ZEDCam* TestCamera1 = g_pCameraHandler->GetZED(CameraHandlerThread::eHeadMainCam); + ZEDCam* ExampleZEDCam1 = g_pCameraHandler->GetZED(CameraHandlerThread::eHeadMainCam); // Turn on ZED features. - TestCamera1->EnablePositionalTracking(); + ExampleZEDCam1->EnablePositionalTracking(); // Check if we should turn on spatial mapping. if (ENABLE_SPATIAL_MAPPING) { // Enable spatial mapping. - TestCamera1->EnableSpatialMapping(); + ExampleZEDCam1->EnableSpatialMapping(); } // Declare mats to store images in. @@ -54,24 +71,47 @@ void RunExample() // Loop forever, or until user hits ESC. while (true) { - // Grab frames from camera. - std::future fuFrameCopyStatus = TestCamera1->RequestFrameCopy(cvGPUNormalFrame1); - std::future fuDepthCopyStatus = TestCamera1->RequestDepthCopy(cvGPUDepthFrame1, false); + // Create instance variables. + std::future fuFrameCopyStatus; + std::future fuDepthCopyStatus; + + // Check if the camera is setup to use CPU or GPU mats. + if (constants::ZED_MAINCAM_USE_GPU_MAT) + { + // Grab frames from camera. + fuFrameCopyStatus = ExampleZEDCam1->RequestFrameCopy(cvGPUNormalFrame1); + fuDepthCopyStatus = ExampleZEDCam1->RequestDepthCopy(cvGPUDepthFrame1, false); + } + else + { + // Grab frames from camera. + fuFrameCopyStatus = ExampleZEDCam1->RequestFrameCopy(cvNormalFrame1); + fuDepthCopyStatus = ExampleZEDCam1->RequestDepthCopy(cvDepthFrame1, false); + } // Grab other info from camera. - std::future fuPoseCopyStatus = TestCamera1->RequestPositionalPoseCopy(slPose); + std::future fuPoseCopyStatus = ExampleZEDCam1->RequestPositionalPoseCopy(slPose); // Wait for the frames to be copied. if (fuFrameCopyStatus.get() && fuDepthCopyStatus.get()) { - // Download memory from gpu mats if necessary. - cvGPUNormalFrame1.download(cvNormalFrame1); - cvGPUDepthFrame1.download(cvDepthFrame1); + // Check if the camera is setup to use CPU or GPU mats. + if (constants::ZED_MAINCAM_USE_GPU_MAT) + { + // Download memory from gpu mats if necessary. + cvGPUNormalFrame1.download(cvNormalFrame1); + cvGPUDepthFrame1.download(cvDepthFrame1); + } // Put FPS on normal frame. - cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(cvNormalFrame1, + std::to_string(ExampleZEDCam1->GetIPS().GetExactIPS()), + cv::Point(50, 50), + cv::FONT_HERSHEY_COMPLEX, + 1, + cv::Scalar(255, 255, 255)); // Put FPS on depth frame. - cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(cvDepthFrame1, std::to_string(ExampleZEDCam1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); // Display frame. cv::imshow("FRAME1", cvNormalFrame1); @@ -89,11 +129,11 @@ void RunExample() } // Print info. - LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); + LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", ExampleZEDCam1->GetIPS().GetAverageIPS(), ExampleZEDCam1->GetIPS().Get1PercentLow()); // Check if spatial mapping is enabled. if (ENABLE_SPATIAL_MAPPING) { - LOG_INFO(g_qConsoleLogger, "Spatial Mapping State: {}", sl::toString(TestCamera1->GetSpatialMappingState()).get()); + LOG_INFO(g_qConsoleLogger, "Spatial Mapping State: {}", sl::toString(ExampleZEDCam1->GetSpatialMappingState()).get()); } } @@ -114,7 +154,7 @@ void RunExample() { // Extract spatial map. std::future fuSpatialMap; - TestCamera1->ExtractSpatialMapAsync(fuSpatialMap); + ExampleZEDCam1->ExtractSpatialMapAsync(fuSpatialMap); sl::Mesh slSpatialMap = fuSpatialMap.get(); slSpatialMap.save("test.obj", sl::MESH_FILE_FORMAT::PLY); } diff --git a/src/main.cpp b/src/main.cpp index 26d79db3..507ac33c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,7 +13,7 @@ #include #include -#include "../examples/vision/OpenZEDCam.hpp" +#include "../examples/vision/OpenBasicCam.hpp" #include "./AutonomyGlobals.h" #include "./AutonomyLogging.h" #include "./interfaces/StateMachine.hpp" From 2ade1505ce5c666c7212f2d9a827890416fb4253 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 28 Sep 2023 00:52:23 +0000 Subject: [PATCH 09/11] Cleaned up class method documentation. --- src/vision/cameras/BasicCam.cpp | 7 +++--- src/vision/cameras/ZEDCam.cpp | 43 ++++++++++++++++++++------------- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index 7505088a..bf54ab8b 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -22,6 +22,7 @@ * @param ePropPixelFormat - The pixel layout/format of the image. * @param dPropHorizontalFOV - The horizontal field of view. * @param dPropVerticalFOV - The vertical field of view. + * @param nNumFrameRetrievalThreads - The number of threads to use for frame queueing and copying. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-20 @@ -68,6 +69,7 @@ BasicCam::BasicCam(const std::string szCameraPath, * @param ePropPixelFormat - The pixel layout/format of the image. * @param dPropHorizontalFOV - The horizontal field of view. * @param dPropVerticalFOV - The vertical field of view. + * @param nNumFrameRetrievalThreads - The number of threads to use for frame queueing and copying. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-20 @@ -217,9 +219,8 @@ void BasicCam::PooledLinearCode() } /****************************************************************************** - * @brief Retrieves a frame from the camera. The given mat reference is placed - * into a queue for copying. Remember, this code will be ran in whatever, - * class/thread calls it. + * @brief Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * Remember, this code will be ran in whatever, class/thread calls it. * * @param cvFrame - A reference to the cv::Mat to store the frame in. * @return std::future - A future that should be waited on before the passed in frame is used. diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 68180aac..5b092cf6 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -392,7 +392,7 @@ void ZEDCam::ThreadedContinuousCode() /****************************************************************************** * @brief This method holds the code that is ran in the thread pool started by * the ThreadedLinearCode() method. It copies the data from the different - * data objects to references of the same type stored in a vector queued up by the + * data objects to references of the same type stored in a queue filled by the * Grab methods. * * @@ -582,7 +582,8 @@ void ZEDCam::PooledLinearCode() } /****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. + * @brief Requests a regular BGRA image from the LEFT eye of the zed camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. * Remember this code will be ran in whatever class/thread calls it. * * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. @@ -609,8 +610,8 @@ std::future ZEDCam::RequestFrameCopy(cv::Mat& cvFrame) } /****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and - * stores it in a GPU mat. + * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and stores it in a GPU mat. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. * Remember this code will be ran in whatever class/thread calls it. * * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. @@ -637,9 +638,10 @@ std::future ZEDCam::RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame) } /****************************************************************************** - * @brief Grabs a depth measure or image from the camera. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. + * @brief Requests a depth measure or image from the camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * This image has the same shape as a grayscale image, but the values represent the depth in + * MILLIMETERS. The ZEDSDK will always return this measure in MILLIMETERS. * * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image @@ -672,9 +674,10 @@ std::future ZEDCam::RequestDepthCopy(cv::Mat& cvDepth, const bool bRetriev } /****************************************************************************** - * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. + * @brief Requests a depth measure or image from the camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * This image has the same shape as a grayscale image, but the values represent the depth in + * MILLIMETERS. The ZEDSDK will always return this measure in MILLIMETERS. * * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image @@ -707,11 +710,13 @@ std::future ZEDCam::RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const b } /****************************************************************************** - * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal + * @brief Requests a point cloud image from the camera. This image has the same resolution as a normal * image but with three XYZ values replacing the old color values in the 3rd dimension. * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM * constants set in AutonomyConstants.h. * + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. * @return std::future - A future that should be waited on before the passed in frame is used. * Value will be true if frame was succesfully retrieved. @@ -741,6 +746,8 @@ std::future ZEDCam::RequestPointCloudCopy(cv::Mat& cvPointCloud) * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM * constants set in AutonomyConstants.h. * + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. * @return std::future - A future that should be waited on before the passed in frame is used. * Value will be true if frame was succesfully retrieved. @@ -1174,7 +1181,8 @@ unsigned int ZEDCam::GetCameraSerial() } /****************************************************************************** - * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. + * @brief Requests the current pose of the camera relative to it's start pose or the origin of the set pose. + * Puts a Pose pointer into a queue so a copy of a frame from the camera can be written to it. * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. * * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. @@ -1233,8 +1241,9 @@ bool ZEDCam::GetPositionalTrackingEnabled() } /****************************************************************************** - * @brief Gets the IMU data from the ZED camera. If getting the data fails, the - * last successfully retrieved value is returned. + * @brief Requests the IMU data from the ZED camera. + * Puts a std::vector pointer into a queue so a copy of a frame from the camera can be written to it. + * If getting the data fails, the last successfully retrieved value is returned. * * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. * @return std::future - A future that should be waited on before the passed in vector is used. @@ -1357,8 +1366,8 @@ bool ZEDCam::GetObjectDetectionEnabled() } /****************************************************************************** - * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the - * given sl::ObjectData vector. + * @brief Requests a current copy of the tracked objects from the camera. + * Puts a pointer to a vector containing sl::ObjectData into a queue so a copy of a frame from the camera can be written to it. * * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. * @return std::future - A future that should be waited on before the passed in vector is used. @@ -1402,7 +1411,7 @@ std::future ZEDCam::RequestObjectsCopy(std::vector& vObjec } /****************************************************************************** - * @brief If batching is enabled, this retrieves the normal objects and passes them to + * @brief If batching is enabled, this requests the normal objects and passes them to * the the iternal batching queue of the zed api. This performs short-term re-identification * with deep learning and trajectories filtering. Batching must have been set to enabled when * EnableObjectDetection() was called. Most of the time the vector will be empty and will be From 150e58c6f9b6307bac44f5570b19ce33e2f59291 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 28 Sep 2023 00:57:38 +0000 Subject: [PATCH 10/11] Fixed IMU copy bug. --- src/vision/cameras/ZEDCam.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 5b092cf6..7097c83a 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -524,6 +524,8 @@ void ZEDCam::PooledLinearCode() sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); // Get IMU linear acceleration. sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; + // Clear the data vector that the IMUData will be copied to. + stContainer.pData->clear(); // Repackage angles and accels into vector. stContainer.pData->emplace_back(slAngles.x); stContainer.pData->emplace_back(slAngles.y); From a559e937bd926f3e0d900458da27855e0853ae77 Mon Sep 17 00:00:00 2001 From: clayjay3 Date: Thu, 28 Sep 2023 01:03:22 +0000 Subject: [PATCH 11/11] Rename camera is BasicCam example. --- examples/vision/OpenBasicCam.hpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/examples/vision/OpenBasicCam.hpp b/examples/vision/OpenBasicCam.hpp index 0678a06f..c1433120 100644 --- a/examples/vision/OpenBasicCam.hpp +++ b/examples/vision/OpenBasicCam.hpp @@ -37,7 +37,7 @@ void RunExample() g_pCameraHandler->StartAllCameras(); // Get reference to camera. - BasicCam* TestCamera1 = g_pCameraHandler->GetBasicCam(CameraHandlerThread::eHeadLeftArucoEye); + BasicCam* ExampleBasicCam1 = g_pCameraHandler->GetBasicCam(CameraHandlerThread::eHeadLeftArucoEye); // Declare mats to store images in. cv::Mat cvNormalFrame1; cv::Mat cvNormalFrame2; @@ -49,17 +49,22 @@ void RunExample() while (true) { // Grab normal frame from camera. - std::future fuCopyStatus1 = TestCamera1->RequestFrameCopy(cvNormalFrame1); - std::future fuCopyStatus2 = TestCamera1->RequestFrameCopy(cvNormalFrame2); + std::future fuCopyStatus1 = ExampleBasicCam1->RequestFrameCopy(cvNormalFrame1); + std::future fuCopyStatus2 = ExampleBasicCam1->RequestFrameCopy(cvNormalFrame2); // Show first frame copy. if (fuCopyStatus1.get() && !cvNormalFrame1.empty()) { // Print info. - LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); + LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", ExampleBasicCam1->GetIPS().GetAverageIPS(), ExampleBasicCam1->GetIPS().Get1PercentLow()); // Put FPS on normal frame. - cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(cvNormalFrame1, + std::to_string(ExampleBasicCam1->GetIPS().GetExactIPS()), + cv::Point(50, 50), + cv::FONT_HERSHEY_COMPLEX, + 1, + cv::Scalar(255, 255, 255)); // Display frame. cv::imshow("BasicCamExample Frame1", cvNormalFrame1); @@ -69,10 +74,15 @@ void RunExample() if (fuCopyStatus2.get() && !cvNormalFrame2.empty()) { // Print info. - LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow()); + LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", ExampleBasicCam1->GetIPS().GetAverageIPS(), ExampleBasicCam1->GetIPS().Get1PercentLow()); // Put FPS on normal frame. - cv::putText(cvNormalFrame2, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255)); + cv::putText(cvNormalFrame2, + std::to_string(ExampleBasicCam1->GetIPS().GetExactIPS()), + cv::Point(50, 50), + cv::FONT_HERSHEY_COMPLEX, + 1, + cv::Scalar(255, 255, 255)); // Display frame. cv::imshow("BasicCamExample Frame2", cvNormalFrame2);