Merge branch 'feature/visualization-stddev' into 'master'

Standard deviation visualization for depth values.

See merge request nicolas.pope/ftl!42

applications/gui/src/main.cpp

@@ -28,6 +28,113 @@ using ftl::rgbd::Source;
 	nanogui::ImageView *view;
 };*/
 
+class StatisticsImage {
+private:
+	std::vector<float> data_;
+	cv::Size size_;
+	int n_;
+
+public:
+	StatisticsImage(cv::Size size) {
+		size_ = size;
+		data_ = std::vector<float>(size.width * size.height * 2, 0.0f);
+		n_ = 0;
+	}
+
+	void update(const cv::Mat &in);
+	void getStdDev(cv::Mat &out);
+	void getMean(cv::Mat &out);
+};
+
+void StatisticsImage::update(const cv::Mat &in) {
+	DCHECK(in.type() == CV_32F);
+	DCHECK(in.size() == size_);
+	// Knuth's method 
+
+	n_++;
+	for (int i = 0; i < size_.width * size_.height; i++) {
+		float x = ((float*) in.data)[i];
+		if (x > 30.0f || x < 0.001) { continue; } // invalid value
+		float &m = data_[2*i];
+		float &S = data_[2*i+1];
+		float m_prev = m;
+		m = m + (x - m) / n_;
+		S = S + (x - m) * (x - m_prev);
+	}
+}
+
+void StatisticsImage::getStdDev(cv::Mat &in) {
+	in = cv::Mat(size_, CV_32F, 0.0f);
+
+	for (int i = 0; i < size_.width * size_.height; i++) {
+		float &m = data_[2*i];
+		float &S = data_[2*i+1];
+		((float*) in.data)[i] = sqrt(S / n_);
+	}
+}
+
+void StatisticsImage::getMean(cv::Mat &in) {
+	in = cv::Mat(size_, CV_32F, 0.0f);
+
+	for (int i = 0; i < size_.width * size_.height; i++) {
+		((float*) in.data)[i] = data_[2*i];
+	}
+}
+
+class StatisticsImageNSamples {
+private:
+	std::vector<cv::Mat> samples_;
+	cv::Size size_;
+	int i_;
+	int n_;
+
+public:
+	StatisticsImageNSamples(cv::Size size, int n) {
+		size_ = size;
+		samples_ = std::vector<cv::Mat>(n);
+		i_ = 0;
+		n_ = n;
+	}
+
+	void update(const cv::Mat &in);
+	void getStdDev(cv::Mat &out);
+	void getMean(cv::Mat &out);
+};
+
+void StatisticsImageNSamples::update(const cv::Mat &in) {
+	DCHECK(in.type() == CV_32F);
+	DCHECK(in.size() == size_);
+
+	i_ = (i_ + 1) % n_;
+	in.copyTo(samples_[i_]);
+}
+
+void StatisticsImageNSamples::getStdDev(cv::Mat &in) {
+	// Knuth's method
+	cv::Mat mat_m(size_, CV_32F, 0.0f);
+	cv::Mat mat_S(size_, CV_32F, 0.0f);
+
+	float n = 0.0f;
+	for (int i_sample = (i_ + 1) % n_; i_sample != i_; i_sample = (i_sample + 1) % n_) {
+		n += 1.0f;
+		for (int i = 0; i < size_.width * size_.height; i++) {
+			float &x = ((float*) samples_[i_sample].data)[i];
+			float &m = ((float*) mat_m.data)[i];
+			float &S = ((float*) mat_S.data)[i];
+			float m_prev = m;
+
+			if (x > 30.0 || x < 0.001) continue;
+
+			m = m + (x - m) / n;
+			S = S + (x - m) * (x - m_prev);
+		}
+	}
+
+	mat_S.copyTo(in);
+	cv::sqrt(in, in);
+}
+
+void StatisticsImageNSamples::getMean(cv::Mat &in) {}
 
 namespace {
     constexpr char const *const defaultImageViewVertexShader =
@@ -211,6 +318,8 @@ class FTLApplication : public nanogui::Screen {
 		}
 	}
 
+	StatisticsImageNSamples *stats_ = nullptr;
+
 	virtual void draw(NVGcontext *ctx) {
 		using namespace Eigen;
 
@@ -230,21 +339,40 @@ class FTLApplication : public nanogui::Screen {
 			src_->grab();
 			src_->getFrames(rgb, depth);
 
-			if (swindow_->getDepth()) {
-				if (depth.rows > 0) {
+			if (!stats_ && depth.rows > 0) {
+				stats_ = new StatisticsImageNSamples(depth.size(), 25);
+			}
+			
+			if (stats_ && depth.rows > 0) { stats_->update(depth); }
+
+			cv::Mat tmp;
+
+			using ftl::gui::SourceWindow;
+			switch(swindow_->getMode()) {
+				case SourceWindow::Mode::depth:
+					if (depth.rows == 0) { break; }
 					imageSize = Vector2f(depth.cols,depth.rows);
-					cv::Mat idepth;
-					depth.convertTo(idepth, CV_8U, 255.0f / 10.0f);  // TODO(nick)
-    				applyColorMap(idepth, idepth, cv::COLORMAP_JET);
-					texture_.update(idepth);
+					depth.convertTo(tmp, CV_8U, 255.0f / 10.0f);  // TODO(nick)
+					applyColorMap(tmp, tmp, cv::COLORMAP_JET);
+					texture_.update(tmp);
 					mImageID = texture_.texture();
-				}
-			} else {
-				if (rgb.rows > 0) {
+					break;
+				
+				case SourceWindow::Mode::stddev:
+					if (depth.rows == 0) { break; }
+					imageSize = Vector2f(depth.cols, depth.rows);
+					stats_->getStdDev(tmp);
+					tmp.convertTo(tmp, CV_8U, 50.0);
+					applyColorMap(tmp, tmp, cv::COLORMAP_HOT);
+					texture_.update(tmp);
+					mImageID = texture_.texture();
+					break;
+
+				default:
+					if (rgb.rows == 0) { break; }
 					imageSize = Vector2f(rgb.cols,rgb.rows);
 					texture_.update(rgb);
 					mImageID = texture_.texture();
-				}
 			}
 		}
 

applications/gui/src/src_window.cpp

 #include "src_window.hpp"
 
 #include <nanogui/imageview.h>
+#include <nanogui/textbox.h>
+#include <nanogui/slider.h>
 #include <nanogui/combobox.h>
 #include <nanogui/label.h>
 #include <nanogui/opengl.h>
@@ -158,37 +160,43 @@ SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 	setLayout(new nanogui::GroupLayout());
 
 	using namespace nanogui;
-
-	depth_ = false;
-    src_ = ftl::create<Source>(ctrl->getRoot(), "source", ctrl->getNet());
+	
+	mode_ = Mode::rgb;
+	src_ = ftl::create<Source>(ctrl->getRoot(), "source", ctrl->getNet());
 
 	//Widget *tools = new Widget(this);
-    //    tools->setLayout(new BoxLayout(Orientation::Horizontal,
-    //                                   Alignment::Middle, 0, 6));
+	//    tools->setLayout(new BoxLayout(Orientation::Horizontal,
+	//                                   Alignment::Middle, 0, 6));
 
-    new Label(this, "Select source","sans-bold");
-    available_ = ctrl->getNet()->findAll<string>("list_streams");
-    auto select = new ComboBox(this, available_);
-    select->setCallback([this,select](int ix) {
-        LOG(INFO) << "Change source: " << ix;
-        src_->set("uri", available_[ix]);
-    });
+	new Label(this, "Select source","sans-bold");
+	available_ = ctrl->getNet()->findAll<string>("list_streams");
+	auto select = new ComboBox(this, available_);
+	select->setCallback([this,select](int ix) {
+		LOG(INFO) << "Change source: " << ix;
+		src_->set("uri", available_[ix]);
+});
 
 	ctrl->getNet()->onConnect([this,select](ftl::net::Peer *p) {
-		 available_ = ctrl_->getNet()->findAll<string>("list_streams");
-		 select->setItems(available_);
+		available_ = ctrl_->getNet()->findAll<string>("list_streams");
+		select->setItems(available_);
 	});
 
-	auto depth = new Button(this, "Depth");
-	depth->setFlags(Button::ToggleButton);
-	depth->setChangeCallback([this](bool state) {
-		//image_->setDepth(state);
-		depth_ = state;
-	});
+	auto button_rgb = new Button(this, "RGB (left)");
+	button_rgb->setFlags(Button::RadioButton);
+	button_rgb->setPushed(true);
+	button_rgb->setChangeCallback([this](bool state) { mode_ = Mode::rgb; });
+
+	auto button_depth = new Button(this, "Depth");
+	button_depth->setFlags(Button::RadioButton);
+	button_depth->setChangeCallback([this](bool state) { mode_ = Mode::depth; });
+
+	auto button_stddev = new Button(this, "Standard Deviation (25 frames)");
+	button_stddev->setFlags(Button::RadioButton);
+	button_stddev->setChangeCallback([this](bool state) { mode_ = Mode::stddev; });
 
 #ifdef HAVE_LIBARCHIVE
-	auto snapshot = new Button(this, "Snapshot");
-	snapshot->setCallback([this] {
+	auto button_snapshot = new Button(this, "Snapshot");
+	button_snapshot->setCallback([this] {
 		try {
 			char timestamp[18];
 			std::time_t t=std::time(NULL);

applications/gui/src/src_window.hpp

@@ -18,16 +18,18 @@ namespace gui {
  */
 class SourceWindow : public nanogui::Window {
 	public:
+	enum class Mode { rgb, depth, stddev };
 	SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl);
 	~SourceWindow();
 
 	ftl::rgbd::Source *getSource() const { return src_; }
-	bool getDepth() const { return depth_; }
+	bool getDepth() const { return mode_ == Mode::depth; }
+	Mode getMode() { return mode_; }
 
 	private:
 	ftl::ctrl::Master *ctrl_;
 	ftl::rgbd::Source *src_;
-	bool depth_;
+	Mode mode_;
 	VirtualCameraView *image_;
 	std::vector<std::string> available_;