Merge branch 'master' of gitlab.utu.fi:nicolas.pope/ftl

applications/reconstruct/include/ftl/ray_cast_sdf.hpp

@@ -42,7 +42,7 @@ public:
 		return params;
 	}
 
-	void render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform);
+	void render(ftl::voxhash::HashData& hashData, ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform);
 
 	const RayCastData& getRayCastData(void) {
 		return m_data;
@@ -60,6 +60,8 @@ private:
 	void create(const RayCastParams& params);
 	void destroy(void);
 
+	void compactifyHashEntries(ftl::voxhash::HashData& hashData, ftl::voxhash::HashParams& hashParams);
+
 	void rayIntervalSplatting(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const Eigen::Matrix4f& lastRigidTransform); // rasterize
 
 	RayCastParams m_params;

applications/reconstruct/include/ftl/scene_rep_hash_sdf.hpp

@@ -98,7 +98,7 @@ class SceneRep : public ftl::Configurable {
 		alloc(depthCameraData, depthCameraParams, d_bitMask);
 
 		//generate a linear hash array with only occupied entries
-		compactifyHashEntries(depthCameraData);
+		compactifyHashEntries();
 
 		//volumetrically integrate the depth data into the depth SDFBlocks
 		integrateDepthMap(depthCameraData, depthCameraParams);
@@ -115,7 +115,7 @@ class SceneRep : public ftl::Configurable {
 
 	void setLastRigidTransformAndCompactify(const Eigen::Matrix4f& lastRigidTransform, const DepthCameraData& depthCameraData) {
 		setLastRigidTransform(lastRigidTransform);
-		compactifyHashEntries(depthCameraData);
+		compactifyHashEntries();
 	}
 
 
@@ -145,7 +145,7 @@ class SceneRep : public ftl::Configurable {
 		return m_hashData;
 	} 
 
-	const HashParams& getHashParams() const {
+	HashParams& getHashParams() {
 		return m_hashParams;
 	}
 
@@ -312,7 +312,7 @@ private:
 	}
 
 
-	void compactifyHashEntries(const DepthCameraData& depthCameraData) {
+	void compactifyHashEntries() { //const DepthCameraData& depthCameraData) {
 		//Start Timing
 		//if(GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.start(); }
 

applications/reconstruct/src/ray_cast_sdf.cpp

@@ -36,7 +36,17 @@ void CUDARayCastSDF::destroy(void)
 	//m_rayIntervalSplatting.OnD3D11DestroyDevice();
 }
 
-void CUDARayCastSDF::render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform)
+extern "C" unsigned int compactifyHashAllInOneCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
+
+
+void CUDARayCastSDF::compactifyHashEntries(ftl::voxhash::HashData& hashData, ftl::voxhash::HashParams& hashParams) { //const DepthCameraData& depthCameraData) {
+		
+	hashParams.m_numOccupiedBlocks = compactifyHashAllInOneCUDA(hashData, hashParams);		//this version uses atomics over prefix sums, which has a much better performance
+	std::cout << "Ray blocks = " << hashParams.m_numOccupiedBlocks << std::endl;
+	hashData.updateParams(hashParams);	//make sure numOccupiedBlocks is updated on the GPU
+}
+
+void CUDARayCastSDF::render(ftl::voxhash::HashData& hashData, ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform)
 {
 	updateConstantDepthCameraParams(cameraParams);
 	//rayIntervalSplatting(hashData, hashParams, lastRigidTransform);
@@ -48,6 +58,8 @@ void CUDARayCastSDF::render(const ftl::voxhash::HashData& hashData, const ftl::v
 	m_params.m_viewMatrixInverse = MatrixConversion::toCUDA(lastRigidTransform);
 	m_data.updateParams(m_params);
 
+	compactifyHashEntries(hashData, hashParams);
+
 	if (hash_render_) nickRenderCUDA(hashData, hashParams, m_data, m_params);
 	else renderCS(hashData, m_data, m_params);
 

applications/reconstruct/src/ray_cast_sdf.cu

@@ -298,6 +298,8 @@ __global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData ra
 	const float2 screenPosf = DepthCameraData::cameraToKinectScreenFloat(camPos);
 	const uint2 screenPos = make_uint2(make_int2(screenPosf)); //  + make_float2(0.5f, 0.5f)
 
+	if (camPos.z < 0.0f) return;
+
 	/*if (screenPos.x < params.m_width && screenPos.y < params.m_height && 
 			rayCastData.d_depth[(screenPos.y)*params.m_width+screenPos.x] > camPos.z) {
 		rayCastData.d_depth[(screenPos.y)*params.m_width+screenPos.x] = camPos.z;

applications/reconstruct/src/registration.cpp

@@ -454,6 +454,7 @@ bool ChessboardRegistration::findFeatures(Source *source, size_t idx) {
 	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
 
 	Mat rgb, depth;
+	source->grab();
 	source->getFrames(rgb, depth);
 
 	bool retval = findChessboardCorners(rgb, depth, source->parameters(), pattern_size_, cloud, error_threshold_);

components/net/cpp/src/peer.cpp

@@ -113,6 +113,7 @@ static SOCKET tcpConnect(URI &uri) {
 
 	if (rc < 0) {
 		if (errno == EINPROGRESS) {
+			// TODO(Nick) Move to main select thread to prevent blocking
 			fd_set myset; 
 			struct timeval tv;
 			tv.tv_sec = 1; 
@@ -389,34 +390,41 @@ void Peer::error(int e) {
 
 void Peer::data() {
 	//if (!is_waiting_) return;
-	is_waiting_ = false;
+	//is_waiting_ = false;
+	std::unique_lock<std::recursive_mutex> lk(recv_mtx_);
+	recv_buf_.reserve_buffer(kMaxMessage);
+
+	if (recv_buf_.buffer_capacity() < (kMaxMessage / 10)) {
+		LOG(WARNING) << "Net buffer at capacity";
+		return;
+	}
+
+	int rc = ftl::net::internal::recv(sock_, recv_buf_.buffer(), recv_buf_.buffer_capacity(), 0);
+
+	if (rc <= 0) {
+		return;
+	}
+	
+	recv_buf_.buffer_consumed(rc);
+	lk.unlock();
+
 	pool.push([](int id, Peer *p) {
 		p->_data();
-		p->is_waiting_ = true;
+		//p->is_waiting_ = true;
 	}, this);
 }
 
-/*inline std::ostream& hex_dump(std::ostream& o, std::string const& v) {
-    std::ios::fmtflags f(o.flags());
-    o << std::hex;
-    for (auto c : v) {
-        o << "0x" << std::setw(2) << std::setfill('0') << (static_cast<int>(c) & 0xff) << ' ';
-    }
-    o.flags(f);
-    return o;
-}*/
-
 bool Peer::_data() {
 	std::unique_lock<std::recursive_mutex> lk(recv_mtx_);
 
-	recv_buf_.reserve_buffer(kMaxMessage);
+	/*recv_buf_.reserve_buffer(kMaxMessage);
 	int rc = ftl::net::internal::recv(sock_, recv_buf_.buffer(), kMaxMessage, 0);
 
 	if (rc <= 0) {
 		return false;
 	}
 	
-	recv_buf_.buffer_consumed(rc);
+	recv_buf_.buffer_consumed(rc);*/
 
 	if (scheme_ == ftl::URI::SCHEME_WS && !ws_read_header_) {
 		wsheader_type ws;
@@ -426,11 +434,6 @@ bool Peer::_data() {
 		ws_read_header_ = true;
 	}
 
-	/*if (rc > 0) {
-		hex_dump(std::cout, std::string((char*)recv_buf_.nonparsed_buffer(), recv_buf_.nonparsed_size()));
-		std::cout << std::endl;
-	}*/
-
 	msgpack::object_handle msg;
 	while (recv_buf_.next(msg)) {
 		ws_read_header_ = false;