Improve tracker frame syncing

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);
 			}

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)
 }
 

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);
 			}

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<std::chrono::high_resolution_clock> now = std::chrono::high_resolution_clock::now();
-	const std::chrono::duration<double, std::milli> 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;
-}

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<std::chrono::high_resolution_clock> 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

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

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;
 		}
 
         {