diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp
index 6673275fe7b4f874b0807c48e690e24b4aaddb51..9aa2143020c89c1331f3d408f33f2ef359cf77b8 100644
--- a/components/codecs/include/ftl/codecs/channels.hpp
+++ b/components/codecs/include/ftl/codecs/channels.hpp
@@ -8,30 +8,31 @@ namespace ftl {
namespace codecs {
enum struct Channel : int {
- None = -1,
- Colour = 0, // 8UC3 or 8UC4
- Left = 0,
- Depth = 1, // 32S or 32F
- Right = 2, // 8UC3 or 8UC4
- Colour2 = 2,
- Disparity = 3,
- Depth2 = 3,
- Deviation = 4,
- Screen = 4,
- Normals = 5, // 32FC4
- Points = 6, // 32FC4 (should be deprecated)
- Confidence = 7, // 32F
- Contribution = 7, // 32F
- EnergyVector = 8, // 32FC4
- Flow = 9, // 32F
- Smoothing = 9, // 32F
- Energy = 10, // 32F
+ None = -1,
+ Colour = 0, // 8UC3 or 8UC4
+ Left = 0,
+ Depth = 1, // 32S or 32F
+ Right = 2, // 8UC3 or 8UC4
+ Colour2 = 2,
+ Disparity = 3,
+ Depth2 = 3,
+ Deviation = 4,
+ Screen = 4,
+ Normals = 5, // 32FC4
+ Points = 6, // 32FC4 (should be deprecated)
+ Confidence = 7, // 32F
+ Contribution = 7, // 32F
+ EnergyVector = 8, // 32FC4
+ Flow = 9, // 32F
+ Smoothing = 9, // 32F
+ Energy = 10, // 32F
Mask = 11, // 32U
Density = 12, // 32F
Support1 = 13, // 8UC4 (currently)
Support2 = 14, // 8UC4 (currently)
- Segmentation = 15, // 32S?
- ColourNormals = 16, // 8UC4
+ Segmentation = 15, // 32S?
+ ColourNormals = 16, // 8UC4
+ ColourHighRes = 20, // 8UC3 or 8UC4
AudioLeft = 32,
AudioRight = 33,
@@ -39,7 +40,7 @@ enum struct Channel : int {
Configuration = 64, // JSON Data
Calibration = 65, // Camera Parameters Object
Pose = 66, // Eigen::Matrix4d
- Index = 67,
+ Index = 67,
Data = 2048 // Custom data, any codec.
};
@@ -51,7 +52,7 @@ std::string name(Channel c);
int type(Channel c);
class Channels {
- public:
+ public:
class iterator {
public:
@@ -67,48 +68,48 @@ class Channels {
unsigned int ix_;
};
- inline Channels() { mask = 0; }
- inline explicit Channels(unsigned int m) { mask = m; }
- inline explicit Channels(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); }
- inline Channels &operator=(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); return *this; }
- inline Channels operator|(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
- inline Channels operator+(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
- inline Channels &operator|=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
- inline Channels &operator+=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
- inline Channels &operator-=(Channel c) { mask &= ~((c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c))); return *this; }
- inline Channels &operator+=(unsigned int c) { mask |= (0x1 << c); return *this; }
- inline Channels &operator-=(unsigned int c) { mask &= ~(0x1 << c); return *this; }
-
- inline bool has(Channel c) const {
- return (c == Channel::None) ? true : mask & (0x1 << static_cast<unsigned int>(c));
- }
-
- inline bool has(unsigned int c) const {
- return mask & (0x1 << c);
- }
+ inline Channels() { mask = 0; }
+ inline explicit Channels(unsigned int m) { mask = m; }
+ inline explicit Channels(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); }
+ inline Channels &operator=(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); return *this; }
+ inline Channels operator|(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
+ inline Channels operator+(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
+ inline Channels &operator|=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
+ inline Channels &operator+=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
+ inline Channels &operator-=(Channel c) { mask &= ~((c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c))); return *this; }
+ inline Channels &operator+=(unsigned int c) { mask |= (0x1 << c); return *this; }
+ inline Channels &operator-=(unsigned int c) { mask &= ~(0x1 << c); return *this; }
+
+ inline bool has(Channel c) const {
+ return (c == Channel::None) ? true : mask & (0x1 << static_cast<unsigned int>(c));
+ }
+
+ inline bool has(unsigned int c) const {
+ return mask & (0x1 << c);
+ }
inline iterator begin() { return iterator(*this, 0); }
inline iterator end() { return iterator(*this, 32); }
- inline operator unsigned int() { return mask; }
- inline operator bool() { return mask > 0; }
- inline operator Channel() {
- if (mask == 0) return Channel::None;
- int ix = 0;
- int tmask = mask;
- while (!(tmask & 0x1) && ++ix < 32) tmask >>= 1;
- return static_cast<Channel>(ix);
- }
-
- inline size_t count() { return std::bitset<32>(mask).count(); }
- inline void clear() { mask = 0; }
-
- static const size_t kMax = 32;
+ inline operator unsigned int() { return mask; }
+ inline operator bool() { return mask > 0; }
+ inline operator Channel() {
+ if (mask == 0) return Channel::None;
+ int ix = 0;
+ int tmask = mask;
+ while (!(tmask & 0x1) && ++ix < 32) tmask >>= 1;
+ return static_cast<Channel>(ix);
+ }
+
+ inline size_t count() { return std::bitset<32>(mask).count(); }
+ inline void clear() { mask = 0; }
+
+ static const size_t kMax = 32;
static Channels All();
- private:
- unsigned int mask;
+ private:
+ unsigned int mask;
};
inline Channels::iterator Channels::iterator::operator++() { Channels::iterator i = *this; while (++ix_ < 32 && !channels_.has(ix_)); return i; }
@@ -124,9 +125,9 @@ static const Channels kAllChannels(0xFFFFFFFFu);
inline bool isFloatChannel(ftl::codecs::Channel chan) {
switch (chan) {
case Channel::Depth :
- //case Channel::Normals :
+ //case Channel::Normals :
case Channel::Confidence:
- case Channel::Flow :
+ case Channel::Flow :
case Channel::Density:
case Channel::Energy : return true;
default : return false;
@@ -139,11 +140,11 @@ inline bool isFloatChannel(ftl::codecs::Channel chan) {
MSGPACK_ADD_ENUM(ftl::codecs::Channel);
inline ftl::codecs::Channels operator|(ftl::codecs::Channel a, ftl::codecs::Channel b) {
- return ftl::codecs::Channels(a) | b;
+ return ftl::codecs::Channels(a) | b;
}
inline ftl::codecs::Channels operator+(ftl::codecs::Channel a, ftl::codecs::Channel b) {
- return ftl::codecs::Channels(a) | b;
+ return ftl::codecs::Channels(a) | b;
}
#endif // _FTL_RGBD_CHANNELS_HPP_
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index fc99d701fa40a8b824248c775a7020ed7d449fd1..88d69ab9c6d2fdc23bcd8f629df3e8041d6a75d9 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -211,6 +211,26 @@ void Calibrate::_updateIntrinsics() {
map2_gpu_.second.upload(map2_.second);
}
+cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
+ double scale_x = ((double) res.width) / ((double) img_size_.width);
+ double scale_y = ((double) res.height) / ((double) img_size_.height);
+ Mat scale(cv::Size(3, 3), CV_64F, 0.0);
+ scale.at<double>(0, 0) = scale_x;
+ scale.at<double>(1, 1) = scale_y;
+ scale.at<double>(2, 2) = 1.0;
+ return scale * Kl_;
+}
+
+cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
+ double scale_x = ((double) res.width) / ((double) img_size_.width);
+ double scale_y = ((double) res.height) / ((double) img_size_.height);
+ Mat scale(cv::Size(3, 3), CV_64F, 0.0);
+ scale.at<double>(0, 0) = scale_x;
+ scale.at<double>(1, 1) = scale_y;
+ scale.at<double>(2, 2) = 1.0;
+ return scale * Kr_;
+}
+
void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) {
// cv::cuda::remap() can not use same Mat for input and output
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 4561b90a79129dd5a0d46d9d54bd005147d766a7..a30abce1fc96b10198d76a09f03da37df26f1bcb 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -55,6 +55,10 @@ class Calibrate : public ftl::Configurable {
*/
const cv::Mat &getQ() const { return Q_; }
+ cv::Mat getCameraMatrixLeft(const cv::Size res);
+ cv::Mat getCameraMatrixRight(const cv::Size res);
+
+ // TODO: Remove. Always require resolution for correct scaling
const cv::Mat &getCameraMatrixLeft() { return Kl_; }
const cv::Mat &getCameraMatrixRight() { return Kr_; }
const cv::Mat &getCameraMatrix() { return getCameraMatrixLeft(); }
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index c2ecf8ff17513d1eb774c5ecf12473ff152b0f5e..9a6f6bf302f740478485fadb3dcafd7a90970381 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -96,7 +96,7 @@ void StereoVideoSource::init(const string &file) {
if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
// Generate camera parameters from camera matrix
- cv::Mat K = calib_->getCameraMatrix();
+ cv::Mat K = calib_->getCameraMatrixLeft(depth_size_);
params_ = {
K.at<double>(0,0), // Fx
K.at<double>(1,1), // Fy
@@ -152,7 +152,7 @@ void StereoVideoSource::init(const string &file) {
ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
if (chan == Channel::Right) {
- cv::Mat q = calib_->getCameraMatrixRight();
+ cv::Mat q = calib_->getCameraMatrixRight(depth_size_);
ftl::rgbd::Camera params = {
q.at<double>(0,0), // Fx
q.at<double>(1,1), // Fy