diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index ea45a23ce6c883e49f9f87340f6458723a6c893d..d070e3941a2622fdbf5dd43840647a0edf29668f 100644
--- a/applications/gui/src/main.cpp
+++ b/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 =
@@ -112,6 +219,17 @@ namespace {
}
+static Eigen::Affine3f create_rotation_matrix(float ax, float ay, float az) {
+ Eigen::Affine3f rx =
+ Eigen::Affine3f(Eigen::AngleAxisf(ax, Eigen::Vector3f(1, 0, 0)));
+ Eigen::Affine3f ry =
+ Eigen::Affine3f(Eigen::AngleAxisf(ay, Eigen::Vector3f(0, 1, 0)));
+ Eigen::Affine3f rz =
+ Eigen::Affine3f(Eigen::AngleAxisf(az, Eigen::Vector3f(0, 0, 1)));
+ return ry * rz * rx;
+}
+
+
class FTLApplication : public nanogui::Screen {
public:
explicit FTLApplication(ftl::Configurable *root, ftl::net::Universe *net, ftl::ctrl::Master *controller) : nanogui::Screen(Eigen::Vector2i(1024, 768), "FT-Lab GUI") {
@@ -123,10 +241,11 @@ class FTLApplication : public nanogui::Screen {
//src_ = nullptr;
eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
- centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
+ neye_ = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 0.0f);
+ orientation_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
up_ = Eigen::Vector3f(0,1.0f,0);
- lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
- lerpSpeed_ = 0.4f;
+ //lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+ lerpSpeed_ = 0.999f;
depth_ = false;
ftime_ = (float)glfwGetTime();
@@ -155,6 +274,18 @@ class FTLApplication : public nanogui::Screen {
mShader.free();
}
+ bool mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2i &rel, int button, int modifiers) {
+ if (Screen::mouseMotionEvent(p, rel, button, modifiers)) {
+ return true;
+ } else {
+ if (button == 1) {
+ orientation_[0] += ((float)rel[1] * 0.2f * delta_);
+ //orientation_[2] += std::cos(orientation_[1])*((float)rel[1] * 0.2f * delta_);
+ orientation_[1] -= (float)rel[0] * 0.2f * delta_;
+ }
+ }
+ }
+
bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
if (Screen::mouseButtonEvent(p, button, down, modifiers)) {
return true;
@@ -181,7 +312,7 @@ class FTLApplication : public nanogui::Screen {
camPos *= -1.0f;
Eigen::Vector4f worldPos = src_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+ //lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
LOG(INFO) << "Depth at click = " << -camPos[2];
return true;
}
@@ -190,18 +321,25 @@ class FTLApplication : public nanogui::Screen {
}
bool keyboardEvent(int key, int scancode, int action, int modifiers) {
+ using namespace Eigen;
if (Screen::keyboardEvent(key, scancode, action, modifiers)) {
return true;
} else {
- LOG(INFO) << "Key press " << key << " - " << action;
+ //LOG(INFO) << "Key press " << key << " - " << action;
if (key == 263 || key == 262) {
- // TODO Should rotate around lookAt object, but requires correct depth
- Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 262) ? 0.01f : -0.01f, up_);
- eye_ = (q * (eye_ - centre_)) + centre_;
+ float scalar = (key == 263) ? -0.1f : 0.1f;
+ Eigen::Affine3f r = create_rotation_matrix(orientation_[0], orientation_[1], orientation_[2]);
+ neye_ += r.matrix()*Vector4f(scalar,0.0,0.0,1.0);
return true;
} else if (key == 264 || key == 265) {
- float scalar = (key == 264) ? 0.99f : 1.01f;
- eye_ = ((eye_ - centre_) * scalar) + centre_;
+ float scalar = (key == 264) ? -0.1f : 0.1f;
+ Eigen::Affine3f r = create_rotation_matrix(orientation_[0], orientation_[1], orientation_[2]);
+ neye_ += r.matrix()*Vector4f(0.0,0.0,scalar,1.0);
+ return true;
+ } else if (key == 266 || key == 267) {
+ float scalar = (key == 266) ? -0.1f : 0.1f;
+ Eigen::Affine3f r = create_rotation_matrix(orientation_[0], orientation_[1], orientation_[2]);
+ neye_ += r.matrix()*Vector4f(0.0,scalar,0.0,1.0);
return true;
} else if (action == 1 && key == 'H') {
swindow_->setVisible(!swindow_->visible());
@@ -211,6 +349,8 @@ class FTLApplication : public nanogui::Screen {
}
}
+ StatisticsImageNSamples *stats_ = nullptr;
+
virtual void draw(NVGcontext *ctx) {
using namespace Eigen;
@@ -218,33 +358,60 @@ class FTLApplication : public nanogui::Screen {
imageSize = {0, 0};
float now = (float)glfwGetTime();
- float delta = now - ftime_;
+ delta_ = now - ftime_;
ftime_ = now;
if (src_) {
cv::Mat rgb, depth;
- centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * delta);
- Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+
+ // Lerp the Eye
+ eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
+ eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
+ eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+
+ Eigen::Affine3f r = create_rotation_matrix(orientation_[0], orientation_[1], orientation_[2]);
+ Eigen::Translation3f trans(eye_);
+ Eigen::Affine3f t(trans);
+ Eigen::Matrix4f viewPose = (t * r).matrix();
src_->setPose(viewPose);
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);
+ 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();
- }
}
}
@@ -288,12 +455,14 @@ class FTLApplication : public nanogui::Screen {
//Source *src_;
GLTexture texture_;
Eigen::Vector3f eye_;
- Eigen::Vector3f centre_;
+ Eigen::Vector4f neye_;
+ Eigen::Vector3f orientation_;
Eigen::Vector3f up_;
- Eigen::Vector3f lookPoint_;
+ //Eigen::Vector3f lookPoint_;
float lerpSpeed_;
bool depth_;
float ftime_;
+ float delta_;
Eigen::Vector2f imageSize;
};
diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 997aaaa645852a7a9c893a9c555074fff6cc22a4..ad61ccccb14555418106b9e408dc48e3daeccee2 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -1,6 +1,8 @@
#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>
@@ -77,37 +79,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);
diff --git a/applications/gui/src/src_window.hpp b/applications/gui/src/src_window.hpp
index 6258267c62fcec81c688a89a9361c4c58d166938..e7aa192133eebd7ec306c41be1d84d30a8861538 100644
--- a/applications/gui/src/src_window.hpp
+++ b/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_;