From 8f5f3ff3f94bc1c8c513e74a0bfa2758f90ce64a Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Sun, 2 Jun 2019 14:57:34 +0300
Subject: [PATCH] Stream pose data over net
---
applications/gui/src/main.cpp | 105 +++++++++++++++---
components/renderers/cpp/src/rgbd_display.cpp | 2 +-
.../rgbd-sources/include/ftl/net_source.hpp | 2 +
components/rgbd-sources/src/net_source.cpp | 6 +
components/rgbd-sources/src/rgbd_streamer.cpp | 10 ++
5 files changed, 106 insertions(+), 19 deletions(-)
diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index ecd16bf9f..c4c171045 100644
--- a/applications/gui/src/main.cpp
+++ b/applications/gui/src/main.cpp
@@ -48,10 +48,90 @@ class GLTexture {
unsigned int glid_;
};
-struct SourceViews {
+/*struct SourceViews {
ftl::rgbd::RGBDSource *source;
GLTexture texture;
nanogui::ImageView *view;
+};*/
+
+template<class T>
+Eigen::Matrix<T,4,4> lookAt
+(
+ Eigen::Matrix<T,3,1> const & eye,
+ Eigen::Matrix<T,3,1> const & center,
+ Eigen::Matrix<T,3,1> const & up
+)
+{
+ typedef Eigen::Matrix<T,4,4> Matrix4;
+ typedef Eigen::Matrix<T,3,1> Vector3;
+
+ Vector3 f = (center - eye).normalized();
+ Vector3 u = up.normalized();
+ Vector3 s = f.cross(u).normalized();
+ u = s.cross(f);
+
+ Matrix4 res;
+ res << s.x(),s.y(),s.z(),-s.dot(eye),
+ u.x(),u.y(),u.z(),-u.dot(eye),
+ -f.x(),-f.y(),-f.z(),f.dot(eye),
+ 0,0,0,1;
+
+ return res;
+}
+
+class VirtualCameraView : public nanogui::ImageView {
+ public:
+ VirtualCameraView(nanogui::Widget *parent) : nanogui::ImageView(parent, 0) {
+ src_ = nullptr;
+ eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+ centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
+ up_ = Eigen::Vector3f(0,1.0f,0);
+ lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+ lerpSpeed_ = 0.4f;
+ }
+
+ void setSource(RGBDSource *src) { src_ = src; }
+
+ bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
+ //LOG(INFO) << "Mouse move: " << p[0];
+ if (src_ && down) {
+ Eigen::Vector4f camPos = src_->point(p[0],p[1]);
+ camPos *= -1.0f;
+ Eigen::Vector4f worldPos = src_->getPose() * camPos;
+ lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+ LOG(INFO) << "Depth at click = " << -camPos[2];
+ }
+ }
+
+ void draw(NVGcontext *ctx) {
+ //net_->broadcast("grab");
+ if (src_) {
+ cv::Mat rgb, depth;
+ centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
+ Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+
+ src_->setPose(viewPose);
+ src_->grab();
+ src_->getRGBD(rgb, depth);
+ if (rgb.rows > 0) {
+ LOG(INFO) << "Update texture";
+ texture_.update(rgb);
+ bindImage(texture_.texture());
+ }
+
+ screen()->performLayout(ctx);
+ }
+ ImageView::draw(ctx);
+ }
+
+ private:
+ RGBDSource *src_;
+ GLTexture texture_;
+ Eigen::Vector3f eye_;
+ Eigen::Vector3f centre_;
+ Eigen::Vector3f up_;
+ Eigen::Vector3f lookPoint_;
+ float lerpSpeed_;
};
class FTLApplication : public nanogui::Screen {
@@ -65,17 +145,18 @@ class FTLApplication : public nanogui::Screen {
if (!in) {
LOG(ERROR) << "Unrecognised source: " << src;
} else {
- auto &cam = sources_.emplace_back();
- cam.source = in;
+ //auto &cam = sources_.emplace_back();
+ //cam.source = in;
auto imageWindow = new Window(this, "Source");
imageWindow->setPosition(Eigen::Vector2i(710, 15));
imageWindow->setLayout(new GroupLayout());
imageWindow->setSize(Vector2i(400,400));
- auto imageView = new ImageView(imageWindow, 0);
- cam.view = imageView;
+ auto imageView = new VirtualCameraView(imageWindow);
+ //cam.view = imageView;
imageView->setGridThreshold(20);
+ imageView->setSource(in);
//imageView->setPixelInfoThreshold(20);
//displays.emplace_back(config["display"], src["uri"]);
@@ -88,24 +169,12 @@ class FTLApplication : public nanogui::Screen {
}
virtual void draw(NVGcontext *ctx) {
- //net_->broadcast("grab");
- for (auto &src : sources_) {
- cv::Mat rgb, depth;
- src.source->grab();
- src.source->getRGBD(rgb, depth);
- if (rgb.rows > 0) {
- src.texture.update(rgb);
- src.view->bindImage(src.texture.texture());
- }
- }
- performLayout();
-
/* Draw the user interface */
Screen::draw(ctx);
}
private:
- std::vector<SourceViews> sources_;
+ //std::vector<SourceViews> sources_;
ftl::net::Universe *net_;
};
diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp
index edc8ada98..fa5d6d3e3 100644
--- a/components/renderers/cpp/src/rgbd_display.cpp
+++ b/components/renderers/cpp/src/rgbd_display.cpp
@@ -120,7 +120,7 @@ void Display::update() {
centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
- source_->setPose(viewPose);
+ //source_->setPose(viewPose);
Mat rgb, depth;
//source_->grab();
diff --git a/components/rgbd-sources/include/ftl/net_source.hpp b/components/rgbd-sources/include/ftl/net_source.hpp
index cf56f6b5a..9730aef15 100644
--- a/components/rgbd-sources/include/ftl/net_source.hpp
+++ b/components/rgbd-sources/include/ftl/net_source.hpp
@@ -28,6 +28,8 @@ class NetSource : public RGBDSource {
return new NetSource(config, net);
}
+ void setPose(const Eigen::Matrix4f &pose);
+
private:
bool has_calibration_;
ftl::UUID peer_;
diff --git a/components/rgbd-sources/src/net_source.cpp b/components/rgbd-sources/src/net_source.cpp
index a4e3c9f0a..ce4e6cc42 100644
--- a/components/rgbd-sources/src/net_source.cpp
+++ b/components/rgbd-sources/src/net_source.cpp
@@ -79,6 +79,12 @@ void NetSource::_recv(const vector<unsigned char> &jpg, const vector<unsigned ch
}
}
+void NetSource::setPose(const Eigen::Matrix4f &pose) {
+ vector<unsigned char> vec((unsigned char*)pose.data(), (unsigned char*)(pose.data()+(pose.size())));
+ net_->send(peer_, "set_pose", getURI(), vec);
+ RGBDSource::setPose(pose);
+}
+
void NetSource::grab() {
// net_.broadcast("grab");
}
diff --git a/components/rgbd-sources/src/rgbd_streamer.cpp b/components/rgbd-sources/src/rgbd_streamer.cpp
index 06d40fd2e..ad4e700c0 100644
--- a/components/rgbd-sources/src/rgbd_streamer.cpp
+++ b/components/rgbd-sources/src/rgbd_streamer.cpp
@@ -42,6 +42,16 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
return streams;
});
+ net->bind("set_pose", [this](const std::string &uri, const std::vector<unsigned char> &buf) {
+ shared_lock<shared_mutex> slk(mutex_);
+
+ if (sources_.find(uri) != sources_.end()) {
+ Eigen::Matrix4f pose;
+ memcpy(pose.data(), buf.data(), buf.size());
+ sources_[uri]->src->setPose(pose);
+ }
+ });
+
// Allow remote users to access camera calibration matrix
net->bind("source_calibration", [this](const std::string &uri) -> vector<unsigned char> {
vector<unsigned char> buf;
--
GitLab