ZEDCamera.hpp

Go to the documentation of this file.

 
#ifndef ZEDCAMERA_HPP
#define ZEDCAMERA_HPP
 
#include "../AutonomyLogging.h"
#include "../util/GeospatialOperations.hpp"
#include "Camera.hpp"
 
#include <sl/Camera.hpp>
#include <sl/Fusion.hpp>
 
 
 
class ZEDCamera : public Camera<cv::Mat>
{
    public:
        // Declare public structs that are specific to and used within this class.
 
 
        struct 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.
 
                struct Corner
                {
                    public:
                        // Declare public struct member variables.
                        unsigned int nX;
                        unsigned int nY;
                };
 
            public:
                // Declare and define public struct member variables.
                Corner CornerTL;      // The top left corner of the bounding box.
                Corner CornerTR;      // The top right corner of the bounding box.
                Corner CornerBL;      // The bottom left corner of the bounding box.
                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; };
        };
 
 
        struct Pose
        {
            private:
                // Declare struct for storing translation values.
                struct Translation
                {
                    public:
                        double dX;    // Translation in ZED_MEASURE_UNITS on the x-axis.
                        double dY;    // Translation in ZED_MEASURE_UNITS on the y-axis.
                        double dZ;    // Translation in ZED_MEASURE_UNITS on the z-axis.
                };
 
                // Declare struct for storing rotation values.
                struct EulerAngles
                {
                    public:
                        double dXO;    // Rotation in degrees around the x-axis.
                        double dYO;    // Rotation in degrees around the y-axis.
                        double dZO;    // Rotation in degrees around the z-axis.
                };
 
            public:
                // Declare struct public member variables.
                Translation stTranslation;
                EulerAngles stEulerAngles;
 
 
                Pose(const double dX = 0.0, const double dY = 0.0, const double dZ = 0.0, const double dXO = 0.0, const double dYO = 0.0, const double dZO = 0.0)
                {
                    // Initialize member variables.
                    stTranslation.dX  = dX;
                    stTranslation.dY  = dY;
                    stTranslation.dZ  = dZ;
                    stEulerAngles.dXO = dXO;
                    stEulerAngles.dYO = dYO;
                    stEulerAngles.dZO = dZO;
                }
        };
 
        // Declare public methods and member variables.
 
 
        ZEDCamera(const int nPropResolutionX,
                  const int nPropResolutionY,
                  const int nPropFramesPerSecond,
                  const double dPropHorizontalFOV,
                  const double dPropVerticalFOV,
                  const bool bEnableRecordingFlag,
                  const bool bMemTypeGPU,
                  const bool bUseHalfDepthPrecision,
                  const bool bEnableFusionMaster,
                  const int nNumFrameRetrievalThreads,
                  const unsigned int unCameraSerialNumber) :
            Camera(nPropResolutionX,
                   nPropResolutionY,
                   nPropFramesPerSecond,
                   PIXEL_FORMATS::eZED,
                   dPropHorizontalFOV,
                   dPropVerticalFOV,
                   bEnableRecordingFlag,
                   nNumFrameRetrievalThreads)
        {
            // Initialize member variables. Some parameters are not used.
            (void) bMemTypeGPU;
            (void) bUseHalfDepthPrecision;
            (void) bEnableFusionMaster;
            m_bCameraIsFusionMaster = bEnableFusionMaster;
            m_unCameraSerialNumber  = unCameraSerialNumber;
        }
 
 
        virtual ~ZEDCamera() = default;
 
 
        std::future<bool> RequestFrameCopy(cv::Mat& cvFrame) override = 0;
 
 
        virtual std::future<bool> RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame)
        {
            // Create instance variables.
            (void) cvGPUFrame;
            std::promise<bool> pmPromise;
            std::future<bool> fuFuture = pmPromise.get_future();
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame) not implemented. If SIM_MODE use cv::Mat version instead.");
 
            return fuFuture;
        }
 
 
        virtual std::future<bool> RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure = true) = 0;
 
 
        virtual std::future<bool> RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true)
        {
            // Initialize instance variables.
            (void) cvGPUDepth;
            (void) bRetrieveMeasure;
            std::promise<bool> pmPromise;
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger,
                      "ZEDCamera::RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true) not implemented. If SIM_MODE use cv::Mat version "
                      "instead.");
 
            return pmPromise.get_future();
        }
 
 
        virtual std::future<bool> RequestPointCloudCopy(cv::Mat& cvPointCloud) = 0;
 
 
        virtual std::future<bool> RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud)
        {
            // Initialize instance variables.
            (void) cvGPUPointCloud;
            std::promise<bool> pmPromise;
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger,
                      "ZEDCamera::RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud) not implemented. If SIM_MODE use cv::Mat version instead.");
 
            return pmPromise.get_future();
        }
 
 
        virtual sl::ERROR_CODE ResetPositionalTracking() = 0;
 
 
        virtual sl::ERROR_CODE TrackCustomBoxObjects(std::vector<ZedObjectData>& vCustomObjects)
        {
            // Initialize instance variables.
            (void) vCustomObjects;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::TrackCustomBoxObjects(const std::vector<ZedObjectData>& vCustomObjects) not implemented.");
 
            return sl::ERROR_CODE::FAILURE;
        }
 
 
        virtual sl::ERROR_CODE RebootCamera() = 0;
 
 
        virtual sl::FUSION_ERROR_CODE SubscribeFusionToCameraUUID(sl::CameraIdentifier& slCameraUUID) = 0;
 
 
        virtual sl::CameraIdentifier PublishCameraToFusion() = 0;
 
 
        virtual sl::FUSION_ERROR_CODE IngestGPSDataToFusion(geoops::GPSCoordinate stNewGPSLocation)
        {
            // Initialize instance variables.
            (void) stNewGPSLocation;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::IngestGPSDataToFusion(geoops::GPSCoordinate stNewGPSLocation) not implemented.");
 
            return sl::FUSION_ERROR_CODE::FAILURE;
        }
 
        // Setters for class member variables.
 
 
        virtual sl::ERROR_CODE EnablePositionalTracking(const float fExpectedCameraHeightFromFloorTolerance = constants::ZED_DEFAULT_FLOOR_PLANE_ERROR) = 0;
 
 
        virtual void DisablePositionalTracking() = 0;
 
 
        virtual void SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) = 0;
 
 
        virtual sl::ERROR_CODE EnableSpatialMapping()
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::EnableSpatialMapping() not implemented.");
 
            return sl::ERROR_CODE::FAILURE;
        }
 
 
        virtual void DisableSpatialMapping()
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::DisableSpatialMapping() not implemented.");
        }
 
 
        virtual sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false)
        {
            // Initialize instance variables.
            (void) bEnableBatching;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::EnableObjectDetection(const bool bEnableBatching = false) not implemented.");
 
            return sl::ERROR_CODE::FAILURE;
        }
 
 
        virtual void DisableObjectDetection()
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::DisableObjectDetection() not implemented.");
        }
 
        // Accessors for class member variables.
 
 
        virtual bool GetUsingGPUMem() const { return false; }
 
 
        virtual bool GetIsFusionMaster() const { return m_bCameraIsFusionMaster; }
 
 
        virtual std::string GetCameraModel() = 0;
 
 
        virtual unsigned int GetCameraSerial() { return m_unCameraSerialNumber; };
 
 
        virtual std::future<bool> RequestPositionalPoseCopy(Pose& stPose) = 0;
 
 
        virtual std::future<bool> RequestFusionGeoPoseCopy(sl::GeoPose& slGeoPose) = 0;
 
 
        virtual std::future<bool> RequestFloorPlaneCopy(sl::Plane& slFloorPlane)
        {
            // Initialize instance variables.
            (void) slFloorPlane;
            std::promise<bool> pmPromise;
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::RequestFloorPlaneCopy(sl::Plane& slFloorPlane) not implemented.");
 
            return pmPromise.get_future();
        }
 
 
        virtual bool GetPositionalTrackingEnabled() = 0;
 
 
        virtual sl::PositionalTrackingStatus GetPositionalTrackingState()
        {
            // Initialize instance variable.
            sl::PositionalTrackingStatus stStatus;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::GetPositionalTrackingState() not implemented.");
 
            return stStatus;
        }
 
 
        virtual sl::FusedPositionalTrackingStatus GetFusedPositionalTrackingState()
        {
            // Initialize instance variable.
            sl::FusedPositionalTrackingStatus stStatus;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::GetFusedPositionalTrackingState() not implemented.");
 
            return stStatus;
        }
 
 
        virtual sl::SPATIAL_MAPPING_STATE GetSpatialMappingState() { return sl::SPATIAL_MAPPING_STATE::NOT_ENABLED; }
 
 
        virtual sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future<sl::Mesh>& fuMeshFuture)
        {
            // Initialize instance variables.
            (void) fuMeshFuture;
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::ExtractSpatialMapAsync(std::future<sl::Mesh>& fuMeshFuture) not implemented.");
 
            return sl::SPATIAL_MAPPING_STATE::NOT_ENABLED;
        }
 
 
        virtual bool GetObjectDetectionEnabled() { return false; }
 
 
        virtual std::future<bool> RequestObjectsCopy(std::vector<sl::ObjectData>& vObjectData)
        {
            // Initialize instance variables.
            (void) vObjectData;
            std::promise<bool> pmPromise;
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::RequestObjectsCopy(std::vector<sl::ObjectData>& vObjectData) not implemented.");
 
            return pmPromise.get_future();
        }
 
 
        virtual std::future<bool> RequestBatchedObjectsCopy(std::vector<sl::ObjectsBatch>& vBatchedObjectData)
        {
            // Initialize instance variables.
            (void) vBatchedObjectData;
            std::promise<bool> pmPromise;
 
            // Immediately set the promise to false.
            pmPromise.set_value(false);
 
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "ZEDCamera::RequestBatchedObjectsCopy(std::vector<sl::ObjectsBatch>& vBatchedObjectData) not implemented.");
 
            return pmPromise.get_future();
        }
 
    protected:
        // Declare class constants.
 
        const std::memory_order ATOMIC_MEMORY_ORDER_METHOD = std::memory_order_relaxed;
 
        // Declare protected member variables.
 
        // ZED Camera specific.
        unsigned int m_unCameraSerialNumber;
        bool m_bCameraIsFusionMaster;
 
    private:
        // Declare private member variables.
};
 
#endif