diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp index be2ac713acbdd7dc37a0424067daaefa438f660c..3df95ca79874cebcf2c9b96439e5d60cf268adb5 100644 --- a/applications/gui/src/main.cpp +++ b/applications/gui/src/main.cpp @@ -161,7 +161,24 @@ class FTLApplication : public nanogui::Screen { } else { auto src_ = swindow_->getSource(); if (src_ && src_->isReady() && down) { - Eigen::Vector4f camPos = src_->point(p[0],p[1]); + Eigen::Vector2f screenSize = size().cast<float>(); + auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff()); + Eigen::Vector2f scaleFactor = mScale * imageSize.cwiseQuotient(screenSize); + Eigen::Vector2f positionInScreen(0.0f, 0.0f); + auto mOffset = (screenSize - (screenSize.cwiseProduct(scaleFactor))) / 2; + Eigen::Vector2f positionAfterOffset = positionInScreen + mOffset; + + float sx = ((float)p[0] - positionAfterOffset[0]) / mScale; + float sy = ((float)p[1] - positionAfterOffset[1]) / mScale; + + Eigen::Vector4f camPos; + + try { + camPos = src_->point(sx,sy); + } catch(...) { + return true; + } + camPos *= -1.0f; Eigen::Vector4f worldPos = src_->getPose() * camPos; lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]); @@ -195,7 +212,7 @@ class FTLApplication : public nanogui::Screen { using namespace Eigen; auto src_ = swindow_->getSource(); - Vector2f imageSize(0, 0); + imageSize = {0, 0}; float now = (float)glfwGetTime(); float delta = now - ftime_; @@ -273,6 +290,7 @@ class FTLApplication : public nanogui::Screen { float lerpSpeed_; bool depth_; float ftime_; + Eigen::Vector2f imageSize; }; int main(int argc, char **argv) { diff --git a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp b/applications/reconstruct/include/ftl/ray_cast_sdf.hpp index 206e37c5d2fcd6044f1924af3d20491833f69ef1..d796b1cfc209d3f69c378cacb7fd4a98b638c0ae 100644 --- a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp +++ b/applications/reconstruct/include/ftl/ray_cast_sdf.hpp @@ -15,9 +15,11 @@ public: CUDARayCastSDF(nlohmann::json& config) : ftl::Configurable(config) { auto &cfg = ftl::config::resolve(config); create(parametersFromConfig(cfg)); - hash_render_ = cfg.value("hash_renderer", false); + hash_render_ = value("hash_renderer", false); } + bool isIntegerDepth() const { return hash_render_; } + ~CUDARayCastSDF(void) { destroy(); } diff --git a/applications/reconstruct/src/virtual_source.cpp b/applications/reconstruct/src/virtual_source.cpp index 072f71cb58d3f741714432077748ace9d6975bc5..157b71a3d420ab08775c2842b2cb5bffd3aa6898 100644 --- a/applications/reconstruct/src/virtual_source.cpp +++ b/applications/reconstruct/src/virtual_source.cpp @@ -26,6 +26,7 @@ VirtualSource::VirtualSource(ftl::rgbd::Source *host) rgb_ = cv::Mat(cv::Size(params_.width,params_.height), CV_8UC3); idepth_ = cv::Mat(cv::Size(params_.width,params_.height), CV_32SC1); + depth_ = cv::Mat(cv::Size(params_.width,params_.height), CV_32FC1); } VirtualSource::~VirtualSource() { @@ -52,8 +53,12 @@ bool VirtualSource::grab() { rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose()); //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); + if (rays_->isIntegerDepth()) { + rays_->getRayCastData().download((int*)idepth_.data, (uchar3*)rgb_.data, rays_->getRayCastParams()); + idepth_.convertTo(depth_, CV_32FC1, 1.0f / 100.0f); + } else { + rays_->getRayCastData().download((int*)depth_.data, (uchar3*)rgb_.data, rays_->getRayCastParams()); + } } return true; } diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp index 620ebd738f46c493471e0106bf4ed30377cb4e3b..0119a659aa14f0cfc43d25ff97349ba6ad9600e9 100644 --- a/applications/vision/src/main.cpp +++ b/applications/vision/src/main.cpp @@ -95,16 +95,20 @@ static void run(ftl::Configurable *root) { Streamer *stream = ftl::create<Streamer>(root, "stream", net); stream->add(source); - stream->run(); - + net->start(); - LOG(INFO) << "Running..."; - while (ftl::running && display->active()) { - cv::Mat rgb, depth; - source->getFrames(rgb, depth); - if (!rgb.empty()) display->render(rgb, depth, source->parameters()); - display->wait(10); + LOG(INFO) << "Running..."; + if (display->hasDisplays()) { + stream->run(); + while (ftl::running && display->active()) { + cv::Mat rgb, depth; + source->getFrames(rgb, depth); + if (!rgb.empty()) display->render(rgb, depth, source->parameters()); + display->wait(10); + } + } else { + stream->run(true); } LOG(INFO) << "Stopping..."; diff --git a/components/renderers/cpp/include/ftl/display.hpp b/components/renderers/cpp/include/ftl/display.hpp index 2e56c459bac79a8e704d0d331ff8f0a926f6fdaa..05ae0bf11e5ebc3a5b8d54339bc85166effbb4a9 100644 --- a/components/renderers/cpp/include/ftl/display.hpp +++ b/components/renderers/cpp/include/ftl/display.hpp @@ -43,6 +43,8 @@ class Display : public ftl::Configurable { bool render(const cv::Mat &img, style_t s=STYLE_NORMAL); bool active() const; + + bool hasDisplays(); void wait(int ms); diff --git a/components/renderers/cpp/src/display.cpp b/components/renderers/cpp/src/display.cpp index 4a7d88e44db0265824f43ba3a16688ff57e47919..0f838df751d0c0a315f4d0acca9798d2d7741066 100644 --- a/components/renderers/cpp/src/display.cpp +++ b/components/renderers/cpp/src/display.cpp @@ -21,7 +21,7 @@ Display::Display(nlohmann::json &config, std::string name) : ftl::Configurable(c //cv::namedWindow("Image", cv::WINDOW_KEEPRATIO); #if defined HAVE_PCL - if (config.value("points", false)) { + if (value("points", false)) { pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name)); pclviz_->setBackgroundColor (255, 255, 255); pclviz_->addCoordinateSystem (1.0); @@ -243,6 +243,10 @@ bool Display::render(const cv::Mat &img, style_t s) { return true; } +bool Display::hasDisplays() { + return value("depth", false) || value("left", false) || value("right", false) || value("points", false); +} + void Display::wait(int ms) { if (value("points", false)) { #if defined HAVE_PCL diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp index 4d31132e2625dea9c998c2b7a5518876f21d6c33..0623bf38c91116b116b85678e732ce98741f0a23 100644 --- a/components/renderers/cpp/src/rgbd_display.cpp +++ b/components/renderers/cpp/src/rgbd_display.cpp @@ -42,7 +42,7 @@ static void setMouseAction(const std::string& winName, const MouseAction &action } Display::Display(nlohmann::json &config) : ftl::Configurable(config) { - name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]")); + name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]")); viewcount__++; init(); @@ -50,7 +50,7 @@ Display::Display(nlohmann::json &config) : ftl::Configurable(config) { Display::Display(nlohmann::json &config, Source *source) : ftl::Configurable(config) { - name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]")); + name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]")); viewcount__++; init(); } diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp index 8e6eea09c968ef372097efc31171272b1e31a0df..1267b168e3650bbf4a8fe77ea3b396133c78689b 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp @@ -12,10 +12,6 @@ using cv::Mat; FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { ssgm_ = nullptr; use_filter_ = value("use_filter", false); - - LOG(INFO) << "Filter status: " << use_filter_; - LOG(INFO) << "Disp config: " << config; - // note: (max_disp_ << 4) libsgm subpixel accuracy. // What is the impact in the filter? (possible artifacts) filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, value("filter_radius", 25), value("filter_iter", 1)); diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index bd68b0ae86bdb79b3ec18b80afa3be09005637ad..2f51b6fdcf32f9661fead5ac550eb9f007ca072e 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/components/rgbd-sources/src/source.cpp @@ -128,8 +128,8 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) { Eigen::Vector4f Source::point(uint ux, uint uy) { const auto ¶ms = parameters(); - const float x = ((float)ux-params.width/2) / (float)params.fx; - const float y = ((float)uy-params.height/2) / (float)params.fy; + const float x = ((float)ux-(float)params.cx) / (float)params.fx; + const float y = ((float)uy-(float)params.cy) / (float)params.fy; shared_lock<shared_mutex> lk(mutex_); const float depth = depth_.at<float>(uy,ux); diff --git a/web-service/src/index.js b/web-service/src/index.js index 8fbba0ad66f458b6ad98e91c17364ae607553e6b..f4096ecdcb17b32510a3baa6e74245771a49e02f 100644 --- a/web-service/src/index.js +++ b/web-service/src/index.js @@ -11,6 +11,76 @@ let peer_uris = {}; let uri_data = {}; +function RGBDClient(peer, N, rate, dest) { + this.peer = peer; + this.txmax = N; + this.rate = rate; + this.dest = dest; + this.txcount = 0; +} + +RGBDClient.prototype.push = function(uri, rgb, depth) { + this.peer.send(uri, rgb, depth); + this.txcount++; +} + +function RGBDStream(uri, peer) { + this.uri = uri; + this.peer = peer; + this.title = ""; + this.rgb = null; + this.depth = null; + this.pose = null; + this.clients = []; + this.rxcount = 10; + this.rxmax = 10; + + peer.bind(uri, (rgb, depth) => { + this.pushFrames(rgb, depth); + this.rxcount++; + if (this.rxcount >= this.rxmax && this.clients.length > 0) { + this.subscribe(); + } + }); +} + +RGBDStream.prototype.addClient = function(peer, N, rate, dest) { + // TODO(Nick) Verify that it isn't already in list... + for (let i=0; i<this.clients.length; i++) { + if (this.clients[i].peer.string_id == peer.string_id) return; + } + + this.clients.push(new RGBDClient(peer, N, rate, dest)); + + if (this.rxcount >= this.rxmax) { + this.subscribe(); + } +} + +RGBDStream.prototype.subscribe = function() { + this.rxcount = 0; + this.rxmax = 10; + console.log("Subscribe to ", this.uri); + this.peer.send("get_stream", this.uri, 10, 0, [Peer.uuid], this.uri); +} + +RGBDStream.prototype.pushFrames = function(rgb, depth) { + this.rgb = rgb; + this.depth = depth; + + for (let i=0; i < this.clients.length; i++) { + this.clients[i].push(this.uri, rgb, depth); + } + + let i=0; + while (i < this.clients.length) { + if (this.clients[i].txcount >= this.clients[i].txmax) { + console.log("remove client"); + this.clients.splice(i, 1); + } else i++; + } +} + // ---- PROTOCOL --------------------------------------------------------------- app.get('/', (req, res) => { @@ -49,13 +119,7 @@ function checkStreams(peer) { //uri_to_peer[streams[i]] = peer; peer_uris[peer.string_id].push(streams[i]); - uri_data[streams[i]] = { - peer: peer, - title: "", - rgb: null, - depth: null, - pose: null - }; + uri_data[streams[i]] = new RGBDStream(streams[i], peer); } }); } @@ -144,13 +208,8 @@ app.ws('/', (ws, req) => { p.bind("get_stream", (uri, N, rate, pid, dest) => { let peer = uri_data[uri].peer; if (peer) { - // FIXME (NICK) BUG HERE, can't have multiple peers listening to same stream... - peer.bind(uri, (rgb, depth) => { - uri_data[uri].rgb = rgb; - uri_data[uri].depth = depth; - p.send(uri, rgb, depth); - }); - peer.send("get_stream", uri, N, rate, [Peer.uuid], dest); + uri_data[uri].addClient(p, N, rate, dest); + //peer.send("get_stream", uri, N, rate, [Peer.uuid], dest); } }); @@ -159,13 +218,7 @@ app.ws('/', (ws, req) => { //uri_to_peer[streams[i]] = peer; peer_uris[p.string_id].push(uri); - uri_data[uri] = { - peer: p, - title: "", - rgb: null, - depth: null, - pose: null - }; + uri_data[uri] = new RGBDStream(uri, p); broadcastExcept(p, "add_stream", uri); });