Use normals in MLS, partial #141

applications/gui/src/camera.cpp

@@ -222,6 +222,9 @@ void ftl::gui::Camera::showSettings() {
 void ftl::gui::Camera::setChannel(ftl::rgbd::channel_t c) {
 	channel_ = c;
 	switch (c) {
+	case ftl::rgbd::kChanFlow:
+	case ftl::rgbd::kChanConfidence:
+	case ftl::rgbd::kChanNormals:
 	case ftl::rgbd::kChanRight:
 		src_->setChannel(c);
 		break;
@@ -293,6 +296,9 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
 				texture_.update(tmp);
 				break;
 
+		case ftl::rgbd::kChanFlow:
+		case ftl::rgbd::kChanConfidence:
+		case ftl::rgbd::kChanNormals:
 			case ftl::rgbd::kChanRight:
 				if (depth.rows == 0 || depth.type() != CV_8UC3) { break; }
 				texture_.update(depth);

applications/gui/src/media_panel.cpp

@@ -110,7 +110,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
     Popup *popup = popbutton->popup();
     popup->setLayout(new GroupLayout());
 	popup->setTheme(screen->toolbuttheme);
-    popup->setAnchorHeight(100);
+    popup->setAnchorHeight(150);
 
     button = new Button(popup, "Left");
     button->setFlags(Button::RadioButton);
@@ -140,6 +140,14 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
         }
     });
 
+	popbutton = new PopupButton(popup, "More");
+    popbutton->setSide(Popup::Side::Right);
+	popbutton->setChevronIcon(ENTYPO_ICON_CHEVRON_SMALL_RIGHT);
+    popup = popbutton->popup();
+    popup->setLayout(new GroupLayout());
+	popup->setTheme(screen->toolbuttheme);
+    popup->setAnchorHeight(150);
+
     button = new Button(popup, "Deviation");
     button->setFlags(Button::RadioButton);
     button->setCallback([this]() {
@@ -148,6 +156,34 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
             cam->setChannel(ftl::rgbd::kChanDeviation);
         }
     });
+
+	button = new Button(popup, "Normals");
+    button->setFlags(Button::RadioButton);
+    button->setCallback([this]() {
+        ftl::gui::Camera *cam = screen_->activeCamera();
+        if (cam) {
+            cam->setChannel(ftl::rgbd::kChanNormals);
+        }
+    });
+
+	button = new Button(popup, "Flow");
+    button->setFlags(Button::RadioButton);
+    button->setCallback([this]() {
+        ftl::gui::Camera *cam = screen_->activeCamera();
+        if (cam) {
+            cam->setChannel(ftl::rgbd::kChanFlow);
+        }
+    });
+
+	button = new Button(popup, "Confidence");
+    button->setFlags(Button::RadioButton);
+    button->setCallback([this]() {
+        ftl::gui::Camera *cam = screen_->activeCamera();
+        if (cam) {
+            cam->setChannel(ftl::rgbd::kChanConfidence);
+        }
+    });
+
 }
 
 MediaPanel::~MediaPanel() {

applications/reconstruct/src/depth_camera.cpp

+#include <loguru.hpp>
 #include <ftl/depth_camera.hpp>
 #include "depth_camera_cuda.hpp"
 #include <opencv2/core/cuda_stream_accessor.hpp>
@@ -24,12 +25,12 @@ void DepthCamera::alloc(const DepthCameraParams& params, bool withNormals) { //!
 	data.colour = colour_tex_->cudaTexture();
 	data.params = params;
 
-	if (withNormals) {
+	//if (withNormals) {
 		normal_tex_ = new ftl::cuda::TextureObject<float4>(params.m_imageWidth, params.m_imageHeight);
 		data.normal = normal_tex_->cudaTexture();
-	} else {
-		data.normal = 0;
-	}
+	//} else {
+	//	data.normal = 0;
+	//}
 }
 
 void DepthCamera::free() {
@@ -44,11 +45,11 @@ void DepthCamera::updateData(const cv::Mat &depth, const cv::Mat &rgb, cv::cuda:
 	depth_tex_->upload(depth, cv::cuda::StreamAccessor::getStream(stream));
 	colour_tex_->upload(rgb, cv::cuda::StreamAccessor::getStream(stream));
 	//if (normal_mat_) {
-	//	_computeNormals(cv::cuda::StreamAccessor::getStream(stream));
+		_computeNormals(cv::cuda::StreamAccessor::getStream(stream));
 	//}
 }
 
 void DepthCamera::_computeNormals(cudaStream_t stream) {
-	//ftl::cuda::point_cloud((float3*)point_mat_->data, data, stream);
-	//ftl::cuda::compute_normals((float3*)point_mat_->data, normal_tex_, stream);
+	ftl::cuda::point_cloud(*points_tex_, data, stream);
+	ftl::cuda::compute_normals(*points_tex_, *normal_tex_, stream);
 }

applications/reconstruct/src/depth_camera.cu

@@ -176,7 +176,7 @@ __device__ float mlsCamera(int cam, const float3 &mPos, uchar4 c1, float3 &wpos)
 	return weights;
 }
 
-__device__ float mlsCameraNoColour(int cam, const float3 &mPos, uchar4 c1, float3 &wpos, float h) {
+__device__ float mlsCameraNoColour(int cam, const float3 &mPos, uchar4 c1, const float4 &norm, float3 &wpos, float h) {
 	const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
 
 	const float3 pf = camera.poseInverse * mPos;
@@ -199,11 +199,15 @@ __device__ float mlsCameraNoColour(int cam, const float3 &mPos, uchar4 c1, float
 				float weight = spatialWeighting(length(pf - camPos), h);
 
 				if (weight > 0.0f) {
-					uchar4 c2 = tex2D<uchar4>(camera.colour, screenPos.x+u, screenPos.y+v);
+					float4 n2 = tex2D<float4>(camera.normal, screenPos.x+u, screenPos.y+v);
+					if (dot(make_float3(norm), make_float3(n2)) > 0.0f) {
 
-					if (colourWeighting(colordiffFloat2(c1,c2)) > 0.0f) {
-						pos += weight*camPos; // (camera.pose * camPos);
-						weights += weight;
+						uchar4 c2 = tex2D<uchar4>(camera.colour, screenPos.x+u, screenPos.y+v);
+
+						if (colourWeighting(colordiffFloat2(c1,c2)) > 0.0f) {
+							pos += weight*camPos; // (camera.pose * camPos);
+							weights += weight;
+						}
 					}
 				}			
 			//}
@@ -286,6 +290,8 @@ __global__ void mls_smooth_kernel(ftl::cuda::TextureObject<float4> output, HashP
 
 		const float depth = tex2D<float>(mainCamera.depth, x, y);
 		const uchar4 c1 = tex2D<uchar4>(mainCamera.colour, x, y);
+		const float4 norm = tex2D<float4>(mainCamera.normal, x, y);
+		//if (x == 400 && y == 200) printf("NORMX: %f\n", norm.x);
 
 		float3 wpos = make_float3(0.0f);
 		float3 wnorm = make_float3(0.0f);
@@ -301,7 +307,7 @@ __global__ void mls_smooth_kernel(ftl::cuda::TextureObject<float4> output, HashP
 				if (hashParams.m_flags & ftl::voxhash::kFlagMLS) {
 					for (uint cam2=0; cam2<numcams; ++cam2) {
 						//if (cam2 == cam) weights += mlsCameraNoColour(cam2, mPos, c1, wpos, c_hashParams.m_spatialSmoothing*0.1f); //weights += 0.5*mlsCamera(cam2, mPos, c1, wpos);
-						weights += mlsCameraNoColour(cam2, mPos, c1, wpos, c_hashParams.m_spatialSmoothing); //*((cam == cam2)? 0.1f : 5.0f));
+						weights += mlsCameraNoColour(cam2, mPos, c1, norm, wpos, c_hashParams.m_spatialSmoothing); //*((cam == cam2)? 0.1f : 5.0f));
 
 						// Previous approach
 						//if (cam2 == cam) continue;
@@ -567,7 +573,7 @@ void ftl::cuda::hole_fill(const TextureObject<int> &depth_in,
 
 /// ===== Point cloud from depth =====
 
-__global__ void point_cloud_kernel(float3* output, DepthCameraCUDA depthCameraData)
+__global__ void point_cloud_kernel(ftl::cuda::TextureObject<float4> output, DepthCameraCUDA depthCameraData)
 {
 	const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
 	const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
@@ -578,13 +584,13 @@ __global__ void point_cloud_kernel(float3* output, DepthCameraCUDA depthCameraDa
 	if (x < width && y < height) {
 		float depth = tex2D<float>(depthCameraData.depth, x, y);
 
-		output[y*width+x] = (depth >= depthCameraData.params.m_sensorDepthWorldMin && depth <= depthCameraData.params.m_sensorDepthWorldMax) ?
-			depthCameraData.params.kinectDepthToSkeleton(x, y, depth) :
-			make_float3(MINF, MINF, MINF);
+		output(x,y) = (depth >= depthCameraData.params.m_sensorDepthWorldMin && depth <= depthCameraData.params.m_sensorDepthWorldMax) ?
+			make_float4(depthCameraData.pose * depthCameraData.params.kinectDepthToSkeleton(x, y, depth), 0.0f) :
+			make_float4(MINF, MINF, MINF, MINF);
 	}
 }
 
-void ftl::cuda::point_cloud(float3* output, const DepthCameraCUDA &depthCameraData, cudaStream_t stream) {
+void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, const DepthCameraCUDA &depthCameraData, cudaStream_t stream) {
 	const dim3 gridSize((depthCameraData.params.m_imageWidth + T_PER_BLOCK - 1)/T_PER_BLOCK, (depthCameraData.params.m_imageHeight + T_PER_BLOCK - 1)/T_PER_BLOCK);
 	const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
@@ -598,7 +604,7 @@ void ftl::cuda::point_cloud(float3* output, const DepthCameraCUDA &depthCameraDa
 /// ===== NORMALS =====
 
 
-__global__ void compute_normals_kernel(const float3 *input, ftl::cuda::TextureObject<float4> output)
+__global__ void compute_normals_kernel(const ftl::cuda::TextureObject<float4> input, ftl::cuda::TextureObject<float4> output)
 {
 	const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
 	const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
@@ -611,31 +617,31 @@ __global__ void compute_normals_kernel(const float3 *input, ftl::cuda::TextureOb
 
 	if(x > 0 && x < output.width()-1 && y > 0 && y < output.height()-1)
 	{
-		// TODO:(Nick) Should use a 7x7 window
-		const float3 CC = input[(y+0)*width+(x+0)];
-		const float3 PC = input[(y+1)*width+(x+0)];
-		const float3 CP = input[(y+0)*width+(x+1)];
-		const float3 MC = input[(y-1)*width+(x+0)];
-		const float3 CM = input[(y+0)*width+(x-1)];
+		const float3 CC = make_float3(input(x,y)); //input[(y+0)*width+(x+0)];
+		const float3 PC = make_float3(input(x,y+1)); //input[(y+1)*width+(x+0)];
+		const float3 CP = make_float3(input(x+1,y)); //input[(y+0)*width+(x+1)];
+		const float3 MC = make_float3(input(x,y-1)); //input[(y-1)*width+(x+0)];
+		const float3 CM = make_float3(input(x-1,y)); //input[(y+0)*width+(x-1)];
 
 		if(CC.x != MINF && PC.x != MINF && CP.x != MINF && MC.x != MINF && CM.x != MINF)
 		{
 			const float3 n = cross(PC-MC, CP-CM);
-			//const float  l = length(n);
+			const float  l = length(n);
 
-			//if(l > 0.0f)
-			//{
-				output(x,y) = make_float4(n, 1.0f); //make_float4(n/-l, 1.0f);
-			//}
+			if(l > 0.0f)
+			{
+				//if (x == 400 && y == 200) printf("Cam NORMX: %f\n", (n/-l).x);
+				output(x,y) = make_float4(n.x/-l, n.y/-l, n.z/-l, 0.0f); //make_float4(n/-l, 1.0f);
+			}
 		}
 	}
 }
 
-void ftl::cuda::compute_normals(const float3 *input, ftl::cuda::TextureObject<float4> *output, cudaStream_t stream) {
-	const dim3 gridSize((output->width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (output->height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+void ftl::cuda::compute_normals(const ftl::cuda::TextureObject<float4> &input, ftl::cuda::TextureObject<float4> &output, cudaStream_t stream) {
+	const dim3 gridSize((output.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (output.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
 	const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
-	compute_normals_kernel<<<gridSize, blockSize, 0, stream>>>(input, *output);
+	compute_normals_kernel<<<gridSize, blockSize, 0, stream>>>(input, output);
 
 #ifdef _DEBUG
 	cudaSafeCall(cudaDeviceSynchronize());

applications/reconstruct/src/depth_camera_cuda.hpp

@@ -25,9 +25,9 @@ void mls_resample(const TextureObject<int> &depthin, const TextureObject<uchar4>
 
 void hole_fill(const TextureObject<int> &depth_in, const TextureObject<float> &depth_out, const DepthCameraParams &params, cudaStream_t stream);
 
-void point_cloud(float3* output, const ftl::voxhash::DepthCameraCUDA &depthCameraData, cudaStream_t stream);
+void point_cloud(ftl::cuda::TextureObject<float4> &output, const ftl::voxhash::DepthCameraCUDA &depthCameraData, cudaStream_t stream);
 
-void compute_normals(const float3 *points, ftl::cuda::TextureObject<float4> *normals, cudaStream_t stream);
+void compute_normals(const ftl::cuda::TextureObject<float4> &points, ftl::cuda::TextureObject<float4> &normals, cudaStream_t stream);
 
 }
 }

applications/reconstruct/src/voxel_scene.cpp

@@ -105,9 +105,9 @@ extern "C" void updateCUDACameraConstant(ftl::voxhash::DepthCameraCUDA *data, in
 void SceneRep::_updateCameraConstant() {
 	std::vector<ftl::voxhash::DepthCameraCUDA> cams(cameras_.size());
 	for (size_t i=0; i<cameras_.size(); ++i) {
+		cameras_[i].gpu.data.pose = MatrixConversion::toCUDA(cameras_[i].source->getPose().cast<float>());
+		cameras_[i].gpu.data.poseInverse = MatrixConversion::toCUDA(cameras_[i].source->getPose().cast<float>().inverse());
 		cams[i] = cameras_[i].gpu.data;
-		cams[i].pose = MatrixConversion::toCUDA(cameras_[i].source->getPose().cast<float>());
-		cams[i].poseInverse = MatrixConversion::toCUDA(cameras_[i].source->getPose().cast<float>().inverse());
 	}
 	updateCUDACameraConstant(cams.data(), cams.size());
 }

components/rgbd-sources/include/ftl/rgbd/detail/source.hpp

@@ -18,15 +18,25 @@ static const channel_t kChanDepth = 0x0002;
 static const channel_t kChanRight = 0x0004;
 static const channel_t kChanDisparity = 0x0008;
 static const channel_t kChanDeviation = 0x0010;
+static const channel_t kChanNormals = 0x0020;
+static const channel_t kChanConfidence = 0x0040;
+static const channel_t kChanFlow = 0x0080;
 
 static const channel_t kChanOverlay1 = 0x1000;
 
+inline bool isFloatChannel(ftl::rgbd::channel_t chan) {
+	return (chan == ftl::rgbd::kChanDepth);
+}
+
+
 typedef unsigned int capability_t;
 
 static const capability_t kCapMovable	= 0x0001;	// A movable virtual cam
 static const capability_t kCapVideo		= 0x0002;	// Is a video feed
 static const capability_t kCapActive	= 0x0004;	// An active depth sensor
-static const capability_t kCapStereo	= 0x0005;	// Has right RGB
+static const capability_t kCapStereo	= 0x0008;	// Has right RGB
+static const capability_t kCapDepth		= 0x0010;	// Has depth capabilities
+
 
 namespace detail {
 

components/rgbd-sources/src/net.cpp

@@ -295,9 +295,9 @@ bool NetSource::grab(int n, int b) {
 		N_ = maxN_;
 
 		// Verify depth destination is of required type
-		if (chan == ftl::rgbd::kChanDepth && depth_.type() != CV_32F) {
+		if (isFloatChannel(chan) && depth_.type() != CV_32F) {
 			depth_ = cv::Mat(cv::Size(params_.width, params_.height), CV_32FC1, 0.0f);
-		} else if (chan == ftl::rgbd::kChanRight && depth_.type() != CV_8UC3) {
+		} else if (!isFloatChannel(chan) && depth_.type() != CV_8UC3) {
 			depth_ = cv::Mat(cv::Size(params_.width, params_.height), CV_8UC3, cv::Scalar(0,0,0));
 		}
 

components/rgbd-sources/src/streamer.cpp

@@ -491,11 +491,11 @@ void Streamer::_encodeChannel1(const cv::Mat &in, vector<unsigned char> &out, un
 bool Streamer::_encodeChannel2(const cv::Mat &in, vector<unsigned char> &out, ftl::rgbd::channel_t c, unsigned int b) {
 	if (c == ftl::rgbd::kChanNone) return false;  // NOTE: Should not happen
 
-	if (c == ftl::rgbd::kChanDepth && in.type() == CV_16U && in.channels() == 1) {
+	if (isFloatChannel(c) && in.type() == CV_16U && in.channels() == 1) {
 		vector<int> params = {cv::IMWRITE_PNG_COMPRESSION, bitrate_settings[b].png_compression};
 		cv::imencode(".png", in, out, params);
 		return true;
-	} else if (c == ftl::rgbd::kChanRight && in.type() == CV_8UC3) {
+	} else if (!isFloatChannel(c) && in.type() == CV_8UC3) {
 		vector<int> params = {cv::IMWRITE_JPEG_QUALITY, bitrate_settings[b].jpg_quality};
 		cv::imencode(".jpg", in, out, params);
 		return true;