diff --git a/net/cpp/include/ctpl_stl.h b/common/cpp/include/ctpl_stl.h
similarity index 100%
rename from net/cpp/include/ctpl_stl.h
rename to common/cpp/include/ctpl_stl.h
diff --git a/reconstruct/src/main.cpp b/reconstruct/src/main.cpp
index 719dcdc9fa5f45bb054623a3ed96fd0cf0905c12..e4385633ab87aff3380476ba3449fb4a50f52c0d 100644
--- a/reconstruct/src/main.cpp
+++ b/reconstruct/src/main.cpp
@@ -125,6 +125,7 @@ static void run(const string &file) {
// Make sure connections are complete
sleep_for(milliseconds(500));
+ // TODO(nick) Allow for many sources
net.subscribe(config["source"], [&rgb,&m,&depth](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
unique_lock<mutex> lk(m);
cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb);
@@ -153,6 +154,7 @@ static void run(const string &file) {
unique_lock<mutex> lk(m);
if (depth.cols > 0) {
+ // If no calibration data then get it from the remote machine
if (Q.rows == 0) {
auto buf = net.findOne<vector<unsigned char>>((string)config["source"]+"/calibration");
if (buf) {
@@ -162,9 +164,6 @@ static void run(const string &file) {
disp.setCalibration(Q);
}
}
- //depth.convertTo(idepth, CV_8U, 255.0f / 256.0f); // TODO(nick)
- //applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- //cv::imshow("Depth", idepth);
}
if (rgb.cols > 0) {
@@ -172,6 +171,7 @@ static void run(const string &file) {
disp.render(rgb,depth);
}
+ // TODO(nick) Use a swap buffer so this can be unlocked earlier
lk.unlock();
//if (cv::waitKey(40) == 27) break;
disp.wait(40);
diff --git a/vision/src/algorithms/fixstars_sgm.cpp b/vision/src/algorithms/fixstars_sgm.cpp
index 334f8c20e817322233c64d2e8edee25f326b4281..1c4b4decffe7beeb977145090300b18b2f08961f 100644
--- a/vision/src/algorithms/fixstars_sgm.cpp
+++ b/vision/src/algorithms/fixstars_sgm.cpp
@@ -28,11 +28,11 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
disp = Mat(cv::Size(l.cols, l.rows), CV_16UC1);
- auto start = std::chrono::high_resolution_clock::now();
+ //auto start = std::chrono::high_resolution_clock::now();
ssgm_->execute(lbw.data, rbw.data, disp.data);
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
+ //std::chrono::duration<double> elapsed =
+ // std::chrono::high_resolution_clock::now() - start;
+ //LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
disp.convertTo(disp, CV_32F, 1.0f/16.0f);
}
diff --git a/vision/src/main.cpp b/vision/src/main.cpp
index a22ebc25edfaf7a745ea56d64631825f7fbff3e2..86909a38b5bc80596c5428dda74de077a042b6a6 100644
--- a/vision/src/main.cpp
+++ b/vision/src/main.cpp
@@ -6,11 +6,15 @@
#include <glog/logging.h>
#include <ftl/config.h>
+#include <ctpl_stl.h>
#include <string>
#include <map>
#include <vector>
#include <fstream>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
#include <opencv2/opencv.hpp>
#include <ftl/local.hpp>
@@ -42,6 +46,9 @@ using ftl::net::Universe;
using std::string;
using std::vector;
using std::map;
+using std::condition_variable;
+using std::mutex;
+using std::unique_lock;
using cv::Mat;
using json = nlohmann::json;
using std::ifstream;
@@ -118,6 +125,7 @@ static void process_options(const map<string, string> &opts) {
}
static void run(const string &file) {
+ ctpl::thread_pool pool(2);
Universe net(config["net"]);
LocalSource *lsrc;
@@ -157,6 +165,7 @@ static void run(const string &file) {
if (!disparity) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
Mat l, r, disp;
+ Mat pl, pdisp;
Display display(config["display"]);
display.setCalibration(Q_32F);
@@ -164,25 +173,58 @@ static void run(const string &file) {
Streamer stream(net, config["stream"]);
while (display.active()) {
- // Read calibrated images.
- calibrate.rectified(l, r);
-
- // Feed into sync buffer and network forward
- sync->feed(ftl::LEFT, l, lsrc->getTimestamp());
- sync->feed(ftl::RIGHT, r, lsrc->getTimestamp());
-
- // Read back from buffer
- sync->get(ftl::LEFT, l);
- sync->get(ftl::RIGHT, r);
-
- // TODO(nick) Pipeline this
- disparity->compute(l, r, disp);
+ mutex m;
+ condition_variable cv;
+ int jobs = 0;
+
+ pool.push([&](int id) {
+ auto start = std::chrono::high_resolution_clock::now();
+ // Read calibrated images.
+ calibrate.rectified(l, r);
+
+ // Feed into sync buffer and network forward
+ sync->feed(ftl::LEFT, l, lsrc->getTimestamp());
+ sync->feed(ftl::RIGHT, r, lsrc->getTimestamp());
+
+ // Read back from buffer
+ sync->get(ftl::LEFT, l);
+ sync->get(ftl::RIGHT, r);
+
+ // TODO(nick) Pipeline this
+ disparity->compute(l, r, disp);
+
+ unique_lock<mutex> lk(m);
+ jobs++;
+ lk.unlock();
+ cv.notify_one();
+
+ std::chrono::duration<double> elapsed =
+ std::chrono::high_resolution_clock::now() - start;
+ LOG(INFO) << "Disparity in " << elapsed.count() << "s";
+ });
+
+ pool.push([&](int id) {
+ auto start = std::chrono::high_resolution_clock::now();
+ if (pl.rows != 0) stream.send(pl, pdisp);
+ unique_lock<mutex> lk(m);
+ jobs++;
+ lk.unlock();
+ cv.notify_one();
+
+ std::chrono::duration<double> elapsed =
+ std::chrono::high_resolution_clock::now() - start;
+ LOG(INFO) << "Stream in " << elapsed.count() << "s";
+ });
// Send RGB+Depth images for local rendering
display.render(l, disp);
-
- stream.send(l, disp);
display.wait(1);
+
+ unique_lock<mutex> lk(m);
+ cv.wait(lk, [&jobs]{return jobs == 2;});
+
+ l.copyTo(pl);
+ disp.copyTo(pdisp);
}
}
diff --git a/vision/src/streamer.cpp b/vision/src/streamer.cpp
index 848c8596f1af54d30260c55ff593258bb796fe76..6a0b04ddc664182aac5037b5eb3f8738935aeac6 100644
--- a/vision/src/streamer.cpp
+++ b/vision/src/streamer.cpp
@@ -40,7 +40,7 @@ void Streamer::send(const Mat &rgb, const Mat &depth) {
deflateEnd(&defstream);
d_buf.resize(defstream.total_out);
- LOG(INFO) << "Depth Size = " << ((float)d_buf.size() / (1024.0f*1024.0f));
+ //LOG(INFO) << "Depth Size = " << ((float)d_buf.size() / (1024.0f*1024.0f));
net_.publish(uri_, rgb_buf, d_buf);
}