Feature/voxelhash

applications/vision/src/main.cpp

@@ -52,6 +52,31 @@ using cv::Mat;
 using json = nlohmann::json;
 using ftl::config;
 
+namespace ftl {
+void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv::Mat &q) {
+	cv::Matx44d _Q;
+    q.convertTo(_Q, CV_64F);
+
+	if (depth.empty()) depth = cv::Mat(disparity.size(), CV_32F);
+
+	for( int y = 0; y < disparity.rows; y++ ) {
+		const float *sptr = disparity.ptr<float>(y);
+		float *dptr = depth.ptr<float>(y);
+
+		for( int x = 0; x < disparity.cols; x++ ) {
+			double d = sptr[x];
+			cv::Vec4d homg_pt = _Q*cv::Vec4d(x, y, d, 1.0);
+			//dptr[x] = Vec3d(homg_pt.val);
+			//dptr[x] /= homg_pt[3];
+			dptr[x] = (homg_pt[2] / homg_pt[3]) / 1000.0f; // Depth in meters
+
+			if( fabs(d) <= FLT_EPSILON )
+				dptr[x] = 1000.0f;
+		}
+	}
+}
+};
+
 
 static void run(const string &file) {
 	ctpl::thread_pool pool(2);

components/common/cpp/include/ftl/cuda_common.hpp

@@ -27,7 +27,7 @@ class HisteresisTexture {
 template <typename T>
 class TextureObject {
 	public:
-	TextureObject() : texobj_(0), ptr_(nullptr) {};
+	__host__ __device__ TextureObject() : texobj_(0), ptr_(nullptr) {};
 	TextureObject(const cv::cuda::PtrStepSz<T> &d);
 	TextureObject(T *ptr, int pitch, int width, int height);
 	TextureObject(size_t width, size_t height);
@@ -39,9 +39,12 @@ class TextureObject {
 	__host__ __device__ T *devicePtr(int v) { return &ptr_[v*pitch2_]; }
 	__host__ __device__ int width() const { return width_; }
 	__host__ __device__ int height() const { return height_; }
-	cudaTextureObject_t cudaTexture() const { return texobj_; }
+	__host__ __device__ cudaTextureObject_t cudaTexture() const { return texobj_; }
+
+	#ifdef __CUDACC__
 	__device__ inline T tex2D(int u, int v) { return ::tex2D<T>(texobj_, u, v); }
 	__device__ inline T tex2D(float u, float v) { return ::tex2D<T>(texobj_, u, v); }
+	#endif
 
 	__host__ __device__ inline const T &operator()(int u, int v) const { return ptr_[u+v*pitch2_]; }
 	__host__ __device__ inline T &operator()(int u, int v) { return ptr_[u+v*pitch2_]; }
@@ -83,7 +86,7 @@ TextureObject<T>::TextureObject(const cv::cuda::PtrStepSz<T> &d) {
 	texDesc.readMode = cudaReadModeElementType;
 
 	cudaTextureObject_t tex = 0;
-	cudaCreateTextureObject(&tex, &resDesc, &texDesc, NULL);
+	cudaSafeCall(cudaCreateTextureObject(&tex, &resDesc, &texDesc, NULL));
 	texobj_ = tex;
 	pitch_ = d.step;
 	pitch2_ = pitch_ / sizeof(T);