From a31e0406b877735d3bd0b4d70c50d7d57fee0768 Mon Sep 17 00:00:00 2001 From: Timocop Date: Thu, 7 Apr 2022 13:57:54 +1200 Subject: [PATCH] Improve tracker frame syncing --- .../Device/Manager/ControllerManager.cpp | 2 +- .../Device/Manager/DeviceManager.cpp | 5 +- .../Device/Manager/HMDManager.cpp | 2 +- .../Device/Manager/TrackerManager.cpp | 67 +++++++++-------- .../Device/Manager/TrackerManager.h | 21 ++++-- .../Device/View/ServerControllerView.cpp | 2 +- .../PSMoveTracker/PS3EyeTracker.cpp | 30 ++++++-- .../PSMoveTracker/PSEye/PSEyeVideoCapture.cpp | 71 +++++++++++++++---- .../PSMoveTracker/PSEye/PSEyeVideoCapture.h | 7 ++ 9 files changed, 141 insertions(+), 66 deletions(-) diff --git a/src/psmoveservice/Device/Manager/ControllerManager.cpp b/src/psmoveservice/Device/Manager/ControllerManager.cpp index 24c9193a..68fedddc 100644 --- a/src/psmoveservice/Device/Manager/ControllerManager.cpp +++ b/src/psmoveservice/Device/Manager/ControllerManager.cpp @@ -133,7 +133,7 @@ ControllerManager::updateStateAndPredict(TrackerManager* tracker_manager) controllerView->getControllerDeviceType() != CommonDeviceState::PSNavi && (controllerView->getIsBluetooth() || controllerView->getIsVirtualController())) { - if (TrackerManager::isTrackerSynced()) + if (TrackerManager::trackersSynced()) { controllerView->updateOpticalPoseEstimation(tracker_manager); } diff --git a/src/psmoveservice/Device/Manager/DeviceManager.cpp b/src/psmoveservice/Device/Manager/DeviceManager.cpp index 0fea35f6..07e5d419 100644 --- a/src/psmoveservice/Device/Manager/DeviceManager.cpp +++ b/src/psmoveservice/Device/Manager/DeviceManager.cpp @@ -194,16 +194,15 @@ DeviceManager::update() m_platform_api->poll(); // Send device hotplug events } - - m_controller_manager->poll(); // Update controller counts and poll button/IMU state m_tracker_manager->poll(); // Update tracker count and poll video frames + m_controller_manager->poll(); // Update controller counts and poll button/IMU state m_hmd_manager->poll(); // Update HMD count and poll IMU state m_controller_manager->updateStateAndPredict(m_tracker_manager); // Compute pose/prediction of tracking blob+IMU state m_hmd_manager->updateStateAndPredict(m_tracker_manager); // Compute pose/prediction of tracking blobs+IMU state - m_controller_manager->publish(); // publish controller state to any listening clients (common case) m_tracker_manager->publish(); // publish tracker state to any listening clients (probably only used by ConfigTool) + m_controller_manager->publish(); // publish controller state to any listening clients (common case) m_hmd_manager->publish(); // publish hmd state to any listening clients (common case) } diff --git a/src/psmoveservice/Device/Manager/HMDManager.cpp b/src/psmoveservice/Device/Manager/HMDManager.cpp index d6af5faf..2ab915b7 100644 --- a/src/psmoveservice/Device/Manager/HMDManager.cpp +++ b/src/psmoveservice/Device/Manager/HMDManager.cpp @@ -91,7 +91,7 @@ HMDManager::updateStateAndPredict(TrackerManager* tracker_manager) if (hmdView->getIsOpen()) { - if (TrackerManager::isTrackerSynced()) + if (TrackerManager::trackersSynced()) { hmdView->updateOpticalPoseEstimation(tracker_manager); } diff --git a/src/psmoveservice/Device/Manager/TrackerManager.cpp b/src/psmoveservice/Device/Manager/TrackerManager.cpp index 50c05a9f..57dddf6e 100644 --- a/src/psmoveservice/Device/Manager/TrackerManager.cpp +++ b/src/psmoveservice/Device/Manager/TrackerManager.cpp @@ -178,7 +178,9 @@ TrackerManagerConfig::get_global_down_axis() const } //-- Tracker Manager ----- -bool TrackerManager::m_isTrackerSycned = false; +bool TrackerManager::m_trackersSynced = true; +bool TrackerManager::m_isTrackerFrameAvailable[TrackerManager::k_max_devices]; +bool TrackerManager::m_readyToReceive = false; TrackerManager::TrackerManager() : DeviceTypeManager(10000, 13) @@ -216,6 +218,36 @@ TrackerManager::startup() return bSuccess; } +void TrackerManager::poll_devices() +{ + m_trackersSynced = false; + m_readyToReceive = true; + for (int i = 0; i < getMaxDevices(); i++) + { + const ServerTrackerViewPtr tracker = getTrackerViewPtr(i); + if (tracker->getIsOpen()) + { + if (!m_isTrackerFrameAvailable[tracker->getDeviceID()]) + { + m_readyToReceive = false; + break; + } + } + } + + DeviceTypeManager::poll_devices(); + + if (m_readyToReceive) + { + for (int i = 0; i < getMaxDevices(); i++) + { + m_isTrackerFrameAvailable[i] = false; + } + m_readyToReceive = false; + m_trackersSynced = true; + } +} + void TrackerManager::closeAllTrackers() { @@ -433,36 +465,3 @@ TrackerManager::freeTrackingColorID(eCommonTrackingColorID color_id) assert(std::find(m_available_color_ids.begin(), m_available_color_ids.end(), color_id) == m_available_color_ids.end()); m_available_color_ids.push_back(color_id); } - -bool -TrackerManager::trackersSynced() -{ - double lowestFps = -1.f; - for (int i = 0; i < getMaxDevices(); i++) - { - const ServerTrackerViewPtr tracker = getTrackerViewPtr(i); - if (tracker->getIsOpen()) - { - const double fps = tracker->getFrameRate(); - - if (lowestFps < 0.f || lowestFps > fps) - lowestFps = fps; - } - } - - const std::chrono::time_point now = std::chrono::high_resolution_clock::now(); - const std::chrono::duration timeSinceLast = now - m_lastSync; - - //###Externet $TODO Use real tracker fps instead not absolute fps? - // Removing 1 frame removes some weird frame buffer glitch. Unsure what causes this. - // Needs to be investigated. - if (timeSinceLast.count() < (1000.f / lowestFps - 1.f)) - { - m_isTrackerSycned = false; - return false; - } - - m_isTrackerSycned = true; - m_lastSync = now; - return true; -} \ No newline at end of file diff --git a/src/psmoveservice/Device/Manager/TrackerManager.h b/src/psmoveservice/Device/Manager/TrackerManager.h index 6d69666f..4c93b811 100644 --- a/src/psmoveservice/Device/Manager/TrackerManager.h +++ b/src/psmoveservice/Device/Manager/TrackerManager.h @@ -85,6 +85,7 @@ class TrackerManager : public DeviceTypeManager TrackerManager(); bool startup() override; + void poll_devices(); void closeAllTrackers(); @@ -112,16 +113,26 @@ class TrackerManager : public DeviceTypeManager return cfg; } - inline static bool isTrackerSynced() + inline static bool trackersSynced() { - return m_isTrackerSycned; + return m_trackersSynced; } + inline static void setTrackFrameAvailable(int deviceId) + { + m_isTrackerFrameAvailable[deviceId] = true; + } + + inline static bool isReadyToReceive() + { + return m_readyToReceive; + } + + eCommonTrackingColorID allocateTrackingColorID(); bool claimTrackingColorID(const class ServerControllerView *controller_view, eCommonTrackingColorID color_id); bool claimTrackingColorID(const class ServerHMDView *hmd_view, eCommonTrackingColorID color_id); void freeTrackingColorID(eCommonTrackingColorID color_id); - bool TrackerManager::trackersSynced(); protected: bool can_update_connected_devices() override; @@ -137,7 +148,9 @@ class TrackerManager : public DeviceTypeManager TrackerManagerConfig cfg; bool m_tracker_list_dirty; std::chrono::time_point m_lastSync; - static bool m_isTrackerSycned; + static bool m_trackersSynced; + static bool m_isTrackerFrameAvailable[TrackerManager::k_max_devices]; + static bool m_readyToReceive; }; #endif // TRACKER_MANAGER_H \ No newline at end of file diff --git a/src/psmoveservice/Device/View/ServerControllerView.cpp b/src/psmoveservice/Device/View/ServerControllerView.cpp index 4ae8170c..40cb6226 100644 --- a/src/psmoveservice/Device/View/ServerControllerView.cpp +++ b/src/psmoveservice/Device/View/ServerControllerView.cpp @@ -895,7 +895,7 @@ void ServerControllerView::updateStateAndPredict() // Ship device id with the packet. We ned it for "OrientationExternal" filter. filter_packet.deviceId = this->getDeviceID(); - filter_packet.isSynced = TrackerManager::isTrackerSynced(); + filter_packet.isSynced = TrackerManager::trackersSynced(); // Create a filter input packet from the sensor data // and the filter's previous orientation and position diff --git a/src/psmoveservice/PSMoveTracker/PS3EyeTracker.cpp b/src/psmoveservice/PSMoveTracker/PS3EyeTracker.cpp index 9bf418fb..a5d94bb3 100644 --- a/src/psmoveservice/PSMoveTracker/PS3EyeTracker.cpp +++ b/src/psmoveservice/PSMoveTracker/PS3EyeTracker.cpp @@ -404,17 +404,33 @@ IDeviceInterface::ePollResult PS3EyeTracker::poll() if (getIsOpen()) { - if (!VideoCapture->grab() || - !TrackerManager::isTrackerSynced() || - !VideoCapture->retrieve(CaptureData->frame, cv::CAP_OPENNI_BGR_IMAGE)) + // Prepare frames whenever we can. + if (VideoCapture->grab()) { - // Device still in valid state - result = IControllerInterface::_PollResultSuccessNoData; + if ((bool)VideoCapture->get(CV_CAP_PROP_FRAMEAVAILABLE)) + { + TrackerManager::setTrackFrameAvailable(VideoCapture->getIndex()); + } + + // Only poll frames when every tracker is ready to sync freams. + if (!TrackerManager::isReadyToReceive() || + !VideoCapture->retrieve(CaptureData->frame, cv::CAP_OPENNI_BGR_IMAGE)) + { + // Device still in valid state + result = IControllerInterface::_PollResultSuccessNoData; + } + else + { + // New data available. Keep iterating. + result = IControllerInterface::_PollResultSuccessNewData; + + // We received the frame and every tracker polled. We need a new frame! + VideoCapture->set(CV_CAP_PROP_FRAMEAVAILABLE, false); + } } else { - // New data available. Keep iterating. - result = IControllerInterface::_PollResultSuccessNewData; + result = IControllerInterface::_PollResultSuccessNoData; } { diff --git a/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.cpp b/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.cpp index d66a13b3..04a1e24b 100644 --- a/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.cpp +++ b/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.cpp @@ -272,7 +272,7 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture { public: PSEYECaptureCAM_PS3EYE(int _index) - : m_index(-1), m_width(-1), m_height(-1), m_widthStep(-1), + : m_index(-1), m_width(-1), m_height(-1), m_widthStep(-1), m_frameAvailable(false), m_size(-1), m_MatBayer(0, 0, CV_8UC1) { //CoInitialize(NULL); @@ -309,6 +309,8 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture case CV_CAP_PROP_SHARPNESS: // [0, 63] -> [0, 255] return (double)(eye->getSharpness())*256.0 / 64.0; + case CV_CAP_PROP_FRAMEAVAILABLE: + return (bool)m_frameAvailable; } return 0; } @@ -365,6 +367,8 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture // [0, 255] -> [0, 63] [0] val = (int)(value * 64.0 / 256.0); eye->setSharpness((int)round(value)); + case CV_CAP_PROP_FRAMEAVAILABLE: + m_frameAvailable = (bool)value; } refreshDimensions(); @@ -374,14 +378,27 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture bool grabFrame() { - return eye->isStreaming(); + if (!eye->isStreaming()) + return false; + + if (m_frameAvailable) + return true; + + bool success = eye->getFrame(m_MatBayer.data); + if (!success) + return false; + + cv::cvtColor(m_MatBayer, capFrame, CV_BayerGB2BGR); + m_frameAvailable = true; + return true; } bool retrieveFrame(int outputType, cv::OutputArray outArray) { - eye->getFrame(m_MatBayer.data); + if (!m_frameAvailable) + return false; - cv::cvtColor(m_MatBayer, outArray, CV_BayerGB2BGR); + capFrame.copyTo(outArray); return true; } @@ -410,11 +427,12 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture return identifier; } - protected: bool open(int _index) { + capFrame = cv::Mat(480, 640, CV_8UC3, CvScalar(0, 0, 0)); + // Enumerate libusb devices std::vector devices = ps3eye::PS3EYECam::getDevices(); std::cout << "ps3eye::PS3EYECam::getDevices() found " << devices.size() << " devices." << std::endl; @@ -459,8 +477,11 @@ class PSEYECaptureCAM_PS3EYE : public cv::IVideoCapture } int m_index, m_width, m_height, m_widthStep; + bool m_frameAvailable; + size_t m_size; cv::Mat m_MatBayer; + cv::Mat capFrame; ps3eye::PS3EYECam::PS3EYERef eye; }; @@ -474,7 +495,7 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture { public: PSEYECaptureCAM_VIRTUAL(int _index) : - m_index(-1), + m_index(-1), m_frameAvailable(false), capturePipe(INVALID_HANDLE_VALUE) { //CoInitialize(NULL); @@ -511,6 +532,8 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture return 0; case CV_CAP_PROP_FORMAT: return cv::CAP_MODE_RGB; + case CV_CAP_PROP_FRAMEAVAILABLE: + return (bool)m_frameAvailable; } return 0; } @@ -518,19 +541,17 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture // Currently there is no way to get/set properties on virtual trackers. bool setProperty(int property_id, double value) { + switch (property_id) + { + case CV_CAP_PROP_FRAMEAVAILABLE: + m_frameAvailable = (bool)value; + } + return false; } bool grabFrame() { - const std::chrono::time_point now = std::chrono::high_resolution_clock::now(); - const std::chrono::duration timeSinceLast = now - m_lastRequest; - if (timeSinceLast.count() < 2.f) - { - return true; - } - - m_lastRequest = now; if (capturePipe == INVALID_HANDLE_VALUE) { @@ -545,6 +566,7 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture 2 ); + m_frameAvailable = true; return true; } @@ -577,6 +599,7 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture 2 ); + m_frameAvailable = true; return true; } @@ -623,18 +646,26 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture 2 ); + m_frameAvailable = true; return true; } return true; } + if (m_frameAvailable) + return true; + + memcpy((uchar*)capFrame.data, pipeBuffer, VRIT_BUFF_SIZE); + + m_frameAvailable = true; return true; } bool retrieveFrame(int outputType, cv::OutputArray outArray) { - memcpy((uchar*)capFrame.data, pipeBuffer, VRIT_BUFF_SIZE); + if (!m_frameAvailable) + return false; // ###Externet We need to flip the input image because PSEyes do so too. // If we dont do this the Y axis will be flipped on pose calibration. @@ -719,6 +750,7 @@ class PSEYECaptureCAM_VIRTUAL : public cv::IVideoCapture void refreshDimensions() {} int m_index; + bool m_frameAvailable; HANDLE capturePipe; char pipeBuffer[VRIT_BUFF_SIZE]; @@ -806,6 +838,7 @@ bool PSEyeVideoCapture::open(int index) #ifdef WIN32 icap = cv::makePtr(index); m_indentifier = icap.dynamicCast()->getUniqueIndentifier(); + m_index = index; return (!icap.empty()); #else @@ -905,6 +938,11 @@ std::string PSEyeVideoCapture::getUniqueIndentifier() const return m_indentifier; } +int PSEyeVideoCapture::getIndex() const +{ + return m_index; +} + cv::Ptr PSEyeVideoCapture::pseyeVideoCapture_create(int index) { // https://github.com/Itseez/opencv/blob/09e6c82190b558e74e2e6a53df09844665443d6d/modules/videoio/src/cap.cpp#L432 @@ -940,6 +978,7 @@ cv::Ptr PSEyeVideoCapture::pseyeVideoCapture_create(int index { capture = cv::makePtr(index); m_indentifier = capture.dynamicCast()->getUniqueIndentifier(); + m_index = index; } break; @@ -953,6 +992,7 @@ cv::Ptr PSEyeVideoCapture::pseyeVideoCapture_create(int index m_indentifier = "opencv_"; m_indentifier.append(std::to_string(index)); + m_index = index; return capture; } @@ -963,6 +1003,7 @@ cv::Ptr PSEyeVideoCapture::pseyeVideoCapture_create(int index { capture = cv::makePtr(index); m_indentifier = capture.dynamicCast()->getUniqueIndentifier(); + m_index = index; } break; #endif diff --git a/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.h b/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.h index e1909375..547ce995 100644 --- a/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.h +++ b/src/psmoveservice/PSMoveTracker/PSEye/PSEyeVideoCapture.h @@ -3,6 +3,11 @@ #include +enum +{ + CV_CAP_PROP_FRAMEAVAILABLE = -999, +}; + /// Video capture class that prioritizes PS3 Eye devices. /** Device opening priority: @@ -57,6 +62,8 @@ class PSEyeVideoCapture : public cv::VideoCapture { /// Use cv::VideoCapture::get() unless \ref eyeType == PSEYE_CLEYE_DRIVER double get(int propId) const override; + int getIndex() const; + /// Get the unique identifier for the camera std::string getUniqueIndentifier() const;