diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 33f555f92e5eddf2c58d0385d411f0b267918426..5329ac9f2892c172af9cbc5f7fdc6b0e0e82360f 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -12,6 +12,7 @@ set(RGBDSRC
# src/algorithms/rtcensus_sgm.cpp
# src/algorithms/opencv_sgbm.cpp
# src/algorithms/opencv_bm.cpp
+ src/cb_segmentation.cpp
)
if (HAVE_REALSENSE)
diff --git a/components/rgbd-sources/include/ftl/cb_segmentation.hpp b/components/rgbd-sources/include/ftl/cb_segmentation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..75ec0bdc4c7c7c8c67ee77a05e808fb439961f8d
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/cb_segmentation.hpp
@@ -0,0 +1,117 @@
+#pragma once
+
+#include <opencv2/core.hpp>
+
+namespace ftl {
+
+/**
+ * @brief Codebook segmentation and depthmap filling.
+ * @param Input image width
+ * @param Input image height
+ *
+ * Codebook segmentation based on
+ *
+ * Kim, K., Chalidabhongse, T. H., Harwood, D., & Davis, L. (2005).
+ * Real-time foreground-background segmentation using codebook model.
+ * Real-Time Imaging. https://doi.org/10.1016/j.rti.2004.12.004
+ *
+ * and fixed size codebook optimization in
+ *
+ * Rodriguez-Gomez, R., Fernandez-Sanchez, E. J., Diaz, J., & Ros, E.
+ * (2015). Codebook hardware implementation on FPGA for background
+ * subtraction. Journal of Real-Time Image Processing.
+ * https://doi.org/10.1007/s11554-012-0249-6
+ *
+ * Additional modifications to include depth maps as part of the
+ * background model.
+ */
+class CBSegmentation {
+public:
+ CBSegmentation(char codebook_size, size_t width, size_t height, float alpha, float beta, float epsilon, float sigma, int T_add, int T_del, int T_h);
+
+ /**
+ * @brief Segment image.
+ * @param Input image (3-channels)
+ * @param Output Mat. Background pixels set to 0, foreground pixels > 0.
+ *
+ * @todo Template method on OpenCV type
+ */
+ void apply(cv::Mat &in, cv::Mat &out, cv::Mat &depth, bool fill=false);
+ void apply(cv::Mat &in, cv::Mat &out);
+
+protected:
+ class Pixel {
+ public:
+ int idx;
+ float r;
+ float g;
+ float b;
+ float i;
+ int d;
+ long t;
+ Pixel(const int &index, const uchar *bgr, const int &depth, const long &time);
+ };
+
+ class Codeword {
+ public:
+ float r;
+ float g;
+ float b;
+ float i_min, i_max;
+ long f, lambda, p, q;
+
+ float d_m;
+ float d_f;
+ float d_S;
+
+ void set(CBSegmentation::Pixel &pixel);
+ void update(CBSegmentation::Pixel &pixel);
+
+ bool colordiff(CBSegmentation::Pixel &pixel, float epsilon);
+ bool brightness(CBSegmentation::Pixel &pixel, float alpha, float beta);
+ bool depthdiff(CBSegmentation::Pixel &pixel, float sigma);
+
+ inline int freq() { return f; }
+ inline long getLambda() { return lambda; }
+ inline long ctime() { return p; }
+ inline long atime() { return q; }
+ };
+
+ enum EntryType { H, M };
+
+ union Entry {
+ char size;
+ struct Data {
+ EntryType type;
+ CBSegmentation::Codeword cw;
+ } data ;
+ };
+
+ struct CompareEntry{
+ bool operator()(const Entry &a,const Entry &b) const{
+ return !((a.data.type == M && b.data.type == H) ||
+ (a.data.cw.f < b.data.cw.f));
+ }
+ };
+
+ bool processPixel(Pixel &px, Codeword *codeword=nullptr);
+
+ size_t width_;
+ size_t height_;
+ size_t size_;
+
+ int T_h_;
+ int T_add_;
+ int T_del_;
+
+ float alpha_;
+ float beta_;
+ float epsilon_;
+ float sigma_;
+
+private:
+ long t_ = 1;
+ std::vector<Entry> cb_;
+};
+
+}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 200775cf3f91c26cdd459935fdf171842118752a..95511cb2cdc9cde452d6ffe47ec4e3c9fb99c2f9 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -12,6 +12,8 @@
#include <opencv2/cudastereo.hpp>
#include <ftl/configuration.hpp>
+#include "ftl/cb_segmentation.hpp"
+
namespace ftl {
namespace algorithms {
diff --git a/components/rgbd-sources/src/cb_segmentation.cpp b/components/rgbd-sources/src/cb_segmentation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6f5fc2b2093b25fda6fd4554eb6e4bc04eef472
--- /dev/null
+++ b/components/rgbd-sources/src/cb_segmentation.cpp
@@ -0,0 +1,216 @@
+#include "ftl/cb_segmentation.hpp"
+
+#include<algorithm>
+#include <math.h>
+
+using cv::Mat;
+using cv::Vec3f, cv::Vec4f;
+
+using std::vector;
+using std::min;
+using std::max;
+using std::pair;
+
+using namespace ftl;
+
+CBSegmentation::Pixel::Pixel(const int &index, const uchar *bgr, const int &depth, const long &time) {
+ idx = index;
+ r = bgr[2];
+ g = bgr[1];
+ b = bgr[0];
+ i = sqrt(r*r + g*g + b*b);
+ d = depth;
+ t = time;
+}
+
+
+void CBSegmentation::Codeword::set(CBSegmentation::Pixel &px) {
+ r = px.r;
+ g = px.g;
+ b = px.b;
+ i_min = px.i;
+ i_max = px.i;
+ f = px.t;
+ lambda = px.t - 1;
+ p = px.t;
+ q = px.t;
+
+ d_m = px.d;
+ d_S = 0.0;
+ d_f = 1.0;
+}
+
+void CBSegmentation::Codeword::update(CBSegmentation::Pixel &px) {
+ r = (f * r + px.r) / (f + 1);
+ g = (f * g + px.g) / (f + 1);
+ b = (f * b + px.b) / (f + 1);
+ i_min = min(px.i, i_min);
+ i_max = max(px.i, i_max);
+ f = f + 1;
+ lambda = max(lambda, px.t - q);
+ q = px.t;
+
+ if (false /*isValidDepth(px.d)*/) { // check value valid
+ float d_prev = d_m;
+ d_f = d_f + 1;
+ d_m = d_m + (px.d - d_m) / d_f;
+ d_S = d_S + (px.d - d_m) * (px.d - d_prev);
+ }
+}
+
+// eq (2) and BSG
+//
+bool CBSegmentation::Codeword::colordiff(CBSegmentation::Pixel &px, float epsilon) {
+ float x_2 = px.r * px.r + px.g * px.g + px.b * px.b;
+ float v_2 = r*r + g*g + b*b;
+ float xv_2 = pow(px.r * r + px.g * g + px.b * b, 2);
+ float p_2 = xv_2 / v_2;
+ return sqrt(x_2 - p_2) < epsilon;
+}
+
+// eq (3)
+// note: ||x_t|| in the article is equal to I defined in
+// "Algorithm for codebook construction"
+//
+bool CBSegmentation::Codeword::brightness(CBSegmentation::Pixel &px, float alpha, float beta) {
+ return true;
+ float i_low = alpha * i_max;
+ float i_hi = min(beta * i_max, i_min / alpha);
+ return (i_low <= px.i) && (px.i <= i_hi);
+}
+
+CBSegmentation::CBSegmentation(
+ char codebook_size, size_t width, size_t height,
+ float alpha, float beta, float epsilon, float sigma,
+ int T_h, int T_add, int T_del) :
+ size_(codebook_size + 1), width_(width), height_(height),
+ alpha_(alpha), beta_(beta), epsilon_(epsilon), sigma_(sigma),
+ T_h_(T_h), T_add_(T_add), T_del_(T_del) {
+
+ cb_ = vector<Entry>(width * height * size_);
+ for (size_t i = 0; i < cb_.size(); i += size_) {
+ cb_[i].size = 0;
+ }
+}
+
+bool CBSegmentation::processPixel(CBSegmentation::Pixel &px, CBSegmentation::Codeword *codeword) {
+ char &size = cb_[size_ * px.idx].size;
+ size_t idx_begin = size_ * px.idx + 1;
+
+ CBSegmentation::Entry::Data *start = &(cb_[idx_begin].data);
+ CBSegmentation::Entry::Data *entry = start;
+
+ CBSegmentation::Entry::Data *lru = nullptr;
+ CBSegmentation::Entry::Data *lfu = nullptr;
+
+ // TODO: benchmark sorting
+
+ // if value is found (in M or H), loop exits early and no further maintenance
+ // is done. Maintenance may happen when codeword is not found and all entries
+ // are evaluated.
+
+ for (int i = 0; i < size; i++) {
+ if (entry->type == M) {
+ // matching codeword, update and return
+ if (entry->cw.brightness(px, alpha_, beta_) && entry->cw.colordiff(px, epsilon_)) {
+ entry->cw.update(px);
+ codeword = &(entry->cw);
+ return true;
+ }
+
+ // delete (move last to here) if not accessed for longer time than T_del
+ if ((px.t - entry->cw.atime()) > T_del_) {
+ size--;
+ *entry = *(start + size);
+ //std::sort( cb_.begin() + idx_begin,
+ // cb_.begin() + idx_begin + size,
+ // CompareEntry());
+ continue;
+ }
+
+ // update LFU
+ if (!lfu || lfu->cw.freq() > entry->cw.freq()) {
+ lfu = entry;
+ }
+ }
+ else if (entry->type == H) {
+ // matching codeword, update and return
+ if (entry->cw.brightness(px, alpha_, beta_) && entry->cw.colordiff(px, epsilon_)) {
+ entry->cw.update(px);
+
+ // should be moved to M? if so, move and return true
+ if ((px.t - entry->cw.ctime()) > T_add_) {
+ entry->type = M;
+ //std::sort( cb_.begin() + idx_begin,
+ // cb_.begin() + idx_begin + size,
+ // CompareEntry());
+ return true;
+ }
+ else {
+ return false;
+ }
+ }
+
+ // delete if lambda lt T_h
+ if (entry->cw.getLambda() < T_h_) {
+ size--;
+ *entry = *(start + size);
+ continue;
+ }
+
+ // update LRU
+ if (!lru || lru->cw.atime() > entry->cw.atime()) {
+ lru = entry;
+ }
+ }
+ }
+
+ // not found, create new codeword (to empty position or lru h or lfu m)
+ // TODO: Should not prefer H codewords over M codewords?
+ if (size < (size_ - 1)) {
+ entry = start + size;
+ size++;
+ entry->type = H;
+ entry->cw.set(px);
+ }
+ else if (lru) {
+ lru->type = H;
+ lru->cw.set(px);
+ }
+ else {
+ lfu->type = H;
+ lfu->cw.set(px);
+ }
+
+ // sort anyways (frequencies may have changed during earlier iterations)
+ //std::sort(cb_.begin() + idx_begin, cb_.begin() + idx_begin + size, CompareEntry());
+
+ return false;
+}
+
+void CBSegmentation::apply(Mat &in, Mat &out) {
+ if ((out.rows != height_) || (out.cols != width_)
+ || (out.type() != CV_8UC1) || !out.isContinuous()) {
+ out = Mat(height_, width_, CV_8UC1, cv::Scalar(0));
+ }
+
+ // TODO: thread pool, queue N rows
+ #pragma omp parallel for
+ for (int y = 0; y < height_; ++y) {
+ size_t idx = y * width_;
+ uchar *ptr_in = in.ptr<uchar>(y);
+ uchar *ptr_out = out.ptr<uchar>(y);
+
+ for (int x = 0; x < width_; ++x, ++idx, ptr_in += 3) {
+ auto px = Pixel(idx, ptr_in, 0, t_);
+ if(processPixel(px)) {
+ ptr_out[x] = 0;
+ }
+ else {
+ ptr_out[x] = 255;
+ }
+ }
+ }
+
+ t_++;
+}