From 1dadb03e8e6fb404a1cc302c9223ea8e9a0024a2 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Mon, 16 Jun 2014 18:18:52 +0000
Subject: [PATCH] refactored how freenect ready is done

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1391 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 corelib/src/CameraRGBD.cpp | 35 +++++++++++++----------------------
 1 file changed, 13 insertions(+), 22 deletions(-)

diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp
index 78126a7a..d835d7a4 100644
--- a/corelib/src/CameraRGBD.cpp
+++ b/corelib/src/CameraRGBD.cpp
@@ -568,22 +568,12 @@ class FreenectDevice : public UThread {
 		if(this->isRunning())
 		{
 			dataReady_.acquire();
-			{
-				UScopeMutex s1(rgbMutex_);
-				if(!rgbLastFrame_.empty())
-				{
-					rgb = rgbLastFrame_;
-					rgbLastFrame_ = cv::Mat();
-				}
-			}
-			{
-				UScopeMutex s2(depthMutex_);
-				if(!depthLastFrame_.empty())
-				{
-					depth = depthLastFrame_;
-					depthLastFrame_= cv::Mat();
-				}
-			}
+
+			UScopeMutex s(dataMutex_);
+			rgb = rgbLastFrame_;
+			depth = depthLastFrame_;
+			rgbLastFrame_ = cv::Mat();
+			depthLastFrame_= cv::Mat();
 		}
 	}
 
@@ -592,9 +582,10 @@ private:
 	void VideoCallback(void* rgb)
 	{
 		UASSERT(rgbBuffer_.data == rgb);
-		UScopeMutex s1(rgbMutex_);
+		UScopeMutex s(dataMutex_);
+		bool notify = rgbLastFrame_.empty();
 		cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
-		if(!depthLastFrame_.empty() && dataReady_.value() <= 0)
+		if(!depthLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
 		}
@@ -604,9 +595,10 @@ private:
 	void DepthCallback(void* depth)
 	{
 		UASSERT(depthBuffer_.data == depth);
-		UScopeMutex s2(depthMutex_);
+		UScopeMutex s(dataMutex_);
+		bool notify = depthLastFrame_.empty();
 		depthLastFrame_ = depthBuffer_.clone();
-		if(!rgbLastFrame_.empty() && dataReady_.value() <= 0)
+		if(!rgbLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
 		}
@@ -669,8 +661,7 @@ private:
 	freenect_device * device_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
-	UMutex depthMutex_;
-	UMutex rgbMutex_;
+	UMutex dataMutex_;
 	cv::Mat depthLastFrame_;
 	cv::Mat rgbLastFrame_;
 	float depthFocal_;