ObjectDetector Class Reference

This class implements a modular and easy to use object detector for a single camera. Given a camera name, this class will detect objects using the depth measure from a ZED camera and/or inferenced objects from a custom trained model. This class and it's detections are ran in a different thread. More...

#include <ObjectDetector.h>

Inheritance diagram for ObjectDetector:

Collaboration diagram for ObjectDetector:

Public Member Functions
	ObjectDetector (std::shared_ptr< BasicCamera > pBasicCam, const bool bEnableTracking=false, const int nDetectorMaxFPS=30, const bool bEnableRecordingFlag=false, const int nNumDetectedObjectsRetrievalThreads=5, const bool bUsingGpuMats=false)
	Construct a new Object Detector:: Object Detector object.
	ObjectDetector (std::shared_ptr< ZEDCamera > pZEDCam, const bool bEnableTracking=false, const int nDetectorMaxFPS=30, const bool bEnableRecordingFlag=false, const int nNumDetectedObjectsRetrievalThreads=5, const bool bUsingGpuMats=false)
	Construct a new Object Detector:: Object Detector object.
	~ObjectDetector ()
	Destroy the Object Detector:: Object Detector object.
std::future< bool >	RequestDetectionOverlayFrame (cv::Mat &cvFrame)
	Request a copy of the frame containing the detected objects from all detection methods drawn onto the frame.
std::future< bool >	RequestLastGoodDetectionOverlayFrame (cv::Mat &cvFrame)
	Request a copy of the frame containing the last known good detected objects from all detection methods drawn onto the frame.
std::future< bool >	RequestDetectedObjects (std::vector< objectdetectutils::Object > &vObjects)
	Request a copy of the most update to date vector of the detected objects from all detection methods.
bool	InitTorchDetection (const std::string &szModelPath, yolomodel::pytorch::PyTorchInterpreter::HardwareDevices eDevice=yolomodel::pytorch::PyTorchInterpreter::HardwareDevices::eCUDA)
	Initialize the PyTorch interpreter for object detection.
void	EnableTorchDetection (const float fMinObjectConfidence=0.4f, const float fNMSThreshold=0.6f)
	Enable the PyTorch detection method for this ObjectDetector.
void	DisableTorchDetection ()
	Set the flag to enable or disable object detection with the torch model.
void	SetDetectorMaxFPS (const int nRecordingFPS)
	Set the max FPS of the detector.
void	SetEnableRecordingFlag (const bool bEnableRecordingFlag)
	Set the flag to enable or disable recording of the overlay output.
bool	GetIsReady ()
	Check if the ObjectDetector is ready to be used.
int	GetDetectorMaxFPS () const
	Get the max FPS of the detector.
bool	GetEnableRecordingFlag () const
	Get the flag to enable or disable recording of the overlay output.
std::string	GetCameraName ()
	Get the camera name.
cv::Size	GetProcessFrameResolution () const
	Get the process frame resolution.
Public Member Functions inherited from AutonomyThread< void >
	AutonomyThread ()
	Construct a new Autonomy Thread object.
virtual	~AutonomyThread ()
	Destroy the Autonomy Thread object. If the parent object or main thread is destroyed or exited while this thread is still running, a race condition will occur. Stopping and joining the thread here insures that the main program can't exit if the user forgot to stop and join the thread.
void	Start ()
	When this method is called, it starts a new thread that runs the code within the ThreadedContinuousCode method. This is the users main code that will run the important and continuous code for the class.
void	RequestStop ()
	Signals threads to stop executing user code, terminate. DOES NOT JOIN. This method will not force the thread to exit, if the user code is not written properly and contains WHILE statement or any other long-executing or blocking code, then the thread will not exit until the next iteration.
void	Join ()
	Waits for thread to finish executing and then closes thread. This method will block the calling code until thread is finished.
bool	Joinable () const
	Check if the code within the thread and all pools created by it are finished executing and the thread is ready to be closed.
AutonomyThreadState	GetThreadState () const
	Accessor for the Threads State private member.
std::string	GetThreadUUID () const
	Accessor for the Thread U U I D private member.
IPS &	GetIPS ()
	Accessor for the Frame I P S private member.
void	SetMainThreadPriority (AutonomyThreadPriority ePriority)
	Set the OS priority for the main continuous thread.
void	SetPoolThreadPriority (AutonomyThreadPriority ePriority)
	Set the OS priority for the highly parallelized pool threads.

Private Member Functions
void	ThreadedContinuousCode () override
	This method will run continuously in a separate thread. New frames from the given camera are grabbed and the objects for the camera image are detected using the PyTorch interpreter. The detected objects are then filtered and stored. Then any requests for the current objects are fulfilled via a call and join of the thread pooled code.
void	PooledLinearCode () override
	This method will run in a thread pool. It will be called by the main thread and will run the code within the PooledLinearCode() method. This is meant to be used as an internal utility of the child class to further improve parallelization.
void	UpdateDetectedObjects (std::vector< objectdetectutils::Object > &vNewlyDetectedObjects)
	Update the detected objects with the newly detected objects.

Private Attributes
std::shared_ptr< Camera< cv::Mat > >	m_pCamera
std::shared_ptr< yolomodel::pytorch::PyTorchInterpreter >	m_pTorchDetector
std::atomic< float >	m_fTorchMinObjectConfidence
std::atomic< float >	m_fTorchNMSThreshold
std::atomic_bool	m_bTorchInitialized
std::atomic_bool	m_bTorchEnabled
std::shared_ptr< tracking::MultiTracker >	m_pMultiTracker
bool	m_bUsingZedCamera
bool	m_bUsingGpuMats
bool	m_bCameraIsOpened
bool	m_bEnableTracking
int	m_nNumDetectedObjectsRetrievalThreads
std::string	m_szCameraName
std::atomic_bool	m_bEnableRecordingFlag
std::vector< objectdetectutils::Object >	m_vNewlyDetectedObjects
std::vector< objectdetectutils::Object >	m_vDetectedObjects
geoops::RoverPose	m_stRoverPose
cv::Mat	m_cvFrame
cv::cuda::GpuMat	m_cvGPUFrame
cv::Mat	m_cvLastGoodOverlayFrame
cv::Mat	m_cvDetectionOverlayFrame
cv::Mat	m_cvTorchProcFrame
cv::Mat	m_cvPointCloud
cv::cuda::GpuMat	m_cvGPUPointCloud
std::queue< containers::FrameFetchContainer< cv::Mat > >	m_qDetectionOverlayFramesCopySchedule
std::queue< containers::FrameFetchContainer< cv::Mat > >	m_qLastGoodDetectionOverlayFramesCopySchedule
std::queue< containers::DataFetchContainer< std::vector< objectdetectutils::Object > > >	m_qDetectedObjectCopySchedule
std::shared_mutex	m_muPoolScheduleMutex
std::shared_mutex	m_muDetectionOverlayCopyMutex
std::shared_mutex	m_muLastGoodDetectionOverlayCopyMutex
std::shared_mutex	m_muArucoDataCopyMutex

Additional Inherited Members
Public Types inherited from AutonomyThread< void >
enum	AutonomyThreadState
enum	AutonomyThreadPriority
Protected Member Functions inherited from AutonomyThread< void >
void	RunPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts/adds tasks to a thread pool that runs nNumTasksToQueue copies of the code within the PooledLinearCode() method using nNumThreads number of threads. This is meant to be used as an internal utility of the child class to further improve parallelization. Default value for nNumThreads is 2.
void	RunDetachedPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts a thread pool full of threads that don't return std::futures (like a placeholder for the thread return type). This means the thread will not have a return type and there is no way to determine if the thread has finished other than calling the Join() method. Only use this if you want to 'set and forget'. It will be faster as it doesn't return futures. Runs PooledLinearCode() method code. This is meant to be used as an internal utility of the child class to further improve parallelization.
void	ParallelizeLoop (const int nNumThreads, const N tTotalIterations, F &&tLoopFunction)
	Given a ref-qualified looping function and an arbitrary number of iterations, this method will divide up the loop and run each section in a thread pool. This function must not return anything. This method will block until the loop has completed.
void	ClearPoolQueue ()
	Clears any tasks waiting to be ran in the queue, tasks currently running will remain running.
void	JoinPool ()
	Waits for pool to finish executing tasks. This method will block the calling code until thread is finished.
bool	PoolJoinable () const
	Check if the internal pool threads are done executing code and the queue is empty.
void	SetMainThreadIPSLimit (int nMaxIterationsPerSecond=0)
	Mutator for the Main Thread Max I P S private member.
int	GetPoolNumOfThreads ()
	Accessor for the Pool Num Of Threads private member.
int	GetPoolQueueLength ()
	Accessor for the Pool Queue Size private member.
std::vector< void >	GetPoolResults ()
	Accessor for the Pool Results private member. The action of getting results will destroy and remove them from this object. This method blocks if the thread is not finished, so no need to call JoinPool() before getting results.
int	GetMainThreadMaxIPS () const
	Accessor for the Main Thread Max I P S private member.
Protected Attributes inherited from AutonomyThread< void >
IPS	m_IPS

Detailed Description

This class implements a modular and easy to use object detector for a single camera. Given a camera name, this class will detect objects using the depth measure from a ZED camera and/or inferenced objects from a custom trained model. This class and it's detections are ran in a different thread.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-10-24

Constructor & Destructor Documentation

ObjectDetector() [1/2]

ObjectDetector::ObjectDetector	(	std::shared_ptr< BasicCamera >	pBasicCam,
		const bool	bEnableTracking = false,
		const int	nDetectorMaxFPS = 30,
		const bool	bEnableRecordingFlag = false,
		const int	nNumDetectedObjectsRetrievalThreads = 5,
		const bool	bUsingGpuMats = false
	)

Construct a new Object Detector:: Object Detector object.

Parameters

pBasicCam	- A pointer to the BasicCam to use for detection.
bEnableTracking	- Whether or not to enable tracking of detected objects.
nDetectorMaxFPS	- The max FPS limit the detector can run at.
bEnableRecordingFlag	- Whether or not this ObjectDetector's overlay output should be recorded.
nNumDetectedObjectsRetrievalThreads	- The number of threads to use when fulfilling requests for the detected objects. Default is 5.
bUsingGpuMats	- Whether or not the given camera name will be using GpuMats.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Initialize member variables.
    m_pCamera                             = pBasicCam;
    m_bEnableTracking                     = bEnableTracking;
    m_bUsingZedCamera                     = false;    // Toggle ZED functions off.
    m_bEnableRecordingFlag                = bEnableRecordingFlag;
    m_nNumDetectedObjectsRetrievalThreads = nNumDetectedObjectsRetrievalThreads;
    m_bUsingGpuMats                       = bUsingGpuMats;
    m_bTorchInitialized                   = false;
    m_bTorchEnabled                       = false;
    m_bCameraIsOpened                     = false;
    m_szCameraName                        = pBasicCam->GetCameraLocation();
    m_stRoverPose                         = geoops::RoverPose();
 
    // Create a multi-tracker for tracking multiple objects from the torch detectors.
    m_pMultiTracker = std::make_shared<tracking::MultiTracker>(constants::BBOX_TRACKER_LOST_TIMEOUT,
                                                               constants::BBOX_TRACKER_MAX_TRACK_TIME,
                                                               constants::BBOX_TRACKER_IOU_MATCH_THRESHOLD);
 
    // Set max IPS of main thread.
    this->SetMainThreadIPSLimit(nDetectorMaxFPS);
 
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "ObjectDetector created for camera at path/index: {}", m_szCameraName);
}

Here is the call graph for this function:

ObjectDetector() [2/2]

ObjectDetector::ObjectDetector	(	std::shared_ptr< ZEDCamera >	pZEDCam,
		const bool	bEnableTracking = false,
		const int	nDetectorMaxFPS = 30,
		const bool	bEnableRecordingFlag = false,
		const int	nNumDetectedObjectsRetrievalThreads = 5,
		const bool	bUsingGpuMats = false
	)

Construct a new Object Detector:: Object Detector object.

Parameters

pZEDCam	- A pointer to the ZEDCamera to use for detection.
bEnableTracking	- Whether or not to enable tracking of detected objects.
nDetectorMaxFPS	- The max FPS limit the detector can run at.
bEnableRecordingFlag	- Whether or not this ObjectDetector's overlay output should be recorded.
nNumDetectedObjectsRetrievalThreads	- The number of threads to use when fulfilling requests for the detected objects. Default is 5.
bUsingGpuMats	- Whether or not the given camera name will be using GpuMats.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Initialize member variables.
    m_pCamera                             = pZEDCam;
    m_bEnableTracking                     = bEnableTracking;
    m_bUsingZedCamera                     = true;    // Toggle ZED functions on.
    m_bEnableRecordingFlag                = bEnableRecordingFlag;
    m_nNumDetectedObjectsRetrievalThreads = nNumDetectedObjectsRetrievalThreads;
    m_bUsingGpuMats                       = bUsingGpuMats;
    m_bTorchInitialized                   = false;
    m_bTorchEnabled                       = false;
    m_bCameraIsOpened                     = false;
    m_szCameraName                        = pZEDCam->GetCameraModel() + "_" + std::to_string(pZEDCam->GetCameraSerial());
    m_stRoverPose                         = geoops::RoverPose();
 
    // Create a multi-tracker for tracking multiple objects from the torch detectors.
    m_pMultiTracker = std::make_shared<tracking::MultiTracker>(constants::BBOX_TRACKER_LOST_TIMEOUT,
                                                               constants::BBOX_TRACKER_IOU_MATCH_THRESHOLD,
                                                               constants::BBOX_TRACKER_IOU_MATCH_THRESHOLD);
 
    // Set max IPS of main thread.
    this->SetMainThreadIPSLimit(nDetectorMaxFPS);
 
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "ObjectDetector created for camera: {}", m_szCameraName);
}

Here is the call graph for this function:

~ObjectDetector()

ObjectDetector::~ObjectDetector

(

)

Destroy the Object Detector:: Object Detector object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Stop threaded code.
    this->RequestStop();
    this->Join();
 
    // Submit logger message.
    LOG_INFO(logging::g_qSharedLogger, "ObjectDetector for camera {} has been destroyed.", this->GetCameraName());
}

Here is the call graph for this function:

Member Function Documentation

RequestDetectionOverlayFrame()

std::future< bool > ObjectDetector::RequestDetectionOverlayFrame

(

cv::Mat &

cvFrame

)

Request a copy of the frame containing the detected objects from all detection methods drawn onto the frame.

Parameters

cvFrame

- The cv::Mat frame to copy the detection overlay image to.

Returns

std::future<bool> - The future that will be set to true when the frame is copied.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Assemble the DataFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvFrame, PIXEL_FORMATS::eObjectDetection);
 
    // Acquire lock on pool copy queue.
    std::unique_lock<std::shared_mutex> lkScheduler(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qDetectionOverlayFramesCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkScheduler.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedFrameStatus->get_future();
}

RequestLastGoodDetectionOverlayFrame()

std::future< bool > ObjectDetector::RequestLastGoodDetectionOverlayFrame

(

cv::Mat &

cvFrame

)

Request a copy of the frame containing the last known good detected objects from all detection methods drawn onto the frame.

Parameters

cvFrame

- The cv::Mat frame to copy the detection overlay image to.

Returns

std::future<bool> - The future that will be set to true when the frame is copied.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Assemble the DataFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvFrame, PIXEL_FORMATS::eObjectDetection);
 
    // Acquire lock on pool copy queue.
    std::unique_lock<std::shared_mutex> lkScheduler(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qLastGoodDetectionOverlayFramesCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkScheduler.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedFrameStatus->get_future();
}

RequestDetectedObjects()

std::future< bool > ObjectDetector::RequestDetectedObjects

(

std::vector< objectdetectutils::Object > &

vObjects

)

Request a copy of the most update to date vector of the detected objects from all detection methods.

Parameters

vObjects

- The vector of detected objects to copy the detected objects to.

Returns

std::future<bool> - The future that will be set to true when the objects are copied.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Assemble the DataFetchContainer.
    containers::DataFetchContainer<std::vector<objectdetectutils::Object>> stContainer(vObjects);
 
    // Acquire lock on pool copy queue.
    std::unique_lock<std::shared_mutex> lkScheduler(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qDetectedObjectCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkScheduler.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedDataStatus->get_future();
}

InitTorchDetection()

bool ObjectDetector::InitTorchDetection	(	const std::string &	szModelPath,
		yolomodel::pytorch::PyTorchInterpreter::HardwareDevices	eDevice = yolomodel::pytorch::PyTorchInterpreter::HardwareDevices::eCUDA
	)

Initialize the PyTorch interpreter for object detection.

Parameters

szModelPath	- The path to the PyTorch model file.
eDevice	- The hardware device to use for inference (e.g., CPU or GPU).

Returns

true - Model was opened and loaded successfully onto the torch device.

false - Model was not opened and loaded successfully onto the torch device.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Initialize a new YOLOModel object.
    m_pTorchDetector = std::make_shared<yolomodel::pytorch::PyTorchInterpreter>(szModelPath, eDevice);
 
    // Check if device/model was opened without issue.
    if (m_pTorchDetector->IsReadyForInference())
    {
        // Update member variable.
        m_bTorchInitialized = true;
        // Return status.
        return true;
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger, "Unable to initialize Torch detection for ObjectDetector.");
        // Update member variable.
        m_bTorchInitialized = false;
        // Return status.
        return false;
    }
}

EnableTorchDetection()

void ObjectDetector::EnableTorchDetection	(	const float	fMinObjectConfidence = 0.4f,
		const float	fNMSThreshold = 0.6f
	)

Enable the PyTorch detection method for this ObjectDetector.

Parameters

fMinObjectConfidence	- The minimum confidence threshold for detected objects.
fNMSThreshold	- The non-maximum suppression threshold for filtering overlapping detections.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Update member variables.
    m_fTorchMinObjectConfidence = fMinObjectConfidence;
    m_fTorchNMSThreshold        = fNMSThreshold;
 
    // Check if torch model has been initialized.
    if (m_bTorchInitialized)
    {
        // Update member variable.
        m_bTorchEnabled = true;
    }
    else
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Tried to enable torch detection for ObjectDetector but it has not been initialized yet!");
        // Update member variable.
        m_bTorchEnabled = false;
    }
}

DisableTorchDetection()

void ObjectDetector::DisableTorchDetection

(

)

Set the flag to enable or disable object detection with the torch model.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Update member variable.
    m_bTorchEnabled = false;
}

SetDetectorMaxFPS()

void ObjectDetector::SetDetectorMaxFPS

(

const int

nRecordingFPS

)

Set the max FPS of the detector.

Parameters

nRecordingFPS

- The max FPS of the detector.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Set the max iterations per second of the main thread.
    this->SetMainThreadIPSLimit(nRecordingFPS);
}

Here is the call graph for this function:

SetEnableRecordingFlag()

void ObjectDetector::SetEnableRecordingFlag

(

const bool

bEnableRecordingFlag

)

Set the flag to enable or disable recording of the overlay output.

Parameters

bEnableRecordingFlag

- The flag to enable or disable recording of the overlay output.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Update member variable.
    m_bEnableRecordingFlag = bEnableRecordingFlag;
}

GetIsReady()

bool ObjectDetector::GetIsReady

(

)

Check if the ObjectDetector is ready to be used.

Returns

true - The ObjectDetector is ready to be used.

false - The ObjectDetector is not ready to be used.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Create instance variables.
    bool bDetectorIsReady = false;
 
    // Check if this detectors thread is currently running.
    if (this->GetThreadState() == AutonomyThreadState::eRunning)
    {
        // Check if using ZEDCam or BasicCam.
        if (m_bUsingZedCamera)
        {
            // Check if camera is NOT open.
            if (std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->GetCameraIsOpen())
            {
                // Set camera opened toggle.
                bDetectorIsReady = true;
            }
        }
        else
        {
            // Check if camera is NOT open.
            if (std::dynamic_pointer_cast<BasicCamera>(m_pCamera)->GetCameraIsOpen())
            {
                // Set camera opened toggle.
                bDetectorIsReady = true;
            }
        }
    }
 
    // Return if this detector is ready or not.
    return bDetectorIsReady;
}

Here is the call graph for this function:

GetDetectorMaxFPS()

int ObjectDetector::GetDetectorMaxFPS

(

)

const

Get the max FPS of the detector.

Returns

int - The max FPS of the detector.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Return the max FPS of the detector.
    return this->GetMainThreadMaxIPS();
}

Here is the call graph for this function:

GetEnableRecordingFlag()

bool ObjectDetector::GetEnableRecordingFlag

(

)

const

Get the flag to enable or disable recording of the overlay output.

Returns

true - The flag to enable or disable recording of the overlay output.

false - The flag to enable or disable recording of the overlay output.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Return the enable recording flag.
    return m_bEnableRecordingFlag;
}

GetCameraName()

std::string ObjectDetector::GetCameraName

(

)

Get the camera name.

Returns

std::string - The camera name.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Return the camera name.
    return m_szCameraName;
}

Here is the caller graph for this function:

GetProcessFrameResolution()

cv::Size ObjectDetector::GetProcessFrameResolution

(

)

const

Get the process frame resolution.

Returns

cv::Size - The process frame resolution.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Check if using a ZED camera.
    if (m_bUsingZedCamera)
    {
        // Concatenate camera model name and serial number.
        return std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->GetPropResolution();
    }
    else
    {
        // Concatenate camera path or index.
        return std::dynamic_pointer_cast<BasicCamera>(m_pCamera)->GetPropResolution();
    }
}

ThreadedContinuousCode()

void ObjectDetector::ThreadedContinuousCode ( )

overrideprivatevirtual

This method will run continuously in a separate thread. New frames from the given camera are grabbed and the objects for the camera image are detected using the PyTorch interpreter. The detected objects are then filtered and stored. Then any requests for the current objects are fulfilled via a call and join of the thread pooled code.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

Implements AutonomyThread< void >.

{
    // Check if using ZEDCam or BasicCam.
    if (m_bUsingZedCamera)
    {
        // Check if camera is NOT open.
        if (!std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->GetCameraIsOpen())
        {
            // Set camera opened toggle.
            m_bCameraIsOpened = false;
 
            // If camera's not open on first iteration of thread, it's probably not present, so stop.
            if (this->GetThreadState() == AutonomyThreadState::eStarting)
            {
                // Shutdown threads for this ZEDCam.
                this->RequestStop();
 
                // Submit logger message.
                LOG_CRITICAL(logging::g_qSharedLogger,
                             "ObjectDetector start was attempted for ZED camera with serial number {}, but camera never properly opened or it has been closed/rebooted! "
                             "This object detector will now stop.",
                             std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->GetCameraSerial());
            }
        }
        else
        {
            // Set camera opened toggle.
            m_bCameraIsOpened = true;
        }
    }
    else
    {
        // Check if camera is NOT open.
        if (!std::dynamic_pointer_cast<BasicCamera>(m_pCamera)->GetCameraIsOpen())
        {
            // Set camera opened toggle.
            m_bCameraIsOpened = false;
 
            // If camera's not open on first iteration of thread, it's probably not present, so stop.
            if (this->GetThreadState() == AutonomyThreadState::eStarting)
            {
                // Shutdown threads for this BasicCam.
                this->RequestStop();
 
                // Submit logger message.
                LOG_CRITICAL(logging::g_qSharedLogger,
                             "ObjectDetector start was attempted for BasicCam at {}, but camera never properly opened or it has become disconnected!",
                             std::dynamic_pointer_cast<BasicCamera>(m_pCamera)->GetCameraLocation());
            }
        }
        else
        {
            // Set camera opened toggle.
            m_bCameraIsOpened = true;
        }
    }
 
    // Check if camera is opened.
    if (m_bCameraIsOpened)
    {
        // Create future for indicating when the frame has been copied.
        std::future<bool> fuPointCloudCopyStatus;
        std::future<bool> fuRegularFrameCopyStatus;
        bool bRequestingPointCloud = false;
 
        // Check if the camera is setup to use CPU or GPU mats.
        if (m_bUsingZedCamera)
        {
            bRequestingPointCloud = true;
            // Check if the ZED camera is returning cv::cuda::GpuMat or cv:Mat.
            if (m_bUsingGpuMats)
            {
                // Grabs point cloud from ZEDCam. Dynamic casts Camera to ZEDCamera* so we can use ZEDCam methods.
                fuPointCloudCopyStatus = std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->RequestPointCloudCopy(m_cvGPUPointCloud);
                // Get the regular RGB image from the camera.
                fuRegularFrameCopyStatus = std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->RequestFrameCopy(m_cvGPUFrame);
            }
            else
            {
                // Grabs point cloud from ZEDCam.
                fuPointCloudCopyStatus   = std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->RequestPointCloudCopy(m_cvPointCloud);
                fuRegularFrameCopyStatus = std::dynamic_pointer_cast<ZEDCamera>(m_pCamera)->RequestFrameCopy(m_cvFrame);
            }
        }
        else
        {
            // Grab frames from camera.
            fuRegularFrameCopyStatus = std::dynamic_pointer_cast<BasicCamera>(m_pCamera)->RequestFrameCopy(m_cvFrame);
        }
 
        // Safe polling wrapper to prevent deadlocks.
        bool bCloudReady = !bRequestingPointCloud;    // True by default if we don't need a point cloud
        bool bFrameReady = false;
 
        // Keep polling as long as the thread hasn't been asked to stop.
        while (this->GetThreadState() == AutonomyThreadState::eRunning)
        {
            if (!bCloudReady && fuPointCloudCopyStatus.wait_for(std::chrono::milliseconds(10)) == std::future_status::ready)
                bCloudReady = true;
 
            if (!bFrameReady && fuRegularFrameCopyStatus.wait_for(std::chrono::milliseconds(10)) == std::future_status::ready)
                bFrameReady = true;
 
            if (bCloudReady && bFrameReady)
                break;
        }
 
        // If the thread is shutting down, break out of the loop gracefully
        if (this->GetThreadState() != AutonomyThreadState::eRunning)
        {
            return;
        }
 
        // Process the retrieved frames
        if (m_bUsingZedCamera)
        {
            if (m_bUsingGpuMats)
            {
                if (fuPointCloudCopyStatus.get() && fuRegularFrameCopyStatus.get())
                {
                    // Download mat from GPU memory.
                    m_cvGPUPointCloud.download(m_cvPointCloud);
                    m_cvGPUFrame.download(m_cvFrame);
                    // Drop alpha channel.
                    cv::cvtColor(m_cvFrame, m_cvFrame, cv::COLOR_BGRA2BGR);
                }
                else
                {
                    LOG_WARNING(logging::g_qSharedLogger, "ObjectDetector unable to get point cloud or frame from ZEDCam!");
                }
            }
            else
            {
                if (!fuPointCloudCopyStatus.get())
                    LOG_WARNING(logging::g_qSharedLogger, "ObjectDetector unable to get point cloud from ZEDCam!");
                if (!fuRegularFrameCopyStatus.get())
                    LOG_WARNING(logging::g_qSharedLogger, "ObjectDetector unable to get regular frame from ZEDCam!");
            }
        }
        else
        {
            if (!fuRegularFrameCopyStatus.get())
            {
                LOG_WARNING(logging::g_qSharedLogger, "ObjectDetector unable to get RGB image from BasicCam!");
            }
        }
 
        // Actual detection logic goes here.
        // Check if the frame is empty.
        if (m_cvFrame.empty())
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Frame from camera is empty!");
            return;
        }
 
        // Clear the list of newly detected objects.
        m_vNewlyDetectedObjects.clear();
        // Clone frames.
        m_cvDetectionOverlayFrame = m_cvFrame.clone();
        m_cvTorchProcFrame        = m_cvFrame.clone();
        // Copy the camera frame to the pre-processing frame and overlay frame.
        cv::cvtColor(m_cvTorchProcFrame, m_cvTorchProcFrame, cv::COLOR_BGR2RGB);
 
        // Check if torch detection if turned on.
        if (m_bTorchEnabled)
        {
            // Detect objects in the image.
            std::vector<objectdetectutils::Object> vNewTorchObjects =
                torchobject::Detect(m_cvTorchProcFrame, *m_pTorchDetector, m_fTorchMinObjectConfidence, m_fTorchNMSThreshold);
 
            // Add Torch objects to the list of newly detected objects.
            m_vNewlyDetectedObjects.insert(m_vNewlyDetectedObjects.end(), vNewTorchObjects.begin(), vNewTorchObjects.end());
        }
 
        // Set the FOV of the camera in the object structs for this detector's camera.
        for (objectdetectutils::Object& stObject : m_vNewlyDetectedObjects)
        {
            // Set the UUID of the detector that detected this object to this ObjectDetector's camera name so we can associate it with this detector.
            stObject.szDetectorUUID = this->GetThreadUUID();
            // Set object FOV parameter to this object detectors camera's FOV.
            stObject.dHorizontalFOV = m_pCamera->GetPropHorizontalFOV();
        }
 
        // Merge the newly detected objects with the pre-existing detected objects.
        this->UpdateDetectedObjects(m_vNewlyDetectedObjects);
 
        // Draw object overlays onto normal image.
        torchobject::DrawDetections(m_cvDetectionOverlayFrame, m_vDetectedObjects);
 
        // Check if the detected objects vector is not empty.
        if (!m_vDetectedObjects.empty())
        {
            // It's not empty so we should have a valid overlay frame with detections drawn on it.
            m_cvLastGoodOverlayFrame = m_cvDetectionOverlayFrame.clone();
        }
    }
 
    // Acquire a shared_lock on the detected objects copy queue.
    std::shared_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Check if the detected object copy queue is empty.
    if (!m_qDetectionOverlayFramesCopySchedule.empty() || !m_qLastGoodDetectionOverlayFramesCopySchedule.empty() || !m_qDetectedObjectCopySchedule.empty())
    {
        size_t siQueueLength = m_qDetectionOverlayFramesCopySchedule.size() + m_qLastGoodDetectionOverlayFramesCopySchedule.size() + m_qDetectedObjectCopySchedule.size();
        // Start the thread pool to store multiple copies of the detected objects to the requesting threads
        this->RunDetachedPool(siQueueLength, m_nNumDetectedObjectsRetrievalThreads);
        // Wait for thread pool to finish.
        this->JoinPool();
        // Release lock on frame copy queue.
        lkSchedulers.unlock();
    }
}

Here is the call graph for this function:

PooledLinearCode()

void ObjectDetector::PooledLinearCode ( )

overrideprivatevirtual

This method will run in a thread pool. It will be called by the main thread and will run the code within the PooledLinearCode() method. This is meant to be used as an internal utility of the child class to further improve parallelization.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

Implements AutonomyThread< void >.

{
    //  Detection Overlay Frame queue.
    // Acquire sole writing access to the detectedObjectCopySchedule.
    std::unique_lock<std::shared_mutex> lkObjectOverlayFrameQueue(m_muDetectionOverlayCopyMutex);
    // Check if there are unfulfilled requests.
    if (!m_qDetectionOverlayFramesCopySchedule.empty())
    {
        // Get frame container out of queue.
        containers::FrameFetchContainer<cv::Mat> stContainer = m_qDetectionOverlayFramesCopySchedule.front();
        // Pop out of queue.
        m_qDetectionOverlayFramesCopySchedule.pop();
        // Release lock.
        lkObjectOverlayFrameQueue.unlock();
 
        // Check which frame we should copy.
        switch (stContainer.eFrameType)
        {
            case PIXEL_FORMATS::eObjectDetection: *stContainer.pFrame = m_cvDetectionOverlayFrame.clone(); break;
            default: *stContainer.pFrame = m_cvDetectionOverlayFrame.clone(); break;
        }
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedFrameStatus->set_value(true);
    }
 
    //  Last GoodDetection Overlay Frame queue.
    // Acquire sole writing access to the detectedObjectCopySchedule.
    std::unique_lock<std::shared_mutex> lkLastGoodObjectOverlayFrameQueue(m_muLastGoodDetectionOverlayCopyMutex);
    // Check if there are unfulfilled requests.
    if (!m_qLastGoodDetectionOverlayFramesCopySchedule.empty())
    {
        // Get frame container out of queue.
        containers::FrameFetchContainer<cv::Mat> stContainer = m_qLastGoodDetectionOverlayFramesCopySchedule.front();
        // Pop out of queue.
        m_qLastGoodDetectionOverlayFramesCopySchedule.pop();
        // Release lock.
        lkLastGoodObjectOverlayFrameQueue.unlock();
 
        // Check which frame we should copy.
        switch (stContainer.eFrameType)
        {
            case PIXEL_FORMATS::eObjectDetection: *stContainer.pFrame = m_cvLastGoodOverlayFrame.clone(); break;
            default: *stContainer.pFrame = m_cvLastGoodOverlayFrame.clone(); break;
        }
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedFrameStatus->set_value(true);
    }
 
    //  Object queue.
    // Acquire sole writing access to the detectedObjectCopySchedule.
    std::unique_lock<std::shared_mutex> lkObjectQueue(m_muArucoDataCopyMutex);
    // Check if there are unfulfilled requests.
    if (!m_qDetectedObjectCopySchedule.empty())
    {
        // Get frame container out of queue.
        containers::DataFetchContainer<std::vector<objectdetectutils::Object>> stContainer = m_qDetectedObjectCopySchedule.front();
        // Pop out of queue.
        m_qDetectedObjectCopySchedule.pop();
        // Release lock.
        lkObjectQueue.unlock();
 
        // Copy the detected objects to the target location
        *stContainer.pData = m_vDetectedObjects;
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedDataStatus->set_value(true);
    }
}

Here is the call graph for this function:

UpdateDetectedObjects()

void ObjectDetector::UpdateDetectedObjects ( std::vector< objectdetectutils::Object > &  vNewlyDetectedObjects )

private

Update the detected objects with the newly detected objects.

Parameters

vNewlyDetectedObjects

- The vector of newly detected objects to update the detected objects with.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-05-05

{
    // Check if tracking is enabled.
    if (m_bEnableTracking)
    {
        // Check if the given object vector is empty
        if (vNewlyDetectedObjects.empty())
        {
            // Since the objects are empty that means the detector has not detected any new ground truth objects.
            // In this case we will fallback to relying on the multi-tracker to track the objects and just update the objects
            // stored in the m_vDetectedObjects vector.
            // This is necessary because the torch detector is not perfect and may not detect all objects in the frame
            // and it doesn't have the ability to track objects over time.
            // We will use the multi-tracker to track the objects over time and update the bounding box data for the objects.
 
            // Update the multi-tracker with the current frame.
            m_pMultiTracker->Update(m_cvFrame);
        }
        else
        {
            // Loop through the newly detected objects.
            for (objectdetectutils::Object& stObject : vNewlyDetectedObjects)
            {
                // Add the newly detected objects to the multi-tracker.
                bool bMatchedObjectToExistingTracker = m_pMultiTracker->InitTracker(m_cvFrame, stObject.pBoundingBox, constants::BBOX_TRACKER_TYPE);
                // Check if the object was matched to an existing tracker.
                if (!bMatchedObjectToExistingTracker)
                {
                    // Add the new object to the member variable list.
                    m_vDetectedObjects.emplace_back(stObject);
                }
                else
                {
                    // Find the object with the same bounding box pointer and update the ID and confidence.
                    for (objectdetectutils::Object& stExistingObject : m_vDetectedObjects)
                    {
                        // Check if the bounding box pointers are the same.
                        if (stObject.pBoundingBox == stExistingObject.pBoundingBox)
                        {
                            // Update the ID and confidence of the existing object.
                            stExistingObject.dConfidence = stObject.dConfidence;
                        }
                    }
                }
            }
 
            // Update the multi-tracker with the current frame.
            m_pMultiTracker->Update(m_cvFrame);
        }
 
        // Loop through the detected objects and check if there are any we need to remove, and also update the time last seen.
        for (std::vector<objectdetectutils::Object>::iterator itObject = m_vDetectedObjects.begin(); itObject != m_vDetectedObjects.end();)
        {
            // Check if the bounding box is 0,0,0,0.
            if (itObject->pBoundingBox->x == 0 && itObject->pBoundingBox->y == 0 && itObject->pBoundingBox->width == 0 && itObject->pBoundingBox->height == 0)
            {
                // Remove the object from the vector.
                itObject = m_vDetectedObjects.erase(itObject);
            }
            else
            {
                ++itObject;
            }
        }
    }
    else
    {
        // If tracking is not enabled, we will just clear the detected objects and add the new ones.
        m_vDetectedObjects.clear();
        // Loop through the newly detected objects and add them to the detected objects vector.
        for (objectdetectutils::Object& stObject : vNewlyDetectedObjects)
        {
            // Set the object creation time to 0. The objects aren't being tracked, so we can't really tell their age.
            stObject.tmCreation = std::chrono::system_clock::time_point::min();
 
            // Add the new object to the member variable list.
            m_vDetectedObjects.emplace_back(stObject);
        }
    }
 
    // Check if we are using a ZED camera.
    if (m_bUsingZedCamera)
    {
        // Check if the point cloud is empty.
        if (!m_cvPointCloud.empty())
        {
            // Get the rover pose from the waypoint handler.
            m_stRoverPose = globals::g_pStateMachineHandler->SmartRetrieveRoverPose();
 
            // Find the camera's position in UTM coordinates by applying the camera offset to the rover pose.
            geoops::UTMCoordinate stCamera = m_stRoverPose.GetUTMCoordinate();
 
            // Translate the camera's local offsets into global Easting/Northing.
            double dRoverHeadingRad = m_stRoverPose.GetCompassHeading() * (CV_PI / 180.0);
            double dRotatedX        = (m_pCamera->GetCameraPoseOffset().dPosY * sin(dRoverHeadingRad)) + (m_pCamera->GetCameraPoseOffset().dPosX * cos(dRoverHeadingRad));
            double dRotatedY        = (m_pCamera->GetCameraPoseOffset().dPosY * cos(dRoverHeadingRad)) - (m_pCamera->GetCameraPoseOffset().dPosX * sin(dRoverHeadingRad));
 
            // Apply the rotated offsets to the global coordinates.
            stCamera.dEasting += dRotatedX;
            stCamera.dNorthing += dRotatedY;
            stCamera.dAltitude += m_pCamera->GetCameraPoseOffset().dPosZ;
 
            // Creating variables for the camera pose offset values.
            double dQW = m_pCamera->GetCameraPoseOffset().dQW;
            double dQX = m_pCamera->GetCameraPoseOffset().dQX;
            double dQY = m_pCamera->GetCameraPoseOffset().dQY;
            double dQZ = m_pCamera->GetCameraPoseOffset().dQZ;
 
            // Update the rover pose's heading to match the camera's heading. We will need to calculate the camera's heading using the quaternion.
            double dSinYCosP      = 2.0 * (dQW * dQZ + dQX * dQY);
            double dCosYCosP      = 1.0 - 2.0 * (dQY * dQY + dQZ * dQZ);
            double dCameraHeading = std::atan2(dSinYCosP, dCosYCosP) * (180.0 / CV_PI);
 
            // Add the relative camera heading to the absolute rover heading
            double dAbsoluteCameraHeading = m_stRoverPose.GetCompassHeading() + dCameraHeading;
 
            // Recreate the rover pose with the camera's adjusted position and heading.
            geoops::RoverPose stCameraPose = geoops::RoverPose(stCamera, dAbsoluteCameraHeading);
 
            // Loop through the objects and use their center point to lookup their distance in the point cloud.
            for (objectdetectutils::Object& stObject : m_vDetectedObjects)
            {
                // Use either width of height for the neighborhood size.
                int nNeighborhoodSize = std::min(stObject.pBoundingBox->width, stObject.pBoundingBox->height);
                // Geolocate the object in the point cloud.
                geoops::Waypoint stGeolocation = geoloc::GeolocateBox(
                    m_cvPointCloud,
                    stCameraPose,
                    cv::Point(stObject.pBoundingBox->x + stObject.pBoundingBox->width / 2, stObject.pBoundingBox->y + stObject.pBoundingBox->height / 2),
                    nNeighborhoodSize);
 
                // Depending on the class name of the model, set the object type.
                if (stObject.szClassName == "mallet")
                {
                    stObject.eDetectionType = objectdetectutils::ObjectDetectionType::eMallet;
                }
                else if (stObject.szClassName == "bottle")
                {
                    stObject.eDetectionType = objectdetectutils::ObjectDetectionType::eWaterBottle;
                }
                else if (stObject.szClassName == "pick")
                {
                    stObject.eDetectionType = objectdetectutils::ObjectDetectionType::eRockPick;
                }
 
                // Calculate the yaw angle to the tag using the center point of the tag and the camera's field of view.
                // This is a fallback in case the geolocation fails for some reason, we can still provide a relative angle to the tag.
                // Get the center X pixel coordinate of the object's bounding box.
                double dObjectCenterX = stObject.pBoundingBox->x + (stObject.pBoundingBox->width / 2.0);
                // Get the center X pixel coordinate of the camera frame.
                double dFrameCenterX = m_cvFrame.cols / 2.0;
                // Calculate the offset in pixels from the center of the camera frame.
                // (Positive offset = target is to the right, Negative = target is to the left)
                double dPixelOffsetX = dObjectCenterX - dFrameCenterX;
                // Calculate how many real-world degrees each pixel represents.
                double dDegreesPerPixel = stObject.dHorizontalFOV / static_cast<double>(m_cvFrame.cols);
                // Multiply the pixel offset by the degrees per pixel to get the relative yaw angle.
                stObject.dYawAngle = dPixelOffsetX * dDegreesPerPixel;
                // Explicitly set distance to 0.0 so the autonomy state machines know the depth map failed
                // and will properly fall back to using this calculated dYawAngle.
                stObject.dStraightLineDistance = 0.0;
 
                // Check if the geolocation is valid. If it is overwrite the yaw angle and distance with the geolocation data.
                if (stGeolocation != geoops::Waypoint())
                {
                    // Since this is a object detection, set the object's waypoint type appropriately.
                    stGeolocation.eType = geoops::WaypointType::eObjectWaypoint;
                    // Calculate the geo measurement and print the distance to the object.
                    geoops::GeoMeasurement stMeasurement =
                        geoops::CalculateGeoMeasurement(m_stRoverPose.GetUTMCoordinate(), stObject.stGeolocatedPosition.GetUTMCoordinate());
 
                    // Check that the distance is in a reasonable range.
                    if (stMeasurement.dDistanceMeters > 0.0 && stMeasurement.dDistanceMeters < 25.0)
                    {
                        // Set the object's geolocation.
                        stObject.stGeolocatedPosition = stGeolocation;
                        // Use the rover heading and the azimuth angle to calculate the relative heading to the object.
                        stObject.dYawAngle = numops::AngularDifference(m_stRoverPose.GetCompassHeading(), stMeasurement.dStartRelativeBearing);
                        // Set the straight line distance to the object.
                        stObject.dStraightLineDistance = stMeasurement.dDistanceMeters;
                    }
                }
            }
        }
    }
    else
    {
        // Estimate the positions of the objects using some basic trig.
        for (objectdetectutils::Object& stObject : m_vDetectedObjects)
        {
            // Use some trig to get the location of the object.
            objectdetectutils::EstimatePoseFromCameraFrame(stObject);
        }
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

---

The documentation for this class was generated from the following files:

• src/vision/objects/ObjectDetector.h
• src/vision/objects/ObjectDetector.cpp