Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include <glog/logging.h>
#include <ftl/stereovideo_source.hpp>
#include <ftl/configuration.hpp>
#include "calibrate.hpp"
#include "local.hpp"
#include "disparity.hpp"
using ftl::Calibrate;
using ftl::LocalSource;
using ftl::rgbd::StereoVideoSource;
using std::string;
StereoVideoSource::StereoVideoSource(nlohmann::json &config, ftl::net::Universe *net)
: RGBDSource(config, net) {
}
StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
: RGBDSource(config), ready_(false) {
REQUIRED({
{"source","Details on source video [object]","object"}
});
if (ftl::is_video(file)) {
// Load video file
LOG(INFO) << "Using video file...";
lsrc_ = new LocalSource(file, config["source"]);
} else if (file != "") {
auto vid = ftl::locateFile("video.mp4");
if (!vid) {
LOG(FATAL) << "No video.mp4 file found in provided paths";
} else {
LOG(INFO) << "Using test directory...";
lsrc_ = new LocalSource(*vid, config["source"]);
}
} else {
// Use cameras
LOG(INFO) << "Using cameras...";
lsrc_ = new LocalSource(config["source"]);
}
calib_ = new Calibrate(lsrc_, config["calibration"]);
if (config["calibrate"]) calib_->recalibrate();
if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
else LOG(INFO) << "Calibration initiated.";
// Generate camera parameters from Q matrix
// TODO(Nick) Add fx and fy
q.at<double>(0,0), // F
q.at<double>(0,2), // Cx
q.at<double>(1,2), // Cy
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
(unsigned int)left_.cols, // TODO (Nick)
(unsigned int)left_.rows,
0.0f, // 0m min
15.0f // 15m max
};
disp_ = Disparity::create(config["disparity"]);
if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
LOG(INFO) << "StereoVideo source ready...";
ready_ = true;
}
StereoVideoSource::~StereoVideoSource() {
delete disp_;
delete calib_;
delete lsrc_;
}
void StereoVideoSource::grab() {
calib_->rectified(left_, right_);
}
bool StereoVideoSource::isReady() {
return ready_;
}
static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv::Mat &q) {
cv::Matx44d _Q;
q.convertTo(_Q, CV_64F);
if (depth.empty()) depth = cv::Mat(disparity.size(), CV_32F);
for( int y = 0; y < disparity.rows; y++ ) {
const float *sptr = disparity.ptr<float>(y);
float *dptr = depth.ptr<float>(y);
for( int x = 0; x < disparity.cols; x++ ) {
double d = sptr[x];
cv::Vec4d homg_pt = _Q*cv::Vec4d(x, y, d, 1.0);
//dptr[x] = Vec3d(homg_pt.val);
//dptr[x] /= homg_pt[3];
dptr[x] = (homg_pt[2] / homg_pt[3]) / 1000.0f; // Depth in meters
if( fabs(d) <= FLT_EPSILON )
dptr[x] = 1000.0f;
}
}
}
void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
cv::Mat disp;
disp_->compute(left_, right_, disp);
rgb = left_;
disparityToDepth(disp, depth, calib_->getQ());