diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 326e7c3320fceaf5b31a89248d77fe0d51b1ec30..0a66710607fb1b717488087b880af764b597c194 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -10,6 +10,17 @@ namespace rgbd {
class Source;
+typedef unsigned int channel_t;
+
+static const channel_t kChanNone = 0;
+static const channel_t kChanLeft = 0x0001;
+static const channel_t kChanDepth = 0x0002;
+static const channel_t kChanRight = 0x0004;
+static const channel_t kChanDisparity = 0x0008;
+static const channel_t kChanDeviation = 0x0010;
+
+static const channel_t kChanOverlay1 = 0x1000;
+
typedef unsigned int capability_t;
static const capability_t kCapMovable = 0x0001; // A movable virtual cam
@@ -35,6 +46,8 @@ class Source {
virtual bool isReady() { return false; };
virtual void setPose(const Eigen::Matrix4d &pose) { };
+ virtual Camera parameters(channel_t) { return params_; };
+
protected:
capability_t capabilities_;
ftl::rgbd::Source *host_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index c5b84fd69b12cf75aa5485d6b1ad152640479fb0..4efb97175e886ba821f752f7f154eddde0aa1f8f 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -25,17 +25,6 @@ static inline bool isValidDepth(float d) { return (d > 0.01f) && (d < 39.99f); }
class SnapshotReader;
-typedef unsigned int channel_t;
-
-static const channel_t kChanNone = 0;
-static const channel_t kChanLeft = 0x0001;
-static const channel_t kChanDepth = 0x0002;
-static const channel_t kChanRight = 0x0004;
-static const channel_t kChanDisparity = 0x0008;
-static const channel_t kChanDeviation = 0x0010;
-
-static const channel_t kChanOverlay1 = 0x1000;
-
/**
* RGBD Generic data source configurable entity. This class hides the
* internal implementation of an RGBD source by providing accessor functions
@@ -137,6 +126,8 @@ class Source : public ftl::Configurable {
else return params_;
}
+ const Camera parameters(channel_t) const;
+
cv::Mat cameraMatrix() const;
/**
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index fb8ab483762a2f871199fae625c07ec445796b74..388b233cd3e90be2a6be3847128f892ef5cb0b65 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -18,10 +18,10 @@ using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using std::tuple;
-bool NetSource::_getCalibration(Universe &net, const UUID &peer, const string &src, ftl::rgbd::Camera &p) {
+bool NetSource::_getCalibration(Universe &net, const UUID &peer, const string &src, ftl::rgbd::Camera &p, ftl::rgbd::channel_t chan) {
try {
while(true) {
- auto [cap,buf] = net.call<tuple<unsigned int,vector<unsigned char>>>(peer_, "source_details", src);
+ auto [cap,buf] = net.call<tuple<unsigned int,vector<unsigned char>>>(peer_, "source_details", src, chan);
capabilities_ = cap;
@@ -216,6 +216,19 @@ void NetSource::setPose(const Eigen::Matrix4d &pose) {
//Source::setPose(pose);
}
+ftl::rgbd::Camera NetSource::parameters(ftl::rgbd::channel_t chan) {
+ if (chan == ftl::rgbd::kChanRight) {
+ auto uri = host_->get<string>("uri");
+ if (!uri) return params_;
+
+ ftl::rgbd::Camera params;
+ _getCalibration(*host_->getNet(), peer_, *uri, params, chan);
+ return params;
+ } else {
+ return params_;
+ }
+}
+
void NetSource::_updateURI() {
UNIQUE_LOCK(mutex_,lk);
active_ = false;
@@ -234,7 +247,7 @@ void NetSource::_updateURI() {
}
peer_ = *p;
- has_calibration_ = _getCalibration(*host_->getNet(), peer_, *uri, params_);
+ has_calibration_ = _getCalibration(*host_->getNet(), peer_, *uri, params_, ftl::rgbd::kChanLeft);
host_->getNet()->bind(*uri, [this](int64_t frame, int chunk, bool delta, const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
_recvChunk(frame, chunk, delta, jpg, d);
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index 453ebe76696e8243c1b2b26bc2530724809a5702..12186e82317c130c557f36a7ab3a9cc7e8e83dfe 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/components/rgbd-sources/src/net.hpp
@@ -26,6 +26,7 @@ class NetSource : public detail::Source {
bool isReady();
void setPose(const Eigen::Matrix4d &pose);
+ Camera parameters(channel_t chan);
void reset();
@@ -54,7 +55,7 @@ class NetSource : public detail::Source {
cv::Mat d_depth_;
cv::Mat d_rgb_;
- bool _getCalibration(ftl::net::Universe &net, const ftl::UUID &peer, const std::string &src, ftl::rgbd::Camera &p);
+ bool _getCalibration(ftl::net::Universe &net, const ftl::UUID &peer, const std::string &src, ftl::rgbd::Camera &p, ftl::rgbd::channel_t chan);
void _recv(const std::vector<unsigned char> &jpg, const std::vector<unsigned char> &d);
void _recvChunk(int64_t frame, int chunk, bool delta, const std::vector<unsigned char> &jpg, const std::vector<unsigned char> &d);
void _updateURI();
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 229b9d6952c332d3f426ddedf06971628fcce6f1..7c13f485280ae129bd478e5df5c137ef327f398c 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -323,3 +323,7 @@ bool Source::setChannel(ftl::rgbd::channel_t c) {
// FIXME:(Nick) Verify channel is supported by this source...
return true;
}
+
+const ftl::rgbd::Camera Source::parameters(channel_t chan) const {
+ return (impl_) ? impl_->parameters(chan) : parameters();
+}
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index ec040740e7d19e5b6ad5f47fe18eba3acae234f7..cb5e08a3a572d1534f5aae086e635551a6cbd32f 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/components/rgbd-sources/src/stereovideo.cpp
@@ -120,6 +120,28 @@ void StereoVideoSource::init(const string &file) {
ready_ = true;
}
+ftl::rgbd::Camera StereoVideoSource::parameters(ftl::rgbd::channel_t chan) {
+ if (chan == ftl::rgbd::kChanRight) {
+ cv::Mat q = calib_->getCameraMatrixRight();
+ ftl::rgbd::Camera params = {
+ q.at<double>(0,0), // Fx
+ q.at<double>(1,1), // Fy
+ -q.at<double>(0,2), // Cx
+ -q.at<double>(1,2), // Cy
+ (unsigned int)lsrc_->width(),
+ (unsigned int)lsrc_->height(),
+ 0.0f, // 0m min
+ 15.0f, // 15m max
+ 1.0 / calib_->getQ().at<double>(3,2), // Baseline
+ 0.0f // doffs
+ };
+ return params;
+ //params_.doffs = -calib_->getQ().at<double>(3,3) * params_.baseline;
+ } else {
+ return params_;
+ }
+}
+
static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
const cv::Mat &Q, cv::cuda::Stream &stream) {
// Q(3, 2) = -1/Tx
diff --git a/components/rgbd-sources/src/stereovideo.hpp b/components/rgbd-sources/src/stereovideo.hpp
index 28fe6b90dd82f661262eeafc97d46fcd16b65c80..7835742389330046a33b7f22289ac36e8cdab4d5 100644
--- a/components/rgbd-sources/src/stereovideo.hpp
+++ b/components/rgbd-sources/src/stereovideo.hpp
@@ -28,6 +28,7 @@ class StereoVideoSource : public detail::Source {
bool grab(int n, int b);
bool isReady();
+ Camera parameters(channel_t chan);
//const cv::Mat &getRight() const { return right_; }
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index f9d253a9a5266a1b7d10fe800ed87be48dafa670..ec1a9610a0511acdc3669fdcc6e4eecff00509cf 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -75,14 +75,15 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
});
// Allow remote users to access camera calibration matrix
- net->bind("source_details", [this](const std::string &uri) -> tuple<unsigned int,vector<unsigned char>> {
+ net->bind("source_details", [this](const std::string &uri, ftl::rgbd::channel_t chan) -> tuple<unsigned int,vector<unsigned char>> {
vector<unsigned char> buf;
SHARED_LOCK(mutex_,slk);
if (sources_.find(uri) != sources_.end()) {
buf.resize(sizeof(Camera));
LOG(INFO) << "Calib buf size = " << buf.size();
- memcpy(buf.data(), &sources_[uri]->src->parameters(), buf.size());
+ auto params = sources_[uri]->src->parameters(chan);
+ memcpy(buf.data(), ¶ms, buf.size());
return make_tuple(sources_[uri]->src->getCapabilities(), buf);
} else {
return make_tuple(0u,buf);