NHPCD - Network Hardware Point Cloud Decoder (proof-of-concept)

- working proof of concept for NHPCD
- some hardcoded data for Realsense D435

Proof of concept:
- to be turned into final clean code

Related to:
bmegli/unity-network-hardware-video-decoder#5

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "minimal-latency-streaming-protocol"]
 	path = minimal-latency-streaming-protocol
 	url = https://github.com/bmegli/minimal-latency-streaming-protocol.git
+[submodule "hardware-depth-unprojector"]
+	path = hardware-depth-unprojector
+	url = https://github.com/bmegli/hardware-depth-unprojector.git

CMakeLists.txt

@@ -10,16 +10,18 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 # compile dependencies as static libraries
 add_library(hvd hardware-video-decoder/hvd.c)
 add_library(mlsp minimal-latency-streaming-protocol/mlsp.c)
+add_library(hdu hardware-depth-unprojector/hdu.c)
 
 # this is our main target
 add_library(nhvd SHARED nhvd.cpp)
 target_include_directories(nhvd PRIVATE hardware-video-decoder)
 target_include_directories(nhvd PRIVATE minimal-latency-streaming-protocol)
+target_include_directories(nhvd PRIVATE hardware-depth-unprojector)
 
 # note that nhvd depends through hvd on FFMpeg avcodec and avutil, at least 3.4 version
-target_link_libraries(nhvd hvd mlsp avcodec avutil)
+target_link_libraries(nhvd hvd mlsp hdu avcodec avutil)
 
 add_executable(nhvd-receive-example examples/nhvd_receive_example.cpp)
-target_link_libraries(nhvd-receive-example nhvd)
+target_link_libraries(nhvd-receive-example nhvd hdu)
 
 

examples/nhvd_receive_example.cpp

@@ -46,6 +46,7 @@ const int TIMEOUT_MS=500; //timeout, accept new streaming sequence by receiver
 const int FRAMERATE = 30;
 const int SLEEP_US = 1000000/30;
 
+
 int main(int argc, char **argv)
 {
 	nhvd_hw_config hw_config= {HARDWARE, CODEC, DEVICE, PIXEL_FORMAT, WIDTH, HEIGHT, PROFILE};
@@ -72,30 +73,26 @@ void main_loop(nhvd *network_decoder)
 {
 	//this is where we will get the decoded data
 	nhvd_frame frame;
+	nhvd_point_cloud cloud;
 
 	bool keep_working=true;
 
 	while(keep_working)
 	{
-		if( nhvd_get_frame_begin(network_decoder, &frame) == NHVD_OK )
+		//add mechanism to prevent returning the same cloud over again
+		if( nhvd_get_point_cloud_begin(network_decoder, &cloud) == NHVD_OK )
 		{
-			//...
-			//do something with the:
-			// - frame.width
-			// - frame.height
-			// - frame.format
-			// - frame.data
-			// - frame.linesize
-			// be quick - we are holding the mutex
-			// Examples:
-			// - fill the texture
-			// - copy for later use if you can't be quick
-			//...
-			cout << endl << "decoded frame " << frame.width << "x" << frame.height << " format " << frame.format <<
-			" ls[0] " << frame.linesize[0] << " ls[1] " << frame.linesize[1]  << " ls[2]" << frame.linesize[2] << endl;
+//			cout << endl << "decoded frame " << frame.width << "x" << frame.height << " format " << frame.format <<
+//			" ls[0] " << frame.linesize[0] << " ls[1] " << frame.linesize[1]  << " ls[2]" << frame.linesize[2] << endl;
+
+			int u = cloud.used;
+			int m = cloud.used/2;
+			cout << "Unprojected points: " << u << endl;
+			cout << "[m]=[" << cloud.data[m][0] << "," << cloud.data[m][1] << "," << cloud.data[m][2] << "]"  << endl;
+		//	cout << "[used-1]=[" << cloud.data[u-1][0] << "," << cloud.data[u-1][1] << "," << cloud.data[u-1][2] << "]"  << endl;	
 		}
 
-		if( nhvd_get_frame_end(network_decoder) != NHVD_OK )
+		if( nhvd_get_point_cloud_end(network_decoder) != NHVD_OK )
 			break; //error occured
 
 		//this should spin once per frame rendering

nhvd.cpp

@@ -1,7 +1,7 @@
 /*
  * NHVD Network Hardware Video Decoder C++ library implementation
  *
- * Copyright 2019 (C) Bartosz Meglicki <[email protected]>
+ * Copyright 2020 (C) Bartosz Meglicki <[email protected]>
  *
  * This Source Code Form is subject to the terms of the Mozilla Public
  * License, v. 2.0. If a copy of the MPL was not distributed with this
@@ -15,18 +15,27 @@
 #include "mlsp.h"
 // Hardware Video Decoder library
 #include "hvd.h"
+// Hardware Depth Unprojector library
+#include "hdu.h"
 
 #include <thread>
 #include <mutex>
 #include <fstream>
 #include <iostream>
+#include <string.h> //memset
 
 using namespace std;
 
 static void nhvd_network_decoder_thread(nhvd *n);
 static int nhvd_decode_frame(nhvd *n, hvd_packet* packet);
 static nhvd *nhvd_close_and_return_null(nhvd *n);
 
+const float PPX=421.353;
+const float PPY=240.93;
+const float FX=426.768;
+const float FY=426.768;
+const float DEPTH_UNIT=0.0001;
+
 struct nhvd
 {
 	mlsp *network_streamer;
@@ -35,10 +44,18 @@ struct nhvd
 	AVFrame *frame;
 	std::mutex frame_mutex;
 
+	hdu *hardware_unprojector;
+	hdu_point_cloud point_cloud;
+
 	thread network_thread;
 	bool keep_working;
 
-	nhvd(): network_streamer(NULL), hardware_decoder(NULL), frame(NULL), keep_working(true) {}
+	nhvd(): network_streamer(NULL),
+			hardware_decoder(NULL),
+			frame(NULL),
+			hardware_unprojector(NULL),
+			keep_working(true)
+	{}
 };
 
 struct nhvd *nhvd_init(const nhvd_net_config *net_config,const nhvd_hw_config *hw_config)
@@ -71,6 +88,14 @@ struct nhvd *nhvd_init(const nhvd_net_config *net_config,const nhvd_hw_config *h
 	}
 	n->frame->data[0]=NULL;
 
+	if( (n->hardware_unprojector = hdu_init(PPX, PPY, FX, FY, DEPTH_UNIT)) == NULL )
+	{
+		cerr << "nhvd: failed to initialize hardware unprojector" << endl;
+		return nhvd_close_and_return_null(n);
+	}
+
+	n->point_cloud.data = NULL, n->point_cloud.size = 0, n->point_cloud.used = 0;
+
 	n->network_thread = thread(nhvd_network_decoder_thread, n);
 
 	return n;
@@ -123,9 +148,29 @@ static int nhvd_decode_frame(nhvd *n, hvd_packet* packet)
 
 	while( (frame = hvd_receive_frame(n->hardware_decoder, &error) ) != NULL )
 	{
-		std::lock_guard<std::mutex> frame_guard(n->frame_mutex);
+//		std::lock_guard<std::mutex> frame_guard(n->frame_mutex);
+//		av_frame_unref(n->frame);
+//		av_frame_ref(n->frame, frame);
 		av_frame_unref(n->frame);
 		av_frame_ref(n->frame, frame);
+
+		//guard the point cloud, not the frame
+		std::lock_guard<std::mutex> frame_guard(n->frame_mutex);
+		//unproject
+		int size = n->frame->width * n->frame->height;
+		if(size != n->point_cloud.size)
+		{
+			delete [] n->point_cloud.data;
+			n->point_cloud.data = new float3[size];
+			n->point_cloud.size = size;
+			n->point_cloud.used = 0;
+		}
+
+		hdu_depth depth = {(uint16_t*)n->frame->data[0], n->frame->width, n->frame->height, n->frame->linesize[0]};
+		//this could be moved to separate thread
+		hdu_unproject(n->hardware_unprojector, &depth, &n->point_cloud);
+		//temp
+		memset(n->point_cloud.data + n->point_cloud.used, 0, (n->point_cloud.size-n->point_cloud.used)*sizeof(n->point_cloud.data[0]));
 	}
 
 	if(error != HVD_OK)
@@ -170,6 +215,36 @@ int nhvd_get_frame_end(struct nhvd *n)
 	return NHVD_OK;
 }
 
+//NULL if there is no fresh data, non NULL otherwise
+int nhvd_get_point_cloud_begin(nhvd *n, nhvd_point_cloud *pc)
+{
+	if(n == NULL)
+		return NHVD_ERROR;
+
+	n->frame_mutex.lock();
+
+	//for user convinience, return ERROR if there is no data
+	if(n->point_cloud.data == NULL)
+		return NHVD_ERROR;
+
+	pc->data = n->point_cloud.data;
+	pc->size = n->point_cloud.size;
+	pc->used = n->point_cloud.used;
+
+	return NHVD_OK;
+
+}
+//returns HVE_OK on success, HVE_ERROR on fatal error
+int nhvd_get_point_cloud_end(nhvd *n)
+{
+	if(n == NULL)
+		return NHVD_ERROR;
+
+	n->frame_mutex.unlock();
+
+	return NHVD_OK;
+}
+
 static nhvd *nhvd_close_and_return_null(nhvd *n)
 {
 	nhvd_close(n);
@@ -187,6 +262,8 @@ void nhvd_close(nhvd *n)
 
 	mlsp_close(n->network_streamer);
 	hvd_close(n->hardware_decoder);
+	hdu_close(n->hardware_unprojector);
+	delete [] n->point_cloud.data;
 
 	av_frame_free(&n->frame);
 

nhvd.h

@@ -64,6 +64,15 @@ struct nhvd_frame
 	int linesize[NHVD_NUM_DATA_POINTERS];
 };
 
+typedef float float3[3];
+
+struct nhvd_point_cloud
+{
+	float3 *data;
+	int size;
+	int used;
+};
+
 enum nhvd_retval_enum
 {
 	NHVD_ERROR=-1, //!< error occured
@@ -76,10 +85,14 @@ NHVD_EXPORT NHVD_API void nhvd_close(nhvd *n);
 
 //NULL if there is no fresh data, non NULL otherwise
 NHVD_EXPORT NHVD_API int nhvd_get_frame_begin(nhvd *n, nhvd_frame *frame);
-
 //returns HVE_OK on success, HVE_ERROR on fatal error
 NHVD_EXPORT NHVD_API int nhvd_get_frame_end(nhvd *n);
 
+//NULL if there is no fresh data, non NULL otherwise
+NHVD_EXPORT NHVD_API int nhvd_get_point_cloud_begin(nhvd *n, nhvd_point_cloud *pc);
+//returns HVE_OK on success, HVE_ERROR on fatal error
+NHVD_EXPORT NHVD_API int nhvd_get_point_cloud_end(nhvd *n);
+
 }
 
 #endif