TagDetector Class Reference

Run's Aruco detection & camera pose estimation in a multithreading environment. Given a camera, tag detection using OpenCV's ArUco library and a custom trained model for detecting general tags will be continuously ran on the camera frames. More...

#include <TagDetector.h>

Inheritance diagram for TagDetector:

Collaboration diagram for TagDetector:

Public Member Functions
	TagDetector (std::shared_ptr< BasicCamera > pBasicCam, const int nArucoCornerRefinementMaxIterations=30, const int nArucoCornerRefinementMethod=cv::aruco::CORNER_REFINE_NONE, const int nArucoMarkerBorderBits=1, const bool bArucoDetectInvertedMarkers=false, const bool bUseAruco3Detection=false, const bool bEnableTracking=false, const int nDetectorMaxFPS=30, const bool bEnableRecordingFlag=false, const int nNumDetectedTagsRetrievalThreads=5, const bool bUsingGpuMats=false)
	Construct a new TagDetector object.
	TagDetector (std::shared_ptr< ZEDCamera > pZEDCam, const int nArucoCornerRefinementMaxIterations=30, const int nArucoCornerRefinementMethod=cv::aruco::CORNER_REFINE_NONE, const int nArucoMarkerBorderBits=1, const bool bArucoDetectInvertedMarkers=false, const bool bUseAruco3Detection=false, const bool bEnableTracking=false, const int nDetectorMaxFPS=30, const bool bEnableRecordingFlag=false, const int nNumDetectedTagsRetrievalThreads=5, const bool bUsingGpuMats=false)
	Construct a new TagDetector object.
	~TagDetector ()
	Destroy the Tag Detector:: Tag Detector object.
std::future< bool >	RequestDetectionOverlayFrame (cv::Mat &cvFrame)
	Request a copy of a frame containing the tag detection overlays from the aruco and torch library.
std::future< bool >	RequestLastGoodOverlayFrame (cv::Mat &cvFrame)
	Request a copy of a frame containing the last good tag detection overlays from the aruco and torch library.
std::future< bool >	RequestDetectedArucoTags (std::vector< tagdetectutils::ArucoTag > &vArucoTags)
	Request the most up to date vector of detected tags from OpenCV's Aruco algorithm.
bool	InitTorchDetection (const std::string &szModelPath, yolomodel::pytorch::PyTorchInterpreter::HardwareDevices eDevice=yolomodel::pytorch::PyTorchInterpreter::HardwareDevices::eCUDA)
	Attempt to open the next available Torch hardware and load model at the given path onto the device.
void	EnableTorchDetection (const float fMinObjectConfidence=0.4f, const float fNMSThreshold=0.6f)
	Turn on torch detection with given parameters.
void	DisableTorchDetection ()
	Set flag to stop tag detection with the torch model.
void	SetDetectorMaxFPS (const int nRecordingFPS)
	Mutator for the desired max FPS for this detector.
void	SetEnableRecordingFlag (const bool bEnableRecordingFlag)
	Mutator for the Enable Recording Flag private member.
bool	GetIsReady ()
	Accessor for the status of this TagDetector.
int	GetDetectorMaxFPS () const
	Accessor for the desired max FPS for this detector.
bool	GetEnableRecordingFlag () const
	Accessor for the Enable Recording Flag private member.
std::string	GetCameraName ()
	Accessor for the camera name or path that this TagDetector is tied to.
cv::Size	GetProcessFrameResolution () const
	Accessor for the resolution of the process image used for tag detection.
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 code will run continuously in a separate thread. New frames from the given camera are grabbed and the tags for the camera image are detected, filtered, and stored. Then any requests for the current tags are fulfilled.
void	PooledLinearCode () override
	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 queue filled by the Request methods.
void	UpdateDetectedTags (std::vector< tagdetectutils::ArucoTag > &vNewlyDetectedTags)
	Updates the detected torch tags including tracking the detected tags over time and removing tags that haven't been seen for long enough.

Private Attributes
std::shared_ptr< Camera< cv::Mat > >	m_pCamera
cv::aruco::ArucoDetector	m_cvArucoDetector
cv::aruco::DetectorParameters	m_cvArucoDetectionParams
cv::aruco::Dictionary	m_cvTagDictionary
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_nNumDetectedTagsRetrievalThreads
std::string	m_szCameraName
std::atomic_bool	m_bEnableRecordingFlag
std::vector< tagdetectutils::ArucoTag >	m_vNewlyDetectedTags
std::vector< tagdetectutils::ArucoTag >	m_vDetectedArucoTags
geoops::RoverPose	m_stRoverPose
cv::Mat	m_cvFrame
cv::cuda::GpuMat	m_cvGPUFrame
cv::Mat	m_cvArucoProcFrame
cv::Mat	m_cvLastGoodDetectionOverlayFrame
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< tagdetectutils::ArucoTag > > >	m_qDetectedArucoTagCopySchedule
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

Run's Aruco detection & camera pose estimation in a multithreading environment. Given a camera, tag detection using OpenCV's ArUco library and a custom trained model for detecting general tags will be continuously ran on the camera frames.

What are the threads doing? Continuous Thread: In this thread we are constantly getting images and depth maps from the necessary cameras. We then detect the tags in the image and estimate their location with respect to the rover. Pooled Threads: Copy the vector of detected tags to all of the threads requesting it through the RequestDetectedArucoTags(...) function.

Author

jspencerpittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om), clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-30

Constructor & Destructor Documentation

TagDetector() [1/2]

TagDetector::TagDetector	(	std::shared_ptr< BasicCamera >	pBasicCam,
		const int	nArucoCornerRefinementMaxIterations = 30,
		const int	nArucoCornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE,
		const int	nArucoMarkerBorderBits = 1,
		const bool	bArucoDetectInvertedMarkers = false,
		const bool	bUseAruco3Detection = false,
		const bool	bEnableTracking = false,
		const int	nDetectorMaxFPS = 30,
		const bool	bEnableRecordingFlag = false,
		const int	nNumDetectedTagsRetrievalThreads = 5,
		const bool	bUsingGpuMats = false
	)

Construct a new TagDetector object.

Parameters

pBasicCam	- A pointer to the BasicCam camera to get frames from for detection.
nArucoCornerRefinementMaxIterations	- The number of iterations to use when refining marker corners.
nArucoCornerRefinementMethod	- The refinement method to use.
nArucoMarkerBorderBits	- The number of border unit squares around the marker.
bArucoDetectInvertedMarkers	- Enable or disable upside-down marker detection.
bUseAruco3Detection	- Whether or not to use the newer/faster method of detection. Experimental.
bEnableTracking	- Whether or not to enable tracking of detected tags.
nDetectorMaxFPS	- The max FPS limit the detector can run at.
bEnableRecordingFlag	- Whether or not this TagDetector's overlay output should be recorded.
nNumDetectedTagsRetrievalThreads	- The number of threads to use when fulfilling requests for the detected aruco tags. 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

2023-10-10

{
    // Initialize member variables.
    m_pCamera                          = pBasicCam;
    m_bTorchInitialized                = false;
    m_bTorchEnabled                    = false;
    m_bEnableTracking                  = bEnableTracking;
    m_bUsingZedCamera                  = false;    // Toggle ZED functions off.
    m_bUsingGpuMats                    = bUsingGpuMats;
    m_bCameraIsOpened                  = false;
    m_nNumDetectedTagsRetrievalThreads = nNumDetectedTagsRetrievalThreads;
    m_szCameraName                     = pBasicCam->GetCameraLocation();
    m_bEnableRecordingFlag             = bEnableRecordingFlag;
    m_stRoverPose                      = geoops::RoverPose();
 
    // Setup aruco detector params.
    m_cvArucoDetectionParams                               = cv::aruco::DetectorParameters();
    m_cvArucoDetectionParams.cornerRefinementMaxIterations = nArucoCornerRefinementMaxIterations;
    m_cvArucoDetectionParams.cornerRefinementMethod        = nArucoCornerRefinementMethod;
    m_cvArucoDetectionParams.markerBorderBits              = nArucoMarkerBorderBits;
    m_cvArucoDetectionParams.detectInvertedMarker          = bArucoDetectInvertedMarkers;
    m_cvArucoDetectionParams.useAruco3Detection            = bUseAruco3Detection;
    // Get aruco dictionary and initialize aruco detector.
    m_cvTagDictionary = cv::aruco::getPredefinedDictionary(constants::ARUCO_DICTIONARY);
    m_cvArucoDetector = cv::aruco::ArucoDetector(m_cvTagDictionary, m_cvArucoDetectionParams);
 
    // Create a multi-tracker for tracking multiple tags 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, "TagDetector created for camera at path/index: {}", m_szCameraName);
}

Here is the call graph for this function:

TagDetector() [2/2]

TagDetector::TagDetector	(	std::shared_ptr< ZEDCamera >	pZEDCam,
		const int	nArucoCornerRefinementMaxIterations = 30,
		const int	nArucoCornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE,
		const int	nArucoMarkerBorderBits = 1,
		const bool	bArucoDetectInvertedMarkers = false,
		const bool	bUseAruco3Detection = false,
		const bool	bEnableTracking = false,
		const int	nDetectorMaxFPS = 30,
		const bool	bEnableRecordingFlag = false,
		const int	nNumDetectedTagsRetrievalThreads = 5,
		const bool	bUsingGpuMats = false
	)

Construct a new TagDetector object.

Parameters

pZEDCam	- A pointer to the ZEDCam camera to get frames from for detection. Override for ZED camera.
nArucoCornerRefinementMaxIterations	- The number of iterations to use when refining marker corners.
nArucoCornerRefinementMethod	- The refinement method to use.
nArucoMarkerBorderBits	- The number of border unit squares around the marker.
bArucoDetectInvertedMarkers	- Enable or disable upside-down marker detection.
bUseAruco3Detection	- Whether or not to use the newer/faster method of detection. Experimental.
bEnableTracking	- Whether or not to enable tracking of detected tags.
nDetectorMaxFPS	- The max FPS limit the detector can run at.
bEnableRecordingFlag	- Whether or not this TagDetector's overlay output should be recorded.
nNumDetectedTagsRetrievalThreads	- The number of threads to use when fulfilling requests for the detected aruco tags. 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

2023-10-07

{
    // Initialize member variables.
    m_pCamera                          = pZEDCam;
    m_bTorchInitialized                = false;
    m_bTorchEnabled                    = false;
    m_bUsingZedCamera                  = true;    // Toggle ZED functions on.
    m_bEnableTracking                  = bEnableTracking;
    m_bUsingGpuMats                    = bUsingGpuMats;
    m_bCameraIsOpened                  = false;
    m_nNumDetectedTagsRetrievalThreads = nNumDetectedTagsRetrievalThreads;
    m_szCameraName                     = pZEDCam->GetCameraModel() + "_" + std::to_string(pZEDCam->GetCameraSerial());
    m_bEnableRecordingFlag             = bEnableRecordingFlag;
    m_IPS                              = IPS();
    m_stRoverPose                      = geoops::RoverPose();
 
    // Setup aruco detector params.
    m_cvArucoDetectionParams                               = cv::aruco::DetectorParameters();
    m_cvArucoDetectionParams.cornerRefinementMaxIterations = nArucoCornerRefinementMaxIterations;
    m_cvArucoDetectionParams.cornerRefinementMethod        = nArucoCornerRefinementMethod;
    m_cvArucoDetectionParams.markerBorderBits              = nArucoMarkerBorderBits;
    m_cvArucoDetectionParams.detectInvertedMarker          = bArucoDetectInvertedMarkers;
    m_cvArucoDetectionParams.useAruco3Detection            = bUseAruco3Detection;
 
    // Create a multi-tracker for tracking multiple tags 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, "TagDetector created for camera: {}", m_szCameraName);
}

Here is the call graph for this function:

~TagDetector()

TagDetector::~TagDetector

(

)

Destroy the Tag Detector:: Tag Detector object.

Author

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

Date

2024-01-09

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

Here is the call graph for this function:

Member Function Documentation

RequestDetectionOverlayFrame()

std::future< bool > TagDetector::RequestDetectionOverlayFrame

(

cv::Mat &

cvFrame

)

Request a copy of a frame containing the tag detection overlays from the aruco and torch library.

Parameters

cvFrame

- The frame to copy the detection overlay image to.

Returns

std::future<bool> - The future that should be waited on before using the passed in frame. Future will be true or false based on whether or not the frame was successfully retrieved.

Author

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

Date

2023-10-11

{
    // Assemble the DataFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvFrame, PIXEL_FORMATS::eArucoDetection);
 
    // 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();
}

RequestLastGoodOverlayFrame()

std::future< bool > TagDetector::RequestLastGoodOverlayFrame

(

cv::Mat &

cvFrame

)

Request a copy of a frame containing the last good tag detection overlays from the aruco and torch library.

Parameters

cvFrame

- The frame to copy the detection overlay image to.

Returns

std::future<bool> - The future that should be waited on before using the passed in frame. Future will be true or false based on whether or not the frame was successfully retrieved.

Author

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

Date

2023-10-11

{
    // Assemble the DataFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvFrame, PIXEL_FORMATS::eArucoDetection);
 
    // 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();
}

RequestDetectedArucoTags()

std::future< bool > TagDetector::RequestDetectedArucoTags

(

std::vector< tagdetectutils::ArucoTag > &

vArucoTags

)

Request the most up to date vector of detected tags from OpenCV's Aruco algorithm.

Parameters

vArucoTags

- The vector the detected aruco tags will be saved to.

Returns

std::future<bool> - The future that should be waited on before using the passed in tag vector. Future will be true or false based on whether or not the tags were successfully retrieved.

Author

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

Date

2023-10-07

{
    // Assemble the DataFetchContainer.
    containers::DataFetchContainer<std::vector<tagdetectutils::ArucoTag>> stContainer(vArucoTags);
 
    // Acquire lock on pool copy queue.
    std::unique_lock<std::shared_mutex> lkScheduler(m_muPoolScheduleMutex);
    // Append detected tag fetch container to the schedule queue.
    m_qDetectedArucoTagCopySchedule.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 TagDetector::InitTorchDetection	(	const std::string &	szModelPath,
		yolomodel::pytorch::PyTorchInterpreter::HardwareDevices	eDevice = yolomodel::pytorch::PyTorchInterpreter::HardwareDevices::eCUDA
	)

Attempt to open the next available Torch hardware and load model at the given path onto the device.

Parameters

szModelPath	- The absolute path to the model to open.
eDevice	- The hardware device to launch the Torch model on.

Returns

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

false - Something went wrong, model/device not opened.

Author

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

Date

2025-03-06

{
    // 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 TagDetector.");
        // Update member variable.
        m_bTorchInitialized = false;
        // Return status.
        return false;
    }
}

EnableTorchDetection()

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

Turn on torch detection with given parameters.

Parameters

fMinObjectConfidence	- The lower limit of detection confidence.
fNMSThreshold	- The overlap thresh for NMS algorithm.

Author

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

Date

2025-03-06

{
    // 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 TagDetector but it has not been initialized yet!");
        // Update member variable.
        m_bTorchEnabled = false;
    }
}

DisableTorchDetection()

void TagDetector::DisableTorchDetection

(

)

Set flag to stop tag 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-03-06

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

SetDetectorMaxFPS()

void TagDetector::SetDetectorMaxFPS

(

const int

nRecordingFPS

)

Mutator for the desired max FPS for this detector.

Parameters

nRecordingFPS

- The max frames per second to detect tags at.

Author

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

Date

2024-01-22

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

Here is the call graph for this function:

SetEnableRecordingFlag()

void TagDetector::SetEnableRecordingFlag

(

const bool

bEnableRecordingFlag

)

Mutator for the Enable Recording Flag private member.

Parameters

bEnableRecordingFlag

- Whether or not recording should be enabled for this detector.

Author

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

Date

2023-12-31

{
    m_bEnableRecordingFlag = bEnableRecordingFlag;
}

GetIsReady()

bool TagDetector::GetIsReady

(

)

Accessor for the status of this TagDetector.

Returns

true - The detector is running and detecting tags from the camera.

false - The detector thread and/or camera is not running/opened.

Author

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

Date

2024-01-04

{
    // 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 TagDetector::GetDetectorMaxFPS

(

)

const

Accessor for the desired max FPS for this detector.

Returns

int - The max frames per second the detector can run at.

Author

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

Date

2024-01-22

{
    // Return member variable value.
    return this->GetMainThreadMaxIPS();
}

Here is the call graph for this function:

GetEnableRecordingFlag()

bool TagDetector::GetEnableRecordingFlag

(

)

const

Accessor for the Enable Recording Flag private member.

Returns

true - Recording for this detector has been requested/flagged.

false - This detector should not be recorded.

Author

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

Date

2023-12-31

{
    return m_bEnableRecordingFlag;
}

GetCameraName()

std::string TagDetector::GetCameraName

(

)

Accessor for the camera name or path that this TagDetector is tied to.

Returns

std::string - The name/path/index of the camera used by this TagDetector.

Author

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

Date

2024-01-01

{
    return m_szCameraName;
}

Here is the caller graph for this function:

GetProcessFrameResolution()

cv::Size TagDetector::GetProcessFrameResolution

(

)

const

Accessor for the resolution of the process image used for tag detection.

Returns

cv::Size - The resolution stored in an OpenCV cv::Size.

Author

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

Date

2024-01-01

{
    // 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 TagDetector::ThreadedContinuousCode ( )

overrideprivatevirtual

This code will run continuously in a separate thread. New frames from the given camera are grabbed and the tags for the camera image are detected, filtered, and stored. Then any requests for the current tags are fulfilled.

Author

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

Date

2023-10-07

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,
                             "TagDetector start was attempted for ZED camera with serial number {}, but camera never properly opened or it has been closed/rebooted! "
                             "This tag 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,
                             "TagDetector 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, "TagDetector unable to get point cloud or frame from ZEDCam!");
                }
            }
            else
            {
                if (!fuPointCloudCopyStatus.get())
                    LOG_WARNING(logging::g_qSharedLogger, "TagDetector unable to get point cloud from ZEDCam!");
                if (!fuRegularFrameCopyStatus.get())
                    LOG_WARNING(logging::g_qSharedLogger, "TagDetector unable to get regular frame from ZEDCam!");
            }
        }
        else
        {
            if (!fuRegularFrameCopyStatus.get())
            {
                LOG_WARNING(logging::g_qSharedLogger, "TagDetector 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 tags.
        m_vNewlyDetectedTags.clear();
        // Clone frames.
        m_cvArucoProcFrame = m_cvFrame.clone();
        // Detect tags in the image
        std::vector<tagdetectutils::ArucoTag> vNewOpenCVTags = arucotag::Detect(m_cvArucoProcFrame, m_cvArucoDetector);
        // Loop through the newly detected OpenCV tags and set their detector UUID to this TagDetector's camera name so we can associate them with this detector.
        for (tagdetectutils::ArucoTag& stTag : vNewOpenCVTags)
        {
            stTag.szDetectorUUID = this->GetThreadUUID();
        }
        // Add OpenCV tags to the list of newly detected tags.
        m_vNewlyDetectedTags.insert(m_vNewlyDetectedTags.end(), vNewOpenCVTags.begin(), vNewOpenCVTags.end());
 
        // Check if torch detection if turned on.
        if (m_bTorchEnabled)
        {
            // Detect tags in the image.
            std::vector<tagdetectutils::ArucoTag> vNewTorchTags =
                torchtag::Detect(m_cvArucoProcFrame, *m_pTorchDetector, m_fTorchMinObjectConfidence, m_fTorchNMSThreshold);
 
            // Add Torch tags to the list of newly detected tags.
            m_vNewlyDetectedTags.insert(m_vNewlyDetectedTags.end(), vNewTorchTags.begin(), vNewTorchTags.end());
        }
 
        // Set the FOV of the camera in the tag structs for this detector's camera.
        for (tagdetectutils::ArucoTag& stTag : m_vNewlyDetectedTags)
        {
            // Set the UUID of the detector that detected this tag to this TagDetector's camera name so we can associate it with this detector.
            stTag.szDetectorUUID = this->GetThreadUUID();
            // Set tag FOV parameter to this tag detectors camera's FOV.
            stTag.dHorizontalFOV = m_pCamera->GetPropHorizontalFOV();
        }
 
        // Merge the newly detected tags with the pre-existing detected tags.
        this->UpdateDetectedTags(m_vNewlyDetectedTags);
 
        // Draw tag overlays onto normal image.
        arucotag::DrawDetections(m_cvArucoProcFrame, m_vDetectedArucoTags);
        torchtag::DrawDetections(m_cvArucoProcFrame, m_vDetectedArucoTags);
 
        // Check if the detected tags vector is empty. If not, set the last good detection overlay frame to the current one with detections drawn on it.
        if (!m_vDetectedArucoTags.empty())
        {
            m_cvLastGoodDetectionOverlayFrame = m_cvArucoProcFrame.clone();
        }
    }
 
    // Acquire a shared_lock on the detected tags copy queue.
    std::shared_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Check if the detected tag copy queue is empty.
    if (!m_qDetectionOverlayFramesCopySchedule.empty() || !m_qDetectedArucoTagCopySchedule.empty())
    {
        size_t siQueueLength = m_qDetectionOverlayFramesCopySchedule.size() + m_qDetectedArucoTagCopySchedule.size();
        // Start the thread pool to store multiple copies of the detected tags to the requesting threads
        this->RunDetachedPool(siQueueLength, m_nNumDetectedTagsRetrievalThreads);
        // 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 TagDetector::PooledLinearCode ( )

overrideprivatevirtual

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 queue filled by the Request methods.

Author

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

Date

2023-10-08

Implements AutonomyThread< void >.

{
    //  Detection Overlay Frame queue.
    // Acquire sole writing access to the detectedTagCopySchedule.
    std::unique_lock<std::shared_mutex> lkTagOverlayFrameQueue(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.
        lkTagOverlayFrameQueue.unlock();
 
        // Check which frame we should copy.
        switch (stContainer.eFrameType)
        {
            case PIXEL_FORMATS::eArucoDetection: *stContainer.pFrame = m_cvArucoProcFrame.clone(); break;
            default: *stContainer.pFrame = m_cvArucoProcFrame.clone(); break;
        }
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedFrameStatus->set_value(true);
    }
 
    //  Last Good Detection Overlay Frame queue.
    // Acquire sole writing access to the detectedTagCopySchedule.
    std::unique_lock<std::shared_mutex> lkLastGoodDetectionOverlayFrameQueue(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.
        lkLastGoodDetectionOverlayFrameQueue.unlock();
 
        // Check which frame we should copy.
        switch (stContainer.eFrameType)
        {
            case PIXEL_FORMATS::eArucoDetection: *stContainer.pFrame = m_cvLastGoodDetectionOverlayFrame.clone(); break;
            default: *stContainer.pFrame = m_cvLastGoodDetectionOverlayFrame.clone(); break;
        }
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedFrameStatus->set_value(true);
    }
 
    //  ArucoTag queue.
    // Acquire sole writing access to the detectedTagCopySchedule.
    std::unique_lock<std::shared_mutex> lkArucoTagQueue(m_muArucoDataCopyMutex);
    // Check if there are unfulfilled requests.
    if (!m_qDetectedArucoTagCopySchedule.empty())
    {
        // Get frame container out of queue.
        containers::DataFetchContainer<std::vector<tagdetectutils::ArucoTag>> stContainer = m_qDetectedArucoTagCopySchedule.front();
        // Pop out of queue.
        m_qDetectedArucoTagCopySchedule.pop();
        // Release lock.
        lkArucoTagQueue.unlock();
 
        // Copy the detected tags to the target location
        *stContainer.pData = m_vDetectedArucoTags;
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedDataStatus->set_value(true);
    }
}

Here is the call graph for this function:

UpdateDetectedTags()

void TagDetector::UpdateDetectedTags ( std::vector< tagdetectutils::ArucoTag > &  vNewlyDetectedTags )

private

Updates the detected torch tags including tracking the detected tags over time and removing tags that haven't been seen for long enough.

Parameters

vNewlyDetectedTags

- Input vector of TorchTag structs containing the tag info.

Author

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

Date

2025-03-15

{
    // Check if tracking is enabled.
    if (m_bEnableTracking)
    {
        // Check if the given tag vector is empty
        if (vNewlyDetectedTags.empty())
        {
            // Since the tags are empty that means the detector has not detected any new ground truth tags.
            // In this case we will fallback to relying on the multi-tracker to track the tags and just update the tags
            // stored in the m_vDetectedArucoTags vector.
            // This is necessary because the torch detector is not perfect and may not detect all tags in the frame
            // and it doesn't have the ability to track tags over time.
            // We will use the multi-tracker to track the tags over time and update the bounding box data for the tags.
 
            // Update the multi-tracker with the current frame.
            m_pMultiTracker->Update(m_cvFrame);
        }
        else
        {
            // Loop through the newly detected tags.
            for (tagdetectutils::ArucoTag& stTag : vNewlyDetectedTags)
            {
                // Add the newly detected tags to the multi-tracker.
                bool bMatchedTagToExistingTracker = m_pMultiTracker->InitTracker(m_cvFrame, stTag.pBoundingBox, constants::BBOX_TRACKER_TYPE);
                // Check if the tag was matched to an existing tracker.
                if (!bMatchedTagToExistingTracker)
                {
                    // Add the new tag to the member variable list.
                    m_vDetectedArucoTags.emplace_back(stTag);
                }
                else
                {
                    // Find the tag with the same bounding box pointer and update the ID and confidence.
                    for (tagdetectutils::ArucoTag& stExistingTag : m_vDetectedArucoTags)
                    {
                        // Check if the bounding box pointers are the same.
                        if (stTag.pBoundingBox == stExistingTag.pBoundingBox)
                        {
                            // Update the ID and confidence of the existing tag.
                            stExistingTag.nID         = stTag.nID;
                            stExistingTag.dConfidence = stTag.dConfidence;
                        }
                    }
                }
            }
 
            // Update the multi-tracker with the current frame.
            m_pMultiTracker->Update(m_cvFrame);
        }
 
        // Loop through the detected tags and check if there are any we need to remove, and also update the time last seen.
        for (std::vector<tagdetectutils::ArucoTag>::iterator itTag = m_vDetectedArucoTags.begin(); itTag != m_vDetectedArucoTags.end();)
        {
            // Check if the bounding box is 0,0,0,0.
            if (itTag->pBoundingBox->x == 0 && itTag->pBoundingBox->y == 0 && itTag->pBoundingBox->width == 0 && itTag->pBoundingBox->height == 0)
            {
                // Remove the tag from the vector.
                itTag = m_vDetectedArucoTags.erase(itTag);
            }
            else
            {
                ++itTag;
            }
        }
    }
    else
    {
        // If tracking is not enabled, we will just clear the detected tags and add the new ones.
        m_vDetectedArucoTags.clear();
        // Loop through the newly detected tags and add them to the detected tags vector.
        for (tagdetectutils::ArucoTag& stTag : vNewlyDetectedTags)
        {
            // Set the tag creation time to 0. The tags aren't being tracked, so we can't really tell their age.
            stTag.tmCreation = std::chrono::system_clock::time_point::min();
 
            // Add the new tag to the member variable list.
            m_vDetectedArucoTags.emplace_back(stTag);
        }
    }
 
    // 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 quaternion 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 * dQY + dQX * dQZ);
            double dCosYCosP      = 1.0 - 2.0 * (dQX * dQX + dQY * dQY);
            double dCameraHeading = std::atan2(dSinYCosP, dCosYCosP) * (180.0 / CV_PI);
 
            // Add the relative camera heading to the absolute rover heading
            double dAbsoluteCameraHeading = numops::InputAngleModulus<double>(m_stRoverPose.GetCompassHeading() + dCameraHeading, 0.0, 360.0);
 
            // Recreate the rover pose with the camera's adjusted absolute position and absolute heading
            geoops::RoverPose stCameraPose = geoops::RoverPose(stCamera, dAbsoluteCameraHeading);
 
            // LOG_NOTICE(logging::g_qSharedLogger,
            //            "RoverPose GPS: {} {} | RoverPose Heading: {}",
            //            stCameraPose.GetGPSCoordinate().dLatitude,
            //            stCameraPose.GetGPSCoordinate().dLongitude,
            //            stCameraPose.GetCompassHeading());
            // // Recreate the rover pose with the camera's adjusted position and heading.
            // geoops::RoverPose stCameraPose = geoops::RoverPose(stCamera, dCameraHeading);
 
            // Loop through the tags and use their center point to lookup their distance in the point cloud.
            for (tagdetectutils::ArucoTag& stTag : m_vDetectedArucoTags)
            {
                // Use either width of height for the neighborhood size.
                int nNeighborhoodSize = std::min(stTag.pBoundingBox->width, stTag.pBoundingBox->height);
                // Geolocate the tag in the point cloud.
                geoops::Waypoint stGeolocation =
                    geoloc::GeolocateBox(m_cvPointCloud,
                                         stCameraPose,
                                         cv::Point(stTag.pBoundingBox->x + stTag.pBoundingBox->width / 2, stTag.pBoundingBox->y + stTag.pBoundingBox->height / 2),
                                         nNeighborhoodSize);
 
                // 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 tag's bounding box.
                double dTagCenterX = stTag.pBoundingBox->x + (stTag.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 = dTagCenterX - dFrameCenterX;
                // Calculate how many real-world degrees each pixel represents.
                double dDegreesPerPixel = stTag.dHorizontalFOV / static_cast<double>(m_cvFrame.cols);
                // Multiply the pixel offset by the degrees per pixel to get the relative yaw angle.
                stTag.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.
                stTag.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 tag detection, set the tag's waypoint type appropriately.
                    stGeolocation.eType = geoops::WaypointType::eTagWaypoint;
                    // Calculate the geo measurement and print the distance to the tag.
                    geoops::GeoMeasurement stMeasurement = geoops::CalculateGeoMeasurement(m_stRoverPose.GetUTMCoordinate(), stGeolocation.GetUTMCoordinate());
 
                    // Check that the distance is in a reasonable range.
                    if (stMeasurement.dDistanceMeters > 0.0 && stMeasurement.dDistanceMeters < 25.0)
                    {
                        // Set the tag's geolocation.
                        stTag.stGeolocatedPosition = stGeolocation;
                        // Use the rover heading and the azimuth angle to calculate the relative heading to the tag.
                        stTag.dYawAngle = numops::AngularDifference(m_stRoverPose.GetCompassHeading(), stMeasurement.dStartRelativeBearing);
                        // Set the straight line distance to the tag.
                        stTag.dStraightLineDistance = stMeasurement.dDistanceMeters;
                    }
                }
            }
        }
    }
    else
    {
        // Estimate the positions of the tags using some basic trig.
        for (tagdetectutils::ArucoTag& stTag : m_vDetectedArucoTags)
        {
            // Use some trig to get the location of the tag.
            tagdetectutils::EstimatePoseFromCameraFrame(stTag);
        }
    }
}

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/aruco/TagDetector.h
• src/vision/aruco/TagDetector.cpp