Newer
Older
#include <ftl/operators/disparity.hpp>
#include "ftl/operators/smoothing.hpp"
#include "ftl/operators/colours.hpp"
#include "ftl/operators/normals.hpp"
#include "ftl/operators/filling.hpp"
#include "ftl/operators/segmentation.hpp"
#include "ftl/operators/disparity.hpp"
#include "ftl/operators/mask.hpp"
#include "./disparity/opencv/disparity_bilateral_filter.hpp"
using ftl::operators::DepthChannel;
using ftl::codecs::Channel;
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
49
50
51
52
53
54
55
56
57
58
59
using cv::Mat;
using cv::cuda::GpuMat;
static void calc_color_weighted_table(GpuMat& table_color, float sigma_range, int len)
{
Mat cpu_table_color(1, len, CV_32F);
float* line = cpu_table_color.ptr<float>();
for(int i = 0; i < len; i++)
line[i] = static_cast<float>(std::exp(-double(i * i) / (2 * sigma_range * sigma_range)));
table_color.upload(cpu_table_color);
}
static void calc_space_weighted_filter(GpuMat& table_space, int win_size, float dist_space)
{
int half = (win_size >> 1);
Mat cpu_table_space(half + 1, half + 1, CV_32F);
for (int y = 0; y <= half; ++y)
{
float* row = cpu_table_space.ptr<float>(y);
for (int x = 0; x <= half; ++x)
row[x] = exp(-sqrt(float(y * y) + float(x * x)) / dist_space);
}
table_space.upload(cpu_table_space);
}
// ==== Depth Bilateral Filter =================================================
DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg) :
ftl::operators::Operator(cfg) {
scale_ = 16.0;
radius_ = cfg->value("radius", 7);
iter_ = cfg->value("iter", 2);
sigma_range_ = 10.0f;
edge_disc_ = cfg->value("edge_discontinuity", 0.04f);
max_disc_ = cfg->value("max_discontinuity", 0.1f);
channel_ = Channel::Depth;
cfg->on("edge_discontinuity", [this](const ftl::config::Event &e) {
edge_disc_ = config()->value("edge_discontinuity", 0.04f);
});
cfg->on("max_discontinuity", [this](const ftl::config::Event &e) {
max_disc_ = config()->value("max_discontinuity", 0.1f);
});
calc_color_weighted_table(table_color_, sigma_range_, 255);
calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
}
DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg, const std::tuple<ftl::codecs::Channel> &p) :
ftl::operators::Operator(cfg) {
scale_ = 16.0;
radius_ = cfg->value("radius", 7);
iter_ = cfg->value("iter", 2);
sigma_range_ = 10.0f;
edge_disc_ = cfg->value("edge_discontinuity", 0.04f);
max_disc_ = cfg->value("max_discontinuity", 0.1f);
channel_ = std::get<0>(p);
cfg->on("edge_discontinuity", [this](const ftl::config::Event &e) {
edge_disc_ = config()->value("edge_discontinuity", 0.04f);
});
cfg->on("max_discontinuity", [this](const ftl::config::Event &e) {
max_disc_ = config()->value("max_discontinuity", 0.1f);
});
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
calc_color_weighted_table(table_color_, sigma_range_, 255);
calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
}
bool DepthBilateralFilter::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
cudaStream_t stream) {
if (!in.hasChannel(Channel::Colour)) {
LOG(ERROR) << "Joint Bilateral Filter is missing Colour";
return false;
} else if (!in.hasChannel(Channel::Depth)) {
LOG(ERROR) << "Joint Bilateral Filter is missing Depth";
return false;
}
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
const GpuMat &rgb = in.get<GpuMat>(Channel::Colour);
GpuMat &depth = in.get<GpuMat>(channel_);
ftl::cuda::device::disp_bilateral_filter::disp_bilateral_filter<float>(depth, rgb, rgb.channels(), iter_,
table_color_.ptr<float>(), (float *)table_space_.data, table_space_.step / sizeof(float),
radius_, edge_disc_, max_disc_, stream);
//disp_in.convertTo(disp_int_, CV_16SC1, scale_, cvstream);
//filter_->apply(disp_in, rgb, disp_out, cvstream);
//disp_int_result_.convertTo(disp_out, disp_in.type(), 1.0/scale_, cvstream);
return true;
}
// =============================================================================
DepthChannel::DepthChannel(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
pipe_ = nullptr;
}
DepthChannel::~DepthChannel() {
}
void DepthChannel::_createPipeline() {
if (pipe_ != nullptr) return;
pipe_ = ftl::config::create<ftl::operators::Graph>(config(), "depth");
depth_size_ = cv::Size( config()->value("width", 1280),
config()->value("height", 720));
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
pipe_->append<ftl::operators::ColourChannels>("colour"); // Convert BGR to BGRA
pipe_->append<ftl::operators::FixstarsSGM>("algorithm");
#ifdef HAVE_OPTFLOW
pipe_->append<ftl::operators::OpticalFlowTemporalSmoothing>("optflow_filter");
#endif
pipe_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter");
pipe_->append<ftl::operators::DisparityToDepth>("calculate_depth");
pipe_->append<ftl::operators::Normals>("normals"); // Estimate surface normals
pipe_->append<ftl::operators::CrossSupport>("cross");
pipe_->append<ftl::operators::DiscontinuityMask>("discontinuity_mask");
pipe_->append<ftl::operators::AggreMLS>("mls"); // Perform MLS (using smoothing channel)
}
bool DepthChannel::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
rbuf_.resize(in.frames.size());
for (int i=0; i<in.frames.size(); ++i) {
auto &f = in.frames[i];
if (!f.hasChannel(Channel::Depth) && f.hasChannel(Channel::Right)) {
_createPipeline();
cv::cuda::GpuMat& left = f.get<cv::cuda::GpuMat>(Channel::Left);
cv::cuda::GpuMat& right = f.get<cv::cuda::GpuMat>(Channel::Right);
cv::cuda::GpuMat& depth = f.create<cv::cuda::GpuMat>(Channel::Depth);
depth.create(depth_size_, CV_32FC1);
if (left.empty() || right.empty()) continue;
/*if (depth_size_ != left.size()) {
auto &col2 = f.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
cv::cuda::resize(left, col2, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
f.createTexture<uchar4>(Channel::ColourHighRes, true);
f.swapChannels(Channel::Colour, Channel::ColourHighRes);
}
if (depth_size_ != right.size()) {
cv::cuda::resize(right, rbuf_[i], depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
cv::cuda::swap(right, rbuf_[i]);
}*/
}
}
return true;
}
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
bool DepthChannel::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) {
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
//rbuf_.resize(1);
auto &f = in;
if (!f.hasChannel(Channel::Depth) && f.hasChannel(Channel::Right)) {
_createPipeline();
cv::cuda::GpuMat& left = f.get<cv::cuda::GpuMat>(Channel::Left);
cv::cuda::GpuMat& right = f.get<cv::cuda::GpuMat>(Channel::Right);
cv::cuda::GpuMat& depth = f.create<cv::cuda::GpuMat>(Channel::Depth);
depth.create(depth_size_, CV_32FC1);
if (left.empty() || right.empty()) return false;
/*if (depth_size_ != left.size()) {
auto &col2 = f.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
cv::cuda::resize(left, col2, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
f.createTexture<uchar4>(Channel::ColourHighRes, true);
f.swapChannels(Channel::Colour, Channel::ColourHighRes);
}
if (depth_size_ != right.size()) {
cv::cuda::resize(right, rbuf_[i], depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
cv::cuda::swap(right, rbuf_[i]);
}*/
pipe_->apply(f, f, stream);
}
return true;
}