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
49
50
51
52
53
54
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
110
111
112
#include <ftl/rgbd/group.hpp>
#include <ftl/rgbd/source.hpp>
using ftl::rgbd::Group;
using ftl::rgbd::Source;
using ftl::rgbd::kFrameBufferSize;
using std::vector;
Group::Group() : framesets_(kFrameBufferSize), head_(0) {
framesets_[0].timestamp = -1;
}
Group::~Group() {
}
void Group::addSource(ftl::rgbd::Source *src) {
UNIQUE_LOCK(mutex_, lk);
size_t ix = sources_.size();
sources_.push_back(src);
src->setCallback([this,ix](int64_t timestamp, const cv::Mat &rgb, const cv::Mat &depth) {
if (timestamp == 0) return;
UNIQUE_LOCK(mutex_, lk);
if (timestamp > framesets_[head_].timestamp) {
// Add new frameset
_addFrameset(timestamp);
} else if (framesets_[(head_+1)%kFrameBufferSize].timestamp > timestamp) {
// Too old, just ditch it
LOG(WARNING) << "Received frame too old for buffer";
return;
}
// Search backwards to find match
for (size_t i=0; i<kFrameBufferSize; ++i) {
FrameSet &fs = framesets_[(head_+kFrameBufferSize-i) % kFrameBufferSize];
if (fs.timestamp == timestamp) {
//LOG(INFO) << "Adding frame: " << ix << " for " << timestamp;
rgb.copyTo(fs.channel1[ix]);
depth.copyTo(fs.channel2[ix]);
++fs.count;
fs.mask |= (1 << ix);
if (callback_ && fs.count == sources_.size()) {
//LOG(INFO) << "DOING CALLBACK";
if (callback_(fs)) {
//sources_[ix]->grab();
//LOG(INFO) << "GRAB";
}
}
return;
}
}
LOG(WARNING) << "Frame timestamp not found in buffer";
});
}
// TODO: This should be a callback
// Callback returns true if it wishes to continue receiving frames.
void Group::sync(int N, int B) {
for (auto s : sources_) {
s->grab(N,B);
}
}
void Group::sync(std::function<bool(const ftl::rgbd::FrameSet &)> cb) {
callback_ = cb;
sync(-1,-1);
}
bool Group::getFrames(ftl::rgbd::FrameSet &fs, bool complete) {
// Use oldest frameset or search back until first complete set is found?
if (complete) {
UNIQUE_LOCK(mutex_, lk);
// Search backwards to find match
for (size_t i=0; i<kFrameBufferSize; ++i) {
FrameSet &f = framesets_[(head_+kFrameBufferSize-i) % kFrameBufferSize];
if (f.count == sources_.size()) {
LOG(INFO) << "Complete set found";
fs = f; // FIXME: This needs to move or copy safely...
return true;
}
}
LOG(WARNING) << "No complete frame set found";
return false;
}
return false;
}
void Group::_addFrameset(int64_t timestamp) {
int count = (framesets_[head_].timestamp == -1) ? 1 : (timestamp - framesets_[head_].timestamp) / 40;
// Must make sure to also insert missing framesets
//LOG(INFO) << "Adding " << count << " framesets for " << timestamp << " head=" << framesets_[head_].timestamp;
//if (count > 10 || count < 1) return;
for (int i=0; i<count; ++i) {
int64_t lt = (framesets_[head_].timestamp == -1) ? timestamp-40 : framesets_[head_].timestamp;
head_ = (head_+1) % kFrameBufferSize;
framesets_[head_].timestamp = lt+40;
framesets_[head_].count = 0;
framesets_[head_].mask = 0;
framesets_[head_].channel1.resize(sources_.size());
framesets_[head_].channel2.resize(sources_.size());
for (auto s : sources_) framesets_[head_].sources.push_back(s);
}
}