Merge branch 'feature/occlusion-background-filling' into 'master'

CBSegmentation algorithm

See merge request nicolas.pope/ftl!50

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)

components/rgbd-sources/include/ftl/cb_segmentation.hpp

+#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

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 {
 

components/rgbd-sources/src/cb_segmentation.cpp

+#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_++;
+}