Implements #107 Stereo virtual camera

applications/reconstruct/src/splat_render.cpp

@@ -52,16 +52,41 @@ void Splatter::render(ftl::rgbd::Source *src, cudaStream_t stream) {
 	//LOG(INFO) << "Occupied: " << scene_->getOccupiedCount();
 
 	if (scene_->value("voxels", false)) {
+		// TODO:(Nick) Stereo for voxel version
 		ftl::cuda::isosurface_point_image(scene_->getHashData(), depth1_, params, stream);
 		ftl::cuda::splat_points(depth1_, depth2_, params, stream);
 		ftl::cuda::dibr(depth2_, colour1_, scene_->cameraCount(), params, stream);
+		src->writeFrames(colour1_, depth2_, stream);
 	} else {
 		//ftl::cuda::clear_colour(colour1_, stream);
 		ftl::cuda::clear_depth(depth1_, stream);
 		ftl::cuda::clear_depth(depth2_, stream);
 		ftl::cuda::dibr(depth1_, colour1_, scene_->cameraCount(), params, stream);
 		//ftl::cuda::hole_fill(depth1_, depth2_, params.camera, stream);
-		ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
+
+		// Splat the depth from first DIBR to expand the points
+		ftl::cuda::splat_points(depth1_, depth2_, params, stream);
+		// Use reverse sampling to obtain more colour details
+		// Should use nearest neighbor point depths to verify which camera to use
+		ftl::cuda::dibr(depth2_, colour1_, scene_->cameraCount(), params, stream);
+
+		if (src->getChannel() == ftl::rgbd::kChanDepth) {
+			ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
+			src->writeFrames(colour1_, depth2_, stream);
+		} else if (src->getChannel() == ftl::rgbd::kChanRight) {
+			Eigen::Affine3f transform(Eigen::Translation3f(camera.baseline,0.0f,0.0f));
+			Eigen::Matrix4f matrix =  src->getPose().cast<float>() * transform.matrix();
+			params.m_viewMatrix = MatrixConversion::toCUDA(matrix.inverse());
+			params.m_viewMatrixInverse = MatrixConversion::toCUDA(matrix);
+
+			ftl::cuda::clear_depth(depth1_, stream);
+			ftl::cuda::dibr(depth1_, colour2_, scene_->cameraCount(), params, stream);
+			src->writeFrames(colour1_, colour2_, stream);
+		} else {
+			src->writeFrames(colour1_, depth2_, stream);
+		}
+
+
 		//ftl::cuda::mls_resample(depth1_, colour1_, depth2_, scene_->getHashParams(), scene_->cameraCount(), params, stream);
 	}
 
@@ -69,8 +94,6 @@ void Splatter::render(ftl::rgbd::Source *src, cudaStream_t stream) {
 	//ftl::cuda::splat_points(depth1_, depth2_, params, stream);
 
 	// TODO: Second pass
-
-	src->writeFrames(colour1_, depth2_, stream);
 }
 
 void Splatter::setOutputDevice(int device) {

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

@@ -117,6 +117,7 @@ class Source : public ftl::Configurable {
 	void writeFrames(const cv::Mat &rgb, const cv::Mat &depth);
 	void writeFrames(const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uint> &depth, cudaStream_t stream);
 	void writeFrames(const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<float> &depth, cudaStream_t stream);
+	void writeFrames(const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uchar4> &rgb2, cudaStream_t stream);
 
 	int64_t timestamp() const { return timestamp_; }
 

components/rgbd-sources/src/source.cpp

@@ -278,6 +278,25 @@ void Source::writeFrames(const ftl::cuda::TextureObject<uchar4> &rgb, const ftl:
 	}
 }
 
+void Source::writeFrames(const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uchar4> &rgb2, cudaStream_t stream) {
+	if (!impl_) {
+		UNIQUE_LOCK(mutex_,lk);
+		rgb.download(rgb_, stream);
+		//rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
+		//cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
+		rgb2.download(depth_, stream);
+		//depth_.create(depth.height(), depth.width(), CV_32FC1);
+		//cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(float), depth_.rows, cudaMemcpyDeviceToHost, stream));
+		
+		stream_ = stream;
+		cudaSafeCall(cudaStreamSynchronize(stream_));
+		cv::cvtColor(rgb_,rgb_, cv::COLOR_BGRA2BGR);
+		cv::cvtColor(rgb_,rgb_, cv::COLOR_Lab2BGR);
+		cv::cvtColor(depth_,depth_, cv::COLOR_BGRA2BGR);
+		cv::cvtColor(depth_,depth_, cv::COLOR_Lab2BGR);
+	}
+}
+
 bool Source::thumbnail(cv::Mat &t) {
 	if (!impl_ && stream_ != 0) {
 		cudaSafeCall(cudaStreamSynchronize(stream_));