diff --git a/components/operators/include/ftl/operators/mvmls.hpp b/components/operators/include/ftl/operators/mvmls.hpp
index 6b8eff1be304693cfe7dee96152f9aa4cd3452b4..1c250e98574d1bac9182aa950ec7fbda594c5b2e 100644
--- a/components/operators/include/ftl/operators/mvmls.hpp
+++ b/components/operators/include/ftl/operators/mvmls.hpp
@@ -16,10 +16,10 @@ class MultiViewMLS : public ftl::operators::Operator {
bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
private:
- std::vector<ftl::cuda::TextureObject<float4>> centroid_horiz_;
- std::vector<ftl::cuda::TextureObject<float4>> centroid_vert_;
- std::vector<ftl::cuda::TextureObject<float4>> normals_horiz_;
- std::vector<ftl::cuda::TextureObject<float>> contributions_;
+ std::vector<ftl::cuda::TextureObject<float4>*> centroid_horiz_;
+ std::vector<ftl::cuda::TextureObject<float4>*> centroid_vert_;
+ std::vector<ftl::cuda::TextureObject<float4>*> normals_horiz_;
+ std::vector<ftl::cuda::TextureObject<float>*> contributions_;
};
}
diff --git a/components/operators/src/mvmls.cpp b/components/operators/src/mvmls.cpp
index 305bdd98f539cacb39ce063b6f7d3ff1e65af99e..de69a0691441c7661dc3ae23d51639440a83654f 100644
--- a/components/operators/src/mvmls.cpp
+++ b/components/operators/src/mvmls.cpp
@@ -41,22 +41,25 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
params.cost_ratio = config()->value("cost_ratio", 0.2f);
params.cost_threshold = config()->value("cost_threshold", 1.0f);
+ if (in.frames.size() < 1) return false;
+ auto size = in.frames[0].get<GpuMat>(Channel::Depth).size();
+
// Make sure we have enough buffers
- if (normals_horiz_.size() < in.frames.size()) {
- normals_horiz_.resize(in.frames.size());
- centroid_horiz_.resize(in.frames.size());
- centroid_vert_.resize(in.frames.size());
- contributions_.resize(in.frames.size());
+ while (normals_horiz_.size() < in.frames.size()) {
+ normals_horiz_.push_back(new ftl::cuda::TextureObject<float4>(size.height, size.width));
+ centroid_horiz_.push_back(new ftl::cuda::TextureObject<float4>(size.height, size.width));
+ centroid_vert_.push_back(new ftl::cuda::TextureObject<float4>(size.width, size.height));
+ contributions_.push_back(new ftl::cuda::TextureObject<float>(size.width, size.height));
}
// Make sure all buffers are at correct resolution and are allocated
for (size_t i=0; i<in.frames.size(); ++i) {
auto &f = in.frames[i];
auto size = f.get<GpuMat>(Channel::Depth).size();
- centroid_horiz_[i].create(size.height, size.width);
- normals_horiz_[i].create(size.height, size.width);
- centroid_vert_[i].create(size.width, size.height);
- contributions_[i].create(size.width, size.height);
+ centroid_horiz_[i]->create(size.height, size.width);
+ normals_horiz_[i]->create(size.height, size.width);
+ centroid_vert_[i]->create(size.width, size.height);
+ contributions_[i]->create(size.width, size.height);
if (!f.hasChannel(Channel::Normals)) {
throw FTL_Error("Required normals channel missing for MLS");
@@ -201,8 +204,8 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
//auto *s = in.sources[i];
// Clear data
- cv::cuda::GpuMat data(contributions_[i].height(), contributions_[i].width(), CV_32F, contributions_[i].pixelPitch());
- data.setTo(cv::Scalar(0.0f), cvstream);
+ //cv::cuda::GpuMat data(contributions_[i]->height(), contributions_[i]->width(), CV_32F, contributions_[i]->devicePtr(), contributions_[i]->pixelPitch());
+ //data.setTo(cv::Scalar(0.0f), cvstream);
if (cull_zero && iter == iters-1) {
ftl::cuda::zero_confidence(
@@ -215,9 +218,9 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
ftl::cuda::mls_aggr_horiz(
f.createTexture<uchar4>(Channel::Support2),
f.createTexture<float4>(Channel::Normals),
- normals_horiz_[i],
+ *normals_horiz_[i],
f.createTexture<float>(Channel::Depth),
- centroid_horiz_[i],
+ *centroid_horiz_[i],
f.createTexture<uchar4>(Channel::Colour),
thresh,
col_smooth,
@@ -228,10 +231,10 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
ftl::cuda::mls_aggr_vert(
f.getTexture<uchar4>(Channel::Support2),
- normals_horiz_[i],
+ *normals_horiz_[i],
f.getTexture<float4>(Channel::Normals),
- centroid_horiz_[i],
- centroid_vert_[i],
+ *centroid_horiz_[i],
+ *centroid_vert_[i],
f.getTexture<uchar4>(Channel::Colour),
f.getTexture<float>(Channel::Depth),
thresh,
@@ -242,6 +245,9 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
);
}
+ //return true;
+
+
// Step 3:
// Find corresponding points and perform aggregation of any correspondences
// For each camera combination
@@ -280,8 +286,8 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
ftl::cuda::aggregate_sources(
f1.getTexture<float4>(Channel::Normals),
f2.getTexture<float4>(Channel::Normals),
- centroid_vert_[i],
- centroid_vert_[j],
+ *centroid_vert_[i],
+ *centroid_vert_[j],
f1.getTexture<float>(Channel::Depth),
//contributions_[i],
//contributions_[j],
@@ -315,7 +321,7 @@ bool MultiViewMLS::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cuda
ftl::cuda::mls_adjust_depth(
f.getTexture<float4>(Channel::Normals),
- centroid_vert_[i],
+ *centroid_vert_[i],
f.getTexture<float>(Channel::Depth),
f.createTexture<float>(Channel::Depth2, ftl::rgbd::Format<float>(size)),
f.getLeftCamera(),
diff --git a/components/operators/src/operator.cpp b/components/operators/src/operator.cpp
index 080d5c1707c5e7c578746ab7438ae2bf3ee9c35e..9b87fe1d0ae0b0312852bf19b90d62c94d247de3 100644
--- a/components/operators/src/operator.cpp
+++ b/components/operators/src/operator.cpp
@@ -69,7 +69,9 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
try {
instance->apply(in.frames[j], out.frames[j], stream_actual);
} catch (const std::exception &e) {
- LOG(ERROR) << "Operator exception: " << e.what();
+ LOG(ERROR) << "Operator exception for '" << instance->config()->getID() << "': " << e.what();
+ cudaSafeCall(cudaStreamSynchronize(stream_actual));
+ return false;
}
}
}
@@ -80,7 +82,9 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
try {
instance->apply(in, out, stream_actual);
} catch (const std::exception &e) {
- LOG(ERROR) << "Operator exception: " << e.what();
+ LOG(ERROR) << "Operator exception for '" << instance->config()->getID() << "': " << e.what();
+ cudaSafeCall(cudaStreamSynchronize(stream_actual));
+ return false;
}
}
}
@@ -88,7 +92,7 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
if (stream == 0) {
cudaSafeCall(cudaStreamSynchronize(stream_actual));
- cudaSafeCall( cudaGetLastError() );
+ //cudaSafeCall( cudaGetLastError() );
}
return true;
@@ -111,7 +115,9 @@ bool Graph::apply(Frame &in, Frame &out, cudaStream_t stream) {
try {
instance->apply(in, out, stream_actual);
} catch (const std::exception &e) {
- LOG(ERROR) << "Operator exception: " << e.what();
+ LOG(ERROR) << "Operator exception for '" << instance->config()->getID() << "': " << e.what();
+ cudaSafeCall(cudaStreamSynchronize(stream_actual));
+ return false;
}
}
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
index 92ac321ebd2a3915ab40a51af80ac52930f76ad0..a1e5396622017622a5de9dceed80059012b7106a 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
@@ -136,6 +136,7 @@ class Builder : public Generator {
float fps_;
int stats_count_;
int64_t last_ts_;
+ int64_t last_frame_;
std::atomic<int> jobs_;
volatile bool skip_;
ftl::timer::TimerHandle main_id_;
diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp
index 48da2b5dc34b0399cc640485781c19b0d2e429b4..740af42ff10cb35a6af7aab29ecf83be2eca5979 100644
--- a/components/rgbd-sources/src/frameset.cpp
+++ b/components/rgbd-sources/src/frameset.cpp
@@ -64,6 +64,7 @@ Builder::Builder() : head_(0) {
skip_ = false;
//setFPS(20);
size_ = 0;
+ last_frame_ = 0;
mspf_ = ftl::timer::getInterval();
name_ = "NoName";
@@ -71,6 +72,8 @@ Builder::Builder() : head_(0) {
latency_ = 0.0f;;
stats_count_ = 0;
fps_ = 0.0f;
+
+ if (size_ > 0) states_.resize(size_);
}
Builder::~Builder() {
@@ -84,7 +87,7 @@ Builder::~Builder() {
}
ftl::rgbd::Frame &Builder::get(int64_t timestamp, size_t ix) {
- if (timestamp <= 0 || ix >= kMaxFramesInSet) throw ftl::exception("Invalid frame timestamp or index");
+ if (timestamp <= 0 || ix >= kMaxFramesInSet) throw FTL_Error("Invalid frame timestamp or index");
UNIQUE_LOCK(mutex_, lk);
@@ -98,15 +101,27 @@ ftl::rgbd::Frame &Builder::get(int64_t timestamp, size_t ix) {
}
//states_[ix] = frame.origin();
+ if (timestamp <= last_frame_) {
+ throw FTL_Error("Frameset already completed");
+ }
+
auto *fs = _findFrameset(timestamp);
if (!fs) {
// Add new frameset
fs = _addFrameset(timestamp);
- if (!fs) throw ftl::exception("Could not add frameset");
+ if (!fs) throw FTL_Error("Could not add frameset");
+ }
+
+ if (fs->stale) {
+ throw FTL_Error("Frameset already completed");
+ }
+
+ if (ix >= fs->frames.size()) {
+ throw FTL_Error("Frame index out-of-bounds");
}
- if (fs->frames.size() < size_) fs->frames.resize(size_);
+ //if (fs->frames.size() < size_) fs->frames.resize(size_);
//lk.unlock();
//SHARED_LOCK(fs->mtx, lk2);
@@ -159,7 +174,7 @@ void Builder::_schedule() {
if (fs) {
//UNIQUE_LOCK(fs->mtx, lk2);
// The buffers are invalid after callback so mark stale
- fs->stale = true;
+ //fs->stale = true;
jobs_++;
//lk.unlock();
@@ -310,6 +325,9 @@ ftl::rgbd::FrameSet *Builder::_getFrameset() {
if (!f->stale && static_cast<unsigned int>(f->count) >= size_) {
//LOG(INFO) << "GET FRAMESET and remove: " << f->timestamp;
auto j = framesets_.erase(i);
+
+ last_frame_ = f->timestamp;
+ f->stale = true;
int count = 0;
// Merge all previous frames
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index bccd8ce4f517fc23c43e425521ff1ef7b83725d1..37e6467f0578cb7c27d4fb51d7a89acd3b792555 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -194,7 +194,8 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
// Now split the tiles from surface into frames, doing colour conversions
// at the same time.
- for (int i=0; i<pkt.frame_count; ++i) {
+ // Note: Done in reverse to allocate correct number of frames first time round
+ for (int i=pkt.frame_count-1; i>=0; --i) {
InternalVideoStates &vidstate = _getVideoFrame(spkt,i);
auto &frame = builder_.get(spkt.timestamp, spkt.frame_number+i);