Resolves #35 with depth map rotations

applications/reconstruct/include/ftl/scene_rep_hash_sdf.hpp

@@ -336,7 +336,7 @@ private:
 
 		//t.startEvent("compactifyAllInOne");
 		m_hashParams.m_numOccupiedBlocks = compactifyHashAllInOneCUDA(m_hashData, m_hashParams);		//this version uses atomics over prefix sums, which has a much better performance
-		std::cout << "Occ blocks = " << m_hashParams.m_numOccupiedBlocks << std::endl;
+		//std::cout << "Occ blocks = " << m_hashParams.m_numOccupiedBlocks << std::endl;
 		m_hashData.updateParams(m_hashParams);	//make sure numOccupiedBlocks is updated on the GPU
 		//t.endEvent();
 		//t.evaluate();

applications/reconstruct/include/ftl/virtual_source.hpp

@@ -38,8 +38,7 @@ class VirtualSource : public RGBDSource {
 	ftl::voxhash::SceneRep *scene_;
 	CUDARayCastSDF *rays_;
 	bool ready_;
-	cv::Mat rgb_;
-	cv::Mat depth_;
+	cv::Mat idepth_;
 };
 
 }

applications/reconstruct/src/main.cpp

@@ -444,6 +444,7 @@ static void run() {
 				// Get the RGB-Depth frame from input
 				RGBDSource *input = inputs[i].source;
 				Mat rgb, depth;
+				input->grab();
 				input->getRGBD(rgb,depth);
 				
 				active += 1;

applications/reconstruct/src/virtual_source.cpp

@@ -6,6 +6,8 @@
 #include <glog/logging.h>
 
 using ftl::rgbd::VirtualSource;
+using std::mutex;
+using std::unique_lock;
 
 VirtualSource::VirtualSource(nlohmann::json &config, ftl::net::Universe *net)
 		: RGBDSource(config, net) {
@@ -22,7 +24,7 @@ VirtualSource::VirtualSource(nlohmann::json &config, ftl::net::Universe *net)
 	params_.minDepth = config.value("min_depth", 0.1f);
 
 	rgb_ = cv::Mat(cv::Size(params_.width,params_.height), CV_8UC3);
-	// init depth
+	idepth_ = cv::Mat(cv::Size(params_.width,params_.height), CV_32SC1);
 }
 
 VirtualSource::~VirtualSource() {
@@ -47,13 +49,11 @@ void VirtualSource::grab() {
 		params.m_sensorDepthWorldMax = params_.maxDepth;
 
 		rays_->render(scene_->getHashData(), scene_->getHashParams(), params, getPose());
-		rays_->getRayCastData().download(nullptr, (uchar3*)rgb_.data, rays_->getRayCastParams());
-	}
-}
 
-void VirtualSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
-	rgb_.copyTo(rgb);
-	depth_.copyTo(depth);
+		unique_lock<mutex> lk(mutex_);
+		rays_->getRayCastData().download((int*)idepth_.data, (uchar3*)rgb_.data, rays_->getRayCastParams());
+		idepth_.convertTo(depth_, CV_32FC1, 1.0f / 100.0f);
+	}
 }
 
 bool VirtualSource::isReady() {

applications/vision/src/main.cpp

@@ -149,7 +149,7 @@ static void run(const string &file) {
 
 			std::chrono::duration<double> elapsed =
 				std::chrono::high_resolution_clock::now() - start;
-			LOG(INFO) << "Disparity in " << elapsed.count() << "s";
+			//LOG(INFO) << "Disparity in " << elapsed.count() << "s";
 		});
 
 		// Pipeline for stream compression
@@ -163,7 +163,7 @@ static void run(const string &file) {
 
 			std::chrono::duration<double> elapsed =
 				std::chrono::high_resolution_clock::now() - start;
-			LOG(INFO) << "Stream in " << elapsed.count() << "s";
+			//LOG(INFO) << "Stream in " << elapsed.count() << "s";
 		});
 
 		// Send RGB+Depth images for local rendering

applications/vision/src/streamer.cpp

@@ -53,7 +53,7 @@ void Streamer::send(const Mat &rgb, const Mat &depth) {
     // d_buf.resize(s);
 
     cv::imencode(".png", d2, d_buf);
-    LOG(INFO) << "Depth Size = " << ((float)d_buf.size() / (1024.0f*1024.0f));
+    //LOG(INFO) << "Depth Size = " << ((float)d_buf.size() / (1024.0f*1024.0f));
 
 	try {
     	net_.publish(uri_, rgb_buf, d_buf);

components/renderers/cpp/src/rgbd_display.cpp

@@ -72,10 +72,10 @@ void Display::init() {
 
 	// Keyboard camera controls
 	onKey([this](int key) {
-		LOG(INFO) << "Key = " << key;
+		//LOG(INFO) << "Key = " << key;
 		if (key == 81 || key == 83) {
 			// TODO Should rotate around lookAt object, but requires correct depth
-			Eigen::Quaternion<float> q;  q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, Eigen::Vector3f(0.0,1.0f,0.0f));
+			Eigen::Quaternion<float> q;  q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
 			eye_ = (q * (eye_ - centre_)) + centre_;
 		} else if (key == 84 || key == 82) {
 			float scalar = (key == 84) ? 0.99f : 1.01f;
@@ -85,16 +85,13 @@ void Display::init() {
 
 	// TODO(Nick) Calculate "camera" properties of viewport.
 	mouseaction_ = [this]( int event, int ux, int uy, int) {
-		LOG(INFO) << "Mouse " << ux << "," << uy;
+		//LOG(INFO) << "Mouse " << ux << "," << uy;
 		if (event == 1 && source_) {   // click
-			const auto params = source_->getParameters();
-			const float x = ((float)ux-params.width/2) / params.fx;
-			const float y = ((float)uy-params.height/2) / params.fy;
-			const float depth = -4.0f;
-			Eigen::Vector4f camPos(x*depth,y*depth,depth,1.0);
+			Eigen::Vector4f camPos = source_->point(ux,uy);
+			camPos *= -1.0f;
 			Eigen::Vector4f worldPos =  source_->getPose() * camPos;
 			lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
-			LOG(INFO) << "Look at: " << worldPos;
+			LOG(INFO) << "Depth at click = " << -camPos[2];
 		}
 	};
 	::setMouseAction(name_, mouseaction_);
@@ -128,6 +125,6 @@ void Display::update() {
 	Mat rgb, depth;
 	source_->grab();
 	source_->getRGBD(rgb, depth);
-	cv::imshow(name_, rgb);
+	if (rgb.rows > 0) cv::imshow(name_, rgb);
 	wait(1);
 }

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

@@ -5,7 +5,6 @@
 #include <ftl/net/universe.hpp>
 #include <ftl/rgbd_source.hpp>
 #include <string>
-#include <mutex>
 
 namespace ftl {
 namespace rgbd {
@@ -23,7 +22,6 @@ class NetSource : public RGBDSource {
 	~NetSource();
 
 	void grab();
-	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
 	bool isReady();
 
 	static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) {
@@ -32,9 +30,6 @@ class NetSource : public RGBDSource {
 
 	private:
 	bool has_calibration_;
-	cv::Mat rgb_;
-	cv::Mat depth_;
-	std::mutex m_;
 };
 
 }

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

@@ -8,6 +8,7 @@
 #include <ftl/net/universe.hpp>
 #include <opencv2/opencv.hpp>
 #include <Eigen/Eigen>
+#include <mutex>
 
 namespace ftl {
 namespace rgbd {
@@ -23,14 +24,22 @@ class RGBDSource : public ftl::Configurable {
 	virtual ~RGBDSource();
 
 	virtual void grab()=0;
-	virtual void getRGBD(cv::Mat &rgb, cv::Mat &depth)=0;
 	virtual bool isReady();
 
+	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
+
 	const CameraParameters &getParameters() { return params_; };
 
-	void setPose(const Eigen::Matrix4f &pose) { pose_ = pose; };
+	virtual void setPose(const Eigen::Matrix4f &pose) { pose_ = pose; };
 	const Eigen::Matrix4f &getPose() { return pose_; };
 
+	virtual void reset() {}
+
+	/**
+	 * Get a point in camera coordinates at specified pixel location.
+	 */
+	Eigen::Vector4f point(uint x, uint y);
+
 	/**
 	 * Save the current RGB and Depth images to image files (jpg and png) with
 	 * the specified file prefix (excluding file extension).
@@ -66,6 +75,9 @@ class RGBDSource : public ftl::Configurable {
 	protected:
 	CameraParameters params_;
 	ftl::net::Universe *net_;
+	std::mutex mutex_;
+	cv::Mat rgb_;
+	cv::Mat depth_;
 
 	private:
 	Eigen::Matrix4f pose_;

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

@@ -26,7 +26,6 @@ class StereoVideoSource : public RGBDSource {
 	~StereoVideoSource();
 
 	void grab();
-	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
 	bool isReady();
 
 	const cv::Mat &getRight() const { return right_; }

components/rgbd-sources/src/net_source.cpp

@@ -44,7 +44,7 @@ NetSource::NetSource(nlohmann::json &config, ftl::net::Universe *net)
 	has_calibration_ = getCalibration(*net, config["uri"].get<string>(), params_);
 	
 	net->subscribe(config["uri"].get<string>(), [this](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
-		unique_lock<mutex> lk(m_);
+		unique_lock<mutex> lk(mutex_);
 		cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb_);
 		//Mat(rgb_.size(), CV_16UC1);
 		cv::imdecode(d, cv::IMREAD_UNCHANGED, &depth_);
@@ -63,9 +63,3 @@ void NetSource::grab() {
 bool NetSource::isReady() {
 	return has_calibration_ && !rgb_.empty();
 }
-
-void NetSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
-	unique_lock<mutex> lk(m_);
-	rgb_.copyTo(rgb);
-	depth_.copyTo(depth);
-}

components/rgbd-sources/src/rgbd_source.cpp

@@ -3,6 +3,8 @@
 using ftl::rgbd::RGBDSource;
 using ftl::Configurable;
 using std::string;
+using std::mutex;
+using std::unique_lock;
 
 std::map<std::string, std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)>> *RGBDSource::sources__ = nullptr;
 
@@ -22,6 +24,22 @@ bool RGBDSource::isReady() {
 	return false;
 }
 
+void RGBDSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
+	unique_lock<mutex> lk(mutex_);
+	rgb_.copyTo(rgb);
+	depth_.copyTo(depth);
+}
+
+Eigen::Vector4f RGBDSource::point(uint ux, uint uy) {
+	const auto &params = getParameters();
+	const float x = ((float)ux-params.width/2) / params.fx;
+	const float y = ((float)uy-params.height/2) / params.fy;
+
+	unique_lock<mutex> lk(mutex_);
+	const float depth = depth_.at<float>(uy,ux);
+	return Eigen::Vector4f(x*depth,y*depth,depth,1.0);
+}
+
 bool RGBDSource::snapshot(const std::string &fileprefix) {
 	cv::Mat rgb;
 	cv::Mat depth;

components/rgbd-sources/src/stereovideo_source.cpp

@@ -4,11 +4,14 @@
 #include "calibrate.hpp"
 #include "local.hpp"
 #include "disparity.hpp"
+#include <mutex>
 
 using ftl::Calibrate;
 using ftl::LocalSource;
 using ftl::rgbd::StereoVideoSource;
 using std::string;
+using std::mutex;
+using std::unique_lock;
 
 StereoVideoSource::StereoVideoSource(nlohmann::json &config, ftl::net::Universe *net)
 		: RGBDSource(config, net) {
@@ -79,14 +82,6 @@ StereoVideoSource::~StereoVideoSource() {
 	delete lsrc_;
 }
 
-void StereoVideoSource::grab() {
-	calib_->rectified(left_, right_);
-}
-
-bool StereoVideoSource::isReady() {
-	return ready_;
-}
-
 static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv::Mat &q) {
 	cv::Matx44d _Q;
     q.convertTo(_Q, CV_64F);
@@ -110,10 +105,17 @@ static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv:
 	}
 }
 
-void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
+void StereoVideoSource::grab() {
+	calib_->rectified(left_, right_);
+
 	cv::Mat disp;
 	disp_->compute(left_, right_, disp, mask_l_);
-	rgb = left_;
-	disparityToDepth(disp, depth, calib_->getQ());
-	//calib_->distort(rgb,depth);
+
+	unique_lock<mutex> lk(mutex_);
+	left_.copyTo(rgb_);
+	disparityToDepth(disp, depth_, calib_->getQ());
+}
+
+bool StereoVideoSource::isReady() {
+	return ready_;
 }