pointcloud and video render processers added

src/dab_tracker_azure_camera.cpp

@@ -169,6 +169,24 @@ AzureCamera::init() throw (dab::Exception)
 	}
 }
 
+bool 
+AzureCamera::isColorMode() const
+{
+	return mColorMode;
+}
+
+bool 
+AzureCamera::isDepthMode() const
+{
+	return mDepthMode;
+}
+
+bool 
+AzureCamera::isPointCloudMode() const
+{
+	return mPointCloudMode;
+}
+
 ofxCvImage* 
 AzureCamera::colorFrame()
 {
@@ -187,6 +205,54 @@ AzureCamera::pointCloud()
 	return mPointCloud;
 }
 
+const k4a::image 
+AzureCamera::nativeColorFrame()
+{
+	mLock.lock();
+
+	k4a::image _nativeColorImage = mNativeColorImage;
+
+	mLock.unlock();
+
+	return _nativeColorImage;
+}
+
+const k4a::image 
+AzureCamera::nativeDepthFrame()
+{
+	mLock.lock();
+
+	k4a::image _nativeDepthImage = mNativeDepthImage;
+
+	mLock.unlock();
+
+	return _nativeDepthImage;
+}
+
+const k4a::image 
+AzureCamera::nativeDepthColorFrame()
+{
+	mLock.lock();
+
+	k4a::image _nativeColorDepthImage = mNativeDepthColorImage;
+
+	mLock.unlock();
+
+	return _nativeColorDepthImage;
+}
+
+const k4a::image 
+AzureCamera::nativePointCloudFrame()
+{
+	mLock.lock();
+
+	k4a::image _nativePointCloudImage = mNativePointCloudImage;
+
+	mLock.unlock();
+
+	return _nativePointCloudImage;
+}
+
 void 
 AzureCamera::setControlValue(const std::string& pControlName) throw (dab::Exception)
 {
@@ -232,6 +298,8 @@ AzureCamera::capture() throw (dab::Exception)
 {
 	try
 	{
+		mLock.lock();
+
 		AzureTools::get().capture(mSelf);
 
 		bool success;
@@ -271,9 +339,13 @@ AzureCamera::capture() throw (dab::Exception)
 
 			mCaptureStill = false;
 		}
+
+		mLock.unlock();
 	}
 	catch (dab::Exception& e)
 	{
+		mLock.unlock();
+
 		e += dab::Exception("TRACKER ERROR: failed to capture camera frame", __FILE__, __FUNCTION__, __LINE__);
 		throw e;
 	}

src/dab_tracker_azure_camera.h

@@ -3,6 +3,7 @@
 
 #include <iostream>
 #include <vector>
+#include <thread>
 #include <k4a/k4a.hpp>
 #include <k4arecord/playback.hpp>
 #include <k4arecord/record.hpp>
@@ -38,10 +39,19 @@ namespace dab
 
 			~AzureCamera();
 
+			bool isColorMode() const;
+			bool isDepthMode() const;
+			bool isPointCloudMode() const;
+
 			ofxCvImage* colorFrame();
 			ofxCvImage* depthFrame();
 			PointCloud* pointCloud();
 
+			const k4a::image nativeColorFrame();
+			const k4a::image nativeDepthFrame();
+			const k4a::image nativeDepthColorFrame();
+			const k4a::image nativePointCloudFrame();
+
 			void setControlValue(const std::string& pControlName) throw (dab::Exception);
 			void setControlValue(const std::string& pControlName, const AbstractValue& pValue) throw (dab::Exception);
 
@@ -68,7 +78,11 @@ namespace dab
 			// camera internals for transforming depth image into color image
 			k4a::calibration mDepthColorCalibration;
 			k4a::transformation mDepthColorTransformation;
-			k4a::image mDepthColorImage;
+
+			k4a::image mNativeColorImage;
+			k4a::image mNativeDepthImage;
+			k4a::image mNativeDepthColorImage;
+			k4a::image mNativePointCloudImage;
 
 			// camera frame
 			ofxCvColorImage* mColorFrame;
@@ -86,6 +100,8 @@ namespace dab
 			bool mDepthMode;
 			bool mPointCloudMode;
 
+			std::mutex mLock;
+
 			std::shared_ptr<AzureCamera> mSelf;
 
 			void init() throw (dab::Exception);

src/dab_tracker_azure_tools.cpp

@@ -72,6 +72,8 @@ AzureTools::capture(std::shared_ptr<AzureCamera> pCamera) throw (dab::Exception)
 		const k4a::image colorImage = pCamera->mNativeCapture.get_color_image();
 		if(colorImage.is_valid() == true)
 		{ 
+			pCamera->mNativeColorImage = colorImage;
+
 			try
 			{
 				updateColorFrame(pCamera, colorImage);
@@ -92,6 +94,8 @@ AzureTools::capture(std::shared_ptr<AzureCamera> pCamera) throw (dab::Exception)
 
 		if (depthImage.is_valid() == true)
 		{
+			pCamera->mNativeDepthImage = depthImage;
+
 			try
 			{
 				updateDepthFrame(pCamera, depthImage);
@@ -113,12 +117,22 @@ AzureTools::capture(std::shared_ptr<AzureCamera> pCamera) throw (dab::Exception)
 
 		if (colorImage.is_valid() == true && depthImage.is_valid() == true)
 		{
-			pCamera->mDepthColorTransformation.depth_image_to_color_camera(depthImage, &pCamera->mDepthColorImage);
+			pCamera->mNativeColorImage = colorImage;
+			pCamera->mNativeDepthImage = depthImage;
+
+			pCamera->mDepthColorTransformation.depth_image_to_color_camera(depthImage, &pCamera->mNativeDepthColorImage);
 
 			try
 			{
 				updateColorFrame(pCamera, colorImage);
-				updateDepthFrame(pCamera, pCamera->mDepthColorImage);
+				updateDepthFrame(pCamera, pCamera->mNativeDepthColorImage);
+
+				if (pCamera->mPointCloudMode == true)
+				{
+					pCamera->mDepthColorTransformation.depth_image_to_point_cloud(pCamera->mNativeDepthColorImage, K4A_CALIBRATION_TYPE_COLOR, &(pCamera->mNativePointCloudImage));
+
+					updatePointCloud(pCamera, pCamera->mNativePointCloudImage);
+				}
 
 				if (pCamera->mMovieRecording == true) pCamera->mNativeMovieRecorder.write_capture(pCamera->mNativeCapture);
 			}
@@ -246,6 +260,36 @@ AzureTools::updateDepthFrame(std::shared_ptr<AzureCamera> pCamera, const k4a::im
 	}
 }
 
+void
+AzureTools::updatePointCloud(std::shared_ptr<AzureCamera> pCamera, const k4a::image& pNativePointCloudFrame) throw (dab::Exception)
+{
+	PointCloud* pointCloud = pCamera->mPointCloud;
+
+	int frameWidth = pNativePointCloudFrame.get_width_pixels();
+	int frameHeight = pNativePointCloudFrame.get_height_pixels();
+	unsigned int pointCount = frameWidth * frameHeight;
+
+	if (pointCloud->size()[0] != pointCount) pointCloud->setSize({ pointCount });
+
+	std::vector<glm::vec3>& pointCloudPoints = pointCloud->points();
+	int16_t *pointCloudImageData = (int16_t *)(void *)pNativePointCloudFrame.get_buffer();
+
+	for (int pI = 0; pI < pointCount; ++pI)
+	{
+		glm::vec3 point;
+		point.x = pointCloudImageData[3 * pI + 0] / 1000.0f;
+		point.y = pointCloudImageData[3 * pI + 1] / 1000.0f;
+		point.z = pointCloudImageData[3 * pI + 2] / 1000.0f;
+
+		//if (point.z == 0)
+		//{
+		//	continue;
+		//}
+
+		pointCloudPoints[pI] = point;
+	}
+}
+
 void 
 AzureTools::startMovieRecording(std::shared_ptr<AzureCamera> pCamera, const std::string& pFileName) throw (dab::Exception)
 {
@@ -543,11 +587,6 @@ AzureTools::createCameraFrames(std::shared_ptr<AzureCamera> pCamera) throw (dab:
 					pCamera->mDepthFrame->allocate(frameSize[0], frameSize[1]);
 
 					pCamera->mFrameSize = frameSize;
-
-					if (pCamera->mPointCloudMode == true)
-					{
-						pCamera->mPointCloud = new PointCloud(frameSize);
-					}
 				}
 			}
 			else if(colorFrameSuccess == true)
@@ -567,7 +606,7 @@ AzureTools::createCameraFrames(std::shared_ptr<AzureCamera> pCamera) throw (dab:
 
 				pCamera->mDepthColorTransformation = k4a::transformation(pCamera->mDepthColorCalibration);
 
-				pCamera->mDepthColorImage = k4a::image::create(
+				pCamera->mNativeDepthColorImage = k4a::image::create(
 					K4A_IMAGE_FORMAT_DEPTH16,
 					frameSize[0],
 					frameSize[1],
@@ -580,7 +619,15 @@ AzureTools::createCameraFrames(std::shared_ptr<AzureCamera> pCamera) throw (dab:
 
 				if (pCamera->mPointCloudMode == true)
 				{
-					pCamera->mPointCloud = new PointCloud(frameSize);
+					std::cout << "create point cloud\n";
+
+					pCamera->mNativePointCloudImage = k4a::image::create(
+						K4A_IMAGE_FORMAT_CUSTOM,
+						frameSize[0],
+						frameSize[1],
+						frameSize[0] * 3 * (int)sizeof(int16_t));
+
+					pCamera->mPointCloud = new PointCloud({ frameSize[0] * frameSize[1] });
 				}
 
 				depthFrameSuccess = true;
@@ -649,6 +696,33 @@ AzureTools::initControls(std::shared_ptr<AzureCamera> pCamera) throw (dab::Excep
 		maxDepthControl->registerParameterListener(pCamera);
 		pCamera->mControls["maxdepth"] = maxDepthControl;
 	}
+
+	// Color Camera Controls
+	if (pCamera->mColorMode == true && pCamera->mMoviePlaying == false)
+	{
+		const k4a::image colorImage = pCamera->mNativeCapture.get_color_image();
+		if (colorImage.is_valid() == true)
+		{
+			// brightness
+			int brightness_value;
+			k4a_color_control_mode_t brightness_mode;
+
+			pCamera->mNativeDevice.get_color_control(K4A_COLOR_CONTROL_BRIGHTNESS, &brightness_mode, &brightness_value);
+			Parameter<int>* brightnessControl = new Parameter<int>("brightness", brightness_value, 0, 200);
+			brightnessControl->registerParameterListener(pCamera);
+			pCamera->mControls["brightness"] = brightnessControl;
+
+			// exposure
+			int exposure_value;
+			k4a_color_control_mode_t exposure_mode;
+
+			pCamera->mNativeDevice.get_color_control(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, &exposure_mode, &exposure_value);
+			Parameter<int>* exposureControl = new Parameter<int>("exposure", exposure_value, 0, 33330);
+			exposureControl->registerParameterListener(pCamera);
+			pCamera->mControls["exposure"] = exposureControl;
+		}
+
+	}
 }
 
 void 
@@ -668,6 +742,22 @@ AzureTools::setControlValue(const std::string& pControlName, const AbstractValue
 
 			pCamera->mMaxDepth = maxdepth;
 		}
+		else if (pControlName == "brightness")
+		{
+			int brightness_value = pValue.get<int>();
+			k4a_color_control_mode_t brightness_mode = K4A_COLOR_CONTROL_MODE_MANUAL;
+			std::cout << "brightness " << brightness_value << "\n";
+
+			pCamera->mNativeDevice.set_color_control(K4A_COLOR_CONTROL_BRIGHTNESS, brightness_mode, static_cast<int32_t>(brightness_value));
+		}
+		else if (pControlName == "exposure")
+		{
+			int exposure_value = pValue.get<int>();
+			k4a_color_control_mode_t exposure_mode = K4A_COLOR_CONTROL_MODE_MANUAL;
+			std::cout << "exposure " << exposure_value << "\n";
+
+			pCamera->mNativeDevice.set_color_control(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, exposure_mode, static_cast<int32_t>(exposure_value));
+		}
 	}
 	catch (Exception& e)
 	{

src/dab_tracker_azure_tools.h

@@ -54,7 +54,7 @@ namespace dab
 			void capture(std::shared_ptr<AzureCamera> pCamera) throw (dab::Exception);
 			void updateColorFrame(std::shared_ptr<AzureCamera> pCamera, const k4a::image& pNativeColorFrame) throw (dab::Exception);
 			void updateDepthFrame(std::shared_ptr<AzureCamera> pCamera, const k4a::image& pNativeDepthFrame) throw (dab::Exception);
-			//void updatePointCloud(std::shared_ptr<AzureCamera> pCamera, rs2::depth_frame& pNativeDepthFrame) throw (dab::Exception);
+			void updatePointCloud(std::shared_ptr<AzureCamera> pCamera, const k4a::image& pNativePointCloudFrame) throw (dab::Exception);
 
 			void startMovieRecording(std::shared_ptr<AzureCamera> pCamera, const std::string& pFileName) throw (dab::Exception);
 			void stopMovieRecording(std::shared_ptr<AzureCamera> pCamera) throw (dab::Exception);

src/dab_tracker_image_save_queue.cpp

@@ -81,7 +81,10 @@ ImageSaveQueue::saveImages()
 		ofxCvImage* _img = mImageQueue.front();
 		std::string _fileName = mFileNameQueue.front();
 
-		ofSaveImage(_img->getPixels(), _fileName);
+		bool success = ofSaveImage(_img->getPixels(), _fileName);
+
+		// very hacky, since deleting an image creates a memory leak, scaling the image down to mini size alleviates this problems somewhat
+		_img->resize(1, 1);
 
 		delete _img;
 

src/dab_tracker_optical_calibration.cpp

@@ -106,14 +106,14 @@ OpticalCalibration::distCoefficients() const
 	return _distCoefficients;
 }
 
-void
+void 
 OpticalCalibration::startCalibration()
 {
 	mImagePoints.clear();
 	mCalibrationMode = CALIBRATING;
 }
 
-void
+void 
 OpticalCalibration::captureCalibationPattern()
 {
 	if (mCalibrationMode != CALIBRATING) return;
@@ -555,12 +555,6 @@ OpticalCalibration::runCalibration()
 
     std::cout << (success ? "Calibration succeeded" : "Calibration failed")
     << ". avg re projection error = "  << totalAvgErr;
-
-	std::cout << "individual projection errors:\n";
-	for (int i = 0; i < reprojErrs.size(); ++i)
-	{
-		std::cout << "i " << i << " " << reprojErrs[i] << "\n";
-	}
 
     if(success == false) return false;
 

src/dab_tracker_optical_calibration.h

@@ -33,7 +33,7 @@ public:
 	void setDistCoefficients(const std::array<float, 8>& pDistCoefficients);
 
     void setupGui(ofxDatGui* pGui);
-    void update() throw (dab::Exception);
+	void update() throw (dab::Exception);
     void display() throw (dab::Exception);
     void saveSettings() throw (dab::Exception);
     void restoreSettings() throw (dab::Exception);