Autonomy Software C++ 24.5.1
Welcome to the Autonomy Software repository of the Mars Rover Design Team (MRDT) at Missouri University of Science and Technology (Missouri S&T)! API reference contains the source code and other resources for the development of the autonomy software for our Mars rover. The Autonomy Software project aims to compete in the University Rover Challenge (URC) by demonstrating advanced autonomous capabilities and robust navigation algorithms.
Loading...
Searching...
No Matches
ZEDCam.h
Go to the documentation of this file.
1
11#ifndef ZEDCAM_H
12#define ZEDCAM_H
13
14#include "../../interfaces/ZEDCamera.hpp"
15
17
19
20
30class ZEDCam : public ZEDCamera
31{
32 public:
34 // Declare public methods and member variables.
36
37 ZEDCam(const int nPropResolutionX,
38 const int nPropResolutionY,
39 const int nPropFramesPerSecond,
40 const double dPropHorizontalFOV,
41 const double dPropVerticalFOV,
42 const bool bEnableRecordingFlag = false,
43 const bool bExportSVORecordingFlag = false,
44 const float fMinSenseDistance = constants::ZED_DEFAULT_MINIMUM_DISTANCE,
45 const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE,
46 const bool bMemTypeGPU = false,
47 const bool bUseHalfDepthPrecision = false,
48 const bool bEnableFusionMaster = false,
49 const int nNumFrameRetrievalThreads = 10,
50 const unsigned int unCameraSerialNumber = 0);
51 ~ZEDCam();
52 std::future<bool> RequestFrameCopy(cv::Mat& cvFrame) override;
53 std::future<bool> RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame) override;
54 std::future<bool> RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure = true) override;
55 std::future<bool> RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true) override;
56 std::future<bool> RequestPointCloudCopy(cv::Mat& cvPointCloud) override;
57 std::future<bool> RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud) override;
58 sl::ERROR_CODE ResetPositionalTracking() override;
59 sl::ERROR_CODE TrackCustomBoxObjects(std::vector<ZedObjectData>& vCustomObjects) override;
60 sl::ERROR_CODE RebootCamera() override;
61 sl::FUSION_ERROR_CODE SubscribeFusionToCameraUUID(sl::CameraIdentifier& slCameraUUID) override;
62 sl::CameraIdentifier PublishCameraToFusion() override;
63 sl::FUSION_ERROR_CODE IngestGPSDataToFusion(geoops::GPSCoordinate stNewGPSLocation) override;
64
66 // Setters for class member variables.
68
69 sl::ERROR_CODE EnablePositionalTracking(const float fExpectedCameraHeightFromFloorTolerance = constants::ZED_DEFAULT_FLOOR_PLANE_ERROR) override;
70 void DisablePositionalTracking() override;
71 void SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) override;
72 sl::ERROR_CODE EnableSpatialMapping() override;
73 void DisableSpatialMapping() override;
74 sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching = false) override;
75 void DisableObjectDetection() override;
76
78 // Getters.
80
81 bool GetCameraIsOpen() override;
82 bool GetUsingGPUMem() const override;
83 std::string GetCameraModel() override;
84 unsigned int GetCameraSerial() override;
85 std::future<bool> RequestPositionalPoseCopy(Pose& stPose) override;
86 std::future<bool> RequestFusionGeoPoseCopy(sl::GeoPose& slGeoPose) override;
87 std::future<bool> RequestFloorPlaneCopy(sl::Plane& slPlane) override;
88 bool GetPositionalTrackingEnabled() override;
89 sl::PositionalTrackingStatus GetPositionalTrackingState() override;
90 sl::FusedPositionalTrackingStatus GetFusedPositionalTrackingState() override;
91 sl::SPATIAL_MAPPING_STATE GetSpatialMappingState() override;
92 sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future<sl::Mesh>& fuMeshFuture) override;
93 bool GetObjectDetectionEnabled() override;
94 std::future<bool> RequestObjectsCopy(std::vector<sl::ObjectData>& vObjectData) override;
95 std::future<bool> RequestBatchedObjectsCopy(std::vector<sl::ObjectsBatch>& vBatchedObjectData) override;
96
97 private:
99 // Declare private member variables.
101
102 // ZED Camera specific.
103
104 sl::Camera m_slCamera;
105 std::shared_mutex m_muCameraMutex;
106 sl::InitParameters m_slCameraParams;
107 sl::RuntimeParameters m_slRuntimeParams;
108 sl::RecordingParameters m_slRecordingParams;
109 sl::Fusion m_slFusionInstance;
110 std::shared_mutex m_muFusionMutex;
111 sl::InitFusionParameters m_slFusionParams;
112 sl::MEASURE m_slDepthMeasureType;
113 sl::PositionalTrackingParameters m_slPoseTrackingParams;
114 sl::PositionalTrackingFusionParameters m_slFusionPoseTrackingParams;
115 sl::Pose m_slCameraPose;
116 sl::GeoPose m_slFusionGeoPose;
117 sl::Plane m_slFloorPlane;
118 sl::Transform m_slFloorTrackingTransform;
119 sl::SpatialMappingParameters m_slSpatialMappingParams;
120 sl::ObjectDetectionParameters m_slObjectDetectionParams;
121 sl::BatchParameters m_slObjectDetectionBatchParams;
122 sl::Objects m_slDetectedObjects;
123 std::vector<sl::ObjectsBatch> m_slDetectedObjectsBatched;
124 sl::MEM m_slMemoryType;
125 sl::MODEL m_slCameraModel;
126 float m_fExpectedCameraHeightFromFloorTolerance;
127
128 // Pose tracking offsets. (ZEDSDK is broken and can't handle large translations internally)
129
130 double m_dPoseOffsetX;
131 double m_dPoseOffsetY;
132 double m_dPoseOffsetZ;
133 double m_dPoseOffsetXO;
134 double m_dPoseOffsetYO;
135 double m_dPoseOffsetZO;
136
137 // Data from NavBoard.
138
139 geoops::GPSCoordinate m_stCurrentGPSBasedPosition;
140
141 // Mats for storing frames and measures.
142
143 sl::Mat m_slFrame;
144 sl::Mat m_slDepthImage;
145 sl::Mat m_slDepthMeasure;
146 sl::Mat m_slPointCloud;
147
148 // Queues and mutexes for scheduling and copying camera frames and data to other threads.
149
150 std::queue<containers::FrameFetchContainer<cv::cuda::GpuMat>> m_qGPUFrameCopySchedule;
151 std::queue<containers::DataFetchContainer<std::vector<ZedObjectData>>> m_qCustomBoxIngestSchedule;
152 std::queue<containers::DataFetchContainer<Pose>> m_qPoseCopySchedule;
153 std::queue<containers::DataFetchContainer<sl::GeoPose>> m_qGeoPoseCopySchedule;
154 std::queue<containers::DataFetchContainer<sl::Plane>> m_qFloorCopySchedule;
155 std::queue<containers::DataFetchContainer<std::vector<sl::ObjectData>>> m_qObjectDataCopySchedule;
156 std::queue<containers::DataFetchContainer<std::vector<sl::ObjectsBatch>>> m_qObjectBatchedDataCopySchedule;
157
158 // Mutexes for copying frames from the ZEDSDK to the OpenCV Mats.
159
160 std::shared_mutex m_muCustomBoxIngestMutex;
161 std::shared_mutex m_muPoseCopyMutex;
162 std::shared_mutex m_muGeoPoseCopyMutex;
163 std::shared_mutex m_muFloorCopyMutex;
164 std::shared_mutex m_muObjectDataCopyMutex;
165 std::shared_mutex m_muObjectBatchedDataCopyMutex;
166
167 // Atomic flags for checking if data is queued.
168
169 std::atomic<bool> m_bNormalFramesQueued;
170 std::atomic<bool> m_bDepthFramesQueued;
171 std::atomic<bool> m_bPointCloudsQueued;
172 std::atomic<bool> m_bPosesQueued;
173 std::atomic<bool> m_bGeoPosesQueued;
174 std::atomic<bool> m_bFloorsQueued;
175 std::atomic<bool> m_bObjectsQueued;
176 std::atomic<bool> m_bBatchedObjectsQueued;
177
179 // Declare private methods.
181
182 void ThreadedContinuousCode() override;
183 void PooledLinearCode() override;
184};
185#endif
This class implements and interfaces with the most common ZEDSDK cameras and features....
Definition ZEDCam.h:31
std::future< bool > RequestDepthCopy(cv::Mat &cvDepth, const bool bRetrieveMeasure=true) override
Requests a depth measure or image from the camera. Puts a frame pointer into a queue so a copy of a f...
Definition ZEDCam.cpp:1026
void DisableSpatialMapping() override
Disabled the spatial mapping feature of the camera.
Definition ZEDCam.cpp:1758
void DisablePositionalTracking() override
Disable to positional tracking functionality of the camera.
Definition ZEDCam.cpp:1624
sl::FUSION_ERROR_CODE SubscribeFusionToCameraUUID(sl::CameraIdentifier &slCameraUUID) override
Give a UUID for another ZEDCam, subscribe that camera to this camera's Fusion instance....
Definition ZEDCam.cpp:1310
~ZEDCam()
Destroy the Zed Cam:: Zed Cam object.
Definition ZEDCam.cpp:255
sl::ERROR_CODE TrackCustomBoxObjects(std::vector< ZedObjectData > &vCustomObjects) override
A vector containing CustomBoxObjectData objects. These objects simply store information about your de...
Definition ZEDCam.cpp:1233
std::future< bool > RequestFusionGeoPoseCopy(sl::GeoPose &slGeoPose) override
Requests the current geo pose of the camera. This method should be used to retrieve ONLY the GNSS dat...
Definition ZEDCam.cpp:1970
sl::ERROR_CODE RebootCamera() override
Performs a hardware reset of the ZED2 or ZED2i camera.
Definition ZEDCam.cpp:1292
void ThreadedContinuousCode() override
The code inside this private method runs in a separate thread, but still has access to this*....
Definition ZEDCam.cpp:287
sl::ERROR_CODE EnablePositionalTracking(const float fExpectedCameraHeightFromFloorTolerance=constants::ZED_DEFAULT_FLOOR_PLANE_ERROR) override
Enable the positional tracking functionality of the camera.
Definition ZEDCam.cpp:1569
sl::FUSION_ERROR_CODE IngestGPSDataToFusion(geoops::GPSCoordinate stNewGPSLocation) override
If this camera is the fusion instance master, this method can be used to ingest/process/fuse the curr...
Definition ZEDCam.cpp:1389
sl::ERROR_CODE EnableSpatialMapping() override
Enabled the spatial mapping feature of the camera. Pose tracking will be enabled if it is not already...
Definition ZEDCam.cpp:1692
bool GetPositionalTrackingEnabled() override
Accessor for if the positional tracking functionality of the camera has been enabled and functioning.
Definition ZEDCam.cpp:2087
sl::ERROR_CODE EnableObjectDetection(const bool bEnableBatching=false) override
Enables the object detection and tracking feature of the camera.
Definition ZEDCam.cpp:1774
std::future< bool > RequestPositionalPoseCopy(Pose &stPose) override
Requests the current pose of the camera relative to it's start pose or the origin of the set pose....
Definition ZEDCam.cpp:1908
unsigned int GetCameraSerial() override
Accessor for the camera's serial number.
Definition ZEDCam.cpp:1887
sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future< sl::Mesh > &fuMeshFuture) override
Retrieve the built spatial map from the camera. Spatial mapping must be enabled. This method takes in...
Definition ZEDCam.cpp:2155
std::future< bool > RequestObjectsCopy(std::vector< sl::ObjectData > &vObjectData) override
Requests a current copy of the tracked objects from the camera. Puts a pointer to a vector containing...
Definition ZEDCam.cpp:2240
void SetPositionalPose(const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) override
Sets the pose of the positional tracking of the camera. XYZ will point in their respective directions...
Definition ZEDCam.cpp:1668
sl::FusedPositionalTrackingStatus GetFusedPositionalTrackingState() override
Accessor for the current positional tracking status of the fusion instance.
Definition ZEDCam.cpp:2119
std::future< bool > RequestBatchedObjectsCopy(std::vector< sl::ObjectsBatch > &vBatchedObjectData) override
If batching is enabled, this requests the normal objects and passes them to the the internal batching...
Definition ZEDCam.cpp:2302
bool GetUsingGPUMem() const override
Accessor for if this ZED is storing it's frames in GPU memory.
Definition ZEDCam.cpp:1843
sl::SPATIAL_MAPPING_STATE GetSpatialMappingState() override
Accessor for the current state of the camera's spatial mapping feature.
Definition ZEDCam.cpp:2134
std::future< bool > RequestPointCloudCopy(cv::Mat &cvPointCloud) override
Requests a point cloud image from the camera. This image has the same resolution as a normal image bu...
Definition ZEDCam.cpp:1117
std::future< bool > RequestFrameCopy(cv::Mat &cvFrame) override
Requests a regular BGRA image from the LEFT eye of the zed camera. Puts a frame pointer into a queue ...
Definition ZEDCam.cpp:953
sl::PositionalTrackingStatus GetPositionalTrackingState() override
Accessor for the current positional tracking status of the camera.
Definition ZEDCam.cpp:2103
bool GetCameraIsOpen() override
Accessor for the current status of the camera.
Definition ZEDCam.cpp:1827
sl::CameraIdentifier PublishCameraToFusion() override
Signal this camera to make its data available to the Fusion module and retrieve a UUID for this class...
Definition ZEDCam.cpp:1365
void PooledLinearCode() override
This method holds the code that is ran in the thread pool started by the ThreadedLinearCode() method....
Definition ZEDCam.cpp:695
std::future< bool > RequestFloorPlaneCopy(sl::Plane &slPlane) override
Requests the current floor plane of the camera relative to it's current pose. Puts a Plane pointer in...
Definition ZEDCam.cpp:2030
void DisableObjectDetection() override
Disables the object detection and tracking feature of the camera.
Definition ZEDCam.cpp:1810
bool GetObjectDetectionEnabled() override
Accessor for if the cameras object detection and tracking feature is enabled.
Definition ZEDCam.cpp:2222
sl::ERROR_CODE ResetPositionalTracking() override
Resets the cameras X,Y,Z translation and Roll,Pitch,Yaw orientation back to 0. THINK CAREFULLY!...
Definition ZEDCam.cpp:1193
std::string GetCameraModel() override
Accessor for the model enum from the ZEDSDK and represents the camera model as a string.
Definition ZEDCam.cpp:1858
This class serves as a middle inheritor between the Camera interface and the ZEDCam class....
Definition ZEDCamera.hpp:33
This struct is used within the ZEDCam class to store the camera pose with high precision....
Definition ZEDCamera.hpp:77
This struct stores/contains information about a GPS data.
Definition GeospatialOperations.hpp:99