feature/facedetect

components/common/cpp/include/ftl/utility/msgpack.hpp

@@ -13,12 +13,12 @@ MSGPACK_API_VERSION_NAMESPACE(MSGPACK_DEFAULT_API_NS) {
 namespace adaptor {
 
 ////////////////////////////////////////////////////////////////////////////////
-// cv::Size
+// cv::Size_<T>
 
-template<>
-struct pack<cv::Size> {
+template<typename T>
+struct pack<cv::Size_<T>> {
 	template <typename Stream>
-	packer<Stream>& operator()(msgpack::packer<Stream>& o, cv::Size const& v) const {
+	packer<Stream>& operator()(msgpack::packer<Stream>& o, cv::Size_<T> const& v) const {
 
 		o.pack_array(2);
 		o.pack(v.width);
@@ -28,22 +28,22 @@ struct pack<cv::Size> {
 	}
 };
 
-template<>
-struct convert<cv::Size> {
-	msgpack::object const& operator()(msgpack::object const& o, cv::Size& v) const {
+template<typename T>
+struct convert<cv::Size_<T>> {
+	msgpack::object const& operator()(msgpack::object const& o, cv::Size_<T>& v) const {
 		if (o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }
 		if (o.via.array.size != 2) { throw msgpack::type_error(); }
 		
-		int width = o.via.array.ptr[0].as<int>();
-		int height = o.via.array.ptr[1].as<int>();
-		v = cv::Size(width, height);
+		T width = o.via.array.ptr[0].as<T>();
+		T height = o.via.array.ptr[1].as<T>();
+		v = cv::Size_<T>(width, height);
 		return o;
 	}
 };
 
-template <>
-struct object_with_zone<cv::Size> {
-	void operator()(msgpack::object::with_zone& o, cv::Size const& v) const {
+template <typename T>
+struct object_with_zone<cv::Size_<T>> {
+	void operator()(msgpack::object::with_zone& o, cv::Size_<T> const& v) const {
 		o.type = type::ARRAY;
 		o.via.array.size = 2;
 		o.via.array.ptr = static_cast<msgpack::object*>(
@@ -55,6 +55,56 @@ struct object_with_zone<cv::Size> {
 	}
 };
 
+////////////////////////////////////////////////////////////////////////////////
+// cv::Rect_<T>
+
+template<typename T>
+struct pack<cv::Rect_<T>> {
+	template <typename Stream>
+	packer<Stream>& operator()(msgpack::packer<Stream>& o, cv::Rect_<T> const& v) const {
+
+		o.pack_array(4);
+		o.pack(v.height);
+		o.pack(v.width);
+		o.pack(v.x);
+		o.pack(v.y);
+
+		return o;
+	}
+};
+
+template<typename T>
+struct convert<cv::Rect_<T>> {
+	msgpack::object const& operator()(msgpack::object const& o, cv::Rect_<T> &v) const {
+		if (o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }
+		if (o.via.array.size != 4) { throw msgpack::type_error(); }
+		
+		T height = o.via.array.ptr[0].as<T>();
+		T width = o.via.array.ptr[1].as<T>();
+		T x = o.via.array.ptr[2].as<T>();
+		T y = o.via.array.ptr[3].as<T>();
+
+		v = cv::Rect_<T>(x, y, width, height);
+		return o;
+	}
+};
+
+template <typename T>
+struct object_with_zone<cv::Rect_<T>> {
+	void operator()(msgpack::object::with_zone& o, cv::Rect_<T> const& v) const {
+		o.type = type::ARRAY;
+		o.via.array.size = 4;
+		o.via.array.ptr = static_cast<msgpack::object*>(
+			o.zone.allocate_align(	sizeof(msgpack::object) * o.via.array.size,
+									MSGPACK_ZONE_ALIGNOF(msgpack::object)));
+
+		o.via.array.ptr[0] = msgpack::object(v.heigth, o.zone);
+		o.via.array.ptr[1] = msgpack::object(v.width, o.zone);
+		o.via.array.ptr[2] = msgpack::object(v.x, o.zone);
+		o.via.array.ptr[3] = msgpack::object(v.y, o.zone);
+	}
+};
+
 ////////////////////////////////////////////////////////////////////////////////
 // cv::Mat
 
@@ -118,7 +168,6 @@ struct object_with_zone<cv::Mat> {
 	}
 };
 
-
 }
 }
 }

components/common/cpp/test/msgpack_unit.cpp

@@ -19,17 +19,18 @@ std::string msgpack_pack(T v) {
 	return std::string(buffer.str());
 }
 
-Mat msgpack_unpack_mat(std::string str) {
+template<typename T>
+T msgpack_unpack(std::string str) {
 	msgpack::object_handle oh = msgpack::unpack(str.data(), str.size());
 	msgpack::object obj = oh.get();
-	Mat M;
-	return obj.convert<Mat>(M);
+	T res;
+	return obj.convert<T>(res);
 }
 
 TEST_CASE( "msgpack cv::Mat" ) {
 	SECTION( "Mat::ones(Size(5, 5), CV_64FC1)" ) {
 		Mat A = Mat::ones(Size(5, 5), CV_64FC1);
-		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		Mat B = msgpack_unpack<Mat>(msgpack_pack(A));
 
 		REQUIRE(A.size() == B.size());
 		REQUIRE(A.type() == B.type());
@@ -38,7 +39,7 @@ TEST_CASE( "msgpack cv::Mat" ) {
 
 	SECTION( "Mat::ones(Size(1, 5), CV_8UC3)" ) {
 		Mat A = Mat::ones(Size(1, 5), CV_8UC3);
-		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		Mat B = msgpack_unpack<Mat>(msgpack_pack(A));
 		
 		REQUIRE(A.size() == B.size());
 		REQUIRE(A.type() == B.type());
@@ -48,7 +49,7 @@ TEST_CASE( "msgpack cv::Mat" ) {
 	SECTION ( "Mat 10x10 CV_64FC1 with random values [-1000, 1000]" ) {
 		Mat A(Size(10, 10), CV_64FC1);
 		cv::randu(A, -1000, 1000);
-		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		Mat B = msgpack_unpack<Mat>(msgpack_pack(A));
 		
 		REQUIRE(A.size() == B.size());
 		REQUIRE(A.type() == B.type());
@@ -62,7 +63,7 @@ TEST_CASE( "msgpack cv::Mat" ) {
 		msgpack::zone z;
 		auto obj = msgpack::object(A, z);
 		
-		Mat B = msgpack_unpack_mat(msgpack_pack(obj));
+		Mat B = msgpack_unpack<Mat>(msgpack_pack(obj));
 		
 		REQUIRE(A.size() == B.size());
 		REQUIRE(A.type() == B.type());
@@ -75,7 +76,7 @@ TEST_CASE( "msgpack cv::Mat" ) {
 			A = A(Rect(2, 2, 3,3));
 			A.setTo(0);
 
-			Mat B = msgpack_unpack_mat(msgpack_pack(A));
+			Mat B = msgpack_unpack<Mat>(msgpack_pack(A));
 		
 			REQUIRE(A.size() == B.size());
 			REQUIRE(A.type() == B.type());
@@ -85,4 +86,9 @@ TEST_CASE( "msgpack cv::Mat" ) {
 			// if not supported, throws exception
 		}
 	}
+
+	SECTION( "Rect_<T>" ) {
+		auto res = msgpack_unpack<cv::Rect2d>(msgpack_pack(cv::Rect2d(1,2,3,4)));
+		REQUIRE(res == cv::Rect2d(1,2,3,4));
+	}
 }

components/operators/CMakeLists.txt

@@ -23,9 +23,9 @@ set(OPERSRC
 	src/correspondence.cu
 	src/clipping.cpp
 	src/depth.cpp
+	src/detectandtrack.cpp
 )
 
-
 if (LIBSGM_FOUND)
 	list(APPEND OPERSRC src/disparity/fixstars_sgm.cpp)
 endif (LIBSGM_FOUND)

components/operators/include/ftl/operators/detectandtrack.hpp

+#ifndef _FTL_OPERATORS_CASCADECLASSIFIER_HPP_
+#define _FTL_OPERATORS_CASCADECLASSIFIER_HPP_
+
+#include "ftl/operators/operator.hpp"
+#include <opencv2/objdetect.hpp>
+#include <opencv2/tracking.hpp>
+
+namespace ftl {
+namespace operators {
+
+/**
+ * Object detection and tracking. 
+ * 
+ * cv::CascadeClassifier used in detection
+ * https://docs.opencv.org/master/d1/de5/classcv_1_1CascadeClassifier.html
+ * 
+ * cv::TrackerKCF used in tracking
+ * https://docs.opencv.org/master/d2/dff/classcv_1_1TrackerKCF.html
+ * 
+ */
+class DetectAndTrack : public ftl::operators::Operator {
+	public:
+	explicit DetectAndTrack(ftl::Configurable*);
+	~DetectAndTrack() {};
+
+	inline Operator::Type type() const override { return Operator::Type::OneToOne; }
+
+	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) override;
+
+	protected:
+	bool init();
+	
+	bool detect(const cv::Mat &im);
+	bool track(const cv::Mat &im);
+
+	private:
+	ftl::codecs::Channel channel_in_;
+	ftl::codecs::Channel channel_out_;
+
+	struct Object {
+		cv::Ptr<cv::Tracker> tracker;
+		cv::Rect2d object;
+		int fail_count;
+	};
+	std::vector<Object> tracked_;
+
+	// detection: if detected object is farther than max_distance_, new tracked
+	// object is added
+	double max_distance_;
+	// maximum number of tracked objects (performance)
+	int max_tracked_;
+	// maximum number of successive failed trackings before object is removed
+	int	max_fail_;
+	// how often detection is performed
+	int detect_n_frames_;
+
+	// cascade classifier parameters, see OpenCV documentation
+	std::string fname_;
+	double scalef_;
+	int min_neighbors_;
+	// min_size_ and max_size_ relative
+	std::vector<double> min_size_;
+	std::vector<double> max_size_;
+
+	int n_frame_;
+	cv::Mat gray_;
+	cv::CascadeClassifier classifier_;
+};
+
+}
+}
+
+#endif // _FTL_OPERATORS_CASCADECLASSIFIER_HPP_

components/operators/src/detectandtrack.cpp

+#include <cmath>
+
+#include "loguru.hpp"
+#include "ftl/operators/detectandtrack.hpp"
+
+using std::string;
+using std::vector;
+using std::map;
+
+using cv::Mat;
+using cv::Size;
+using cv::Rect;
+using cv::Rect2d;
+using cv::Point2i;
+using cv::Point2d;
+
+using ftl::rgbd::Frame;
+using ftl::operators::DetectAndTrack;
+
+DetectAndTrack::DetectAndTrack(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+	init();
+}
+
+bool DetectAndTrack::init() {
+	fname_ = config()->value<string>("filename", "");
+	
+	detect_n_frames_ = config()->value<int>("n_frames", 10);
+	detect_n_frames_ = detect_n_frames_ < 0.0 ? 0.0 : detect_n_frames_;
+
+	max_distance_ = config()->value<double>("max_distance", 100.0);
+	max_distance_ = max_distance_ < 0.0 ? 0.0 : max_distance_;
+
+	max_fail_ = config()->value<int>("max_fail", 10);
+	max_fail_ = max_fail_ < 0 ? 10 : max_fail_;
+
+	max_tracked_ = config()->value<int>("max_tracked", 3);
+	max_tracked_ = max_tracked_ < 0 ? 10 : max_tracked_;
+
+	scalef_ = config()->value<double>("scalef", 1.1);
+	min_neighbors_ = config()->value<int>("min_neighbors", 3);
+
+	auto min_size = config()->get<vector<double>>("min_size");
+	auto max_size = config()->get<vector<double>>("max_size");
+
+	if (min_size && min_size->size() == 2) { min_size_ = *min_size; }
+	else { min_size_ = {0.0, 0.0}; }
+	if (max_size && max_size->size() == 2) { max_size_ = *max_size; }
+	else { max_size_ = {1.0, 1.0}; }
+	
+	min_size_[0] = max(min(1.0, min_size_[0]), 0.0);
+	min_size_[1] = max(min(1.0, min_size_[1]), 0.0);
+	min_size_[0] = max(min(1.0, max_size_[0]), 0.0);
+	min_size_[1] = max(min(1.0, max_size_[1]), 0.0);
+	if (min_size_[0] > max_size_[0]) { min_size_[0] = max_size_[0]; }
+	if (min_size_[1] > max_size_[1]) { min_size_[1] = max_size_[1]; }
+
+	channel_in_ = ftl::codecs::Channel::Colour;
+
+	bool retval = false;
+
+	try {
+		retval = classifier_.load(fname_);
+	}
+	catch (cv::Exception &ex)
+	{
+		retval = false;
+		LOG(ERROR) << ex.what();
+	}
+
+	if (!retval) {
+		LOG(ERROR) << "can't load: " << fname_;
+		return false;
+	}
+
+	return true;
+}
+
+static double distance(Point2i p, Point2i q) {
+	double a = (p.x-q.x);
+	double b = (p.y-q.y);
+	return sqrt(a*a+b*b);
+}
+
+static Point2d center(Rect2d obj) {
+	return Point2d(obj.x+obj.width/2.0, obj.y+obj.height/2.0);
+}
+
+bool DetectAndTrack::detect(const Mat &im) {
+	Size min_size(im.size().width*min_size_[0], im.size().height*min_size_[1]);
+	Size max_size(im.size().width*max_size_[0], im.size().height*max_size_[1]);
+
+	vector<Rect> objects;
+
+	classifier_.detectMultiScale(im, objects,
+								 scalef_, min_neighbors_, 0, min_size, max_size);
+	
+	LOG(INFO) << "Cascade classifier found " << objects.size() << " objects";
+
+	for (const Rect2d &obj : objects) {
+		Point2d c = center(obj);
+
+		bool found = false;
+		for (auto &tracker : tracked_) {
+			if (distance(center(tracker.object), c) < max_distance_) {
+				// update? (bounding box can be quite different)
+				// tracker.object = obj;
+				found = true;
+				break;
+			}
+		}
+
+		if (!found && (tracked_.size() < max_tracked_)) {
+			cv::Ptr<cv::Tracker> tracker = cv::TrackerKCF::create();
+			tracker->init(im, obj);
+			tracked_.push_back({ tracker, obj, 0 });
+		}
+	}
+
+	return true;
+}
+
+bool DetectAndTrack::track(const Mat &im) {
+	for (auto it = tracked_.begin(); it != tracked_.end();) {
+		if (!it->tracker->update(im, it->object)) {
+			it->fail_count++;
+		}
+		else {
+			it->fail_count = 0;
+		}
+
+		if (it->fail_count > max_fail_) {
+			tracked_.erase(it);
+		}
+		else { it++; }
+	}
+
+	return true;
+}
+
+bool DetectAndTrack::apply(Frame &in, Frame &out, cudaStream_t stream) {
+	if (classifier_.empty()) {
+		LOG(ERROR) << "classifier not loaded";
+		return false;
+	}
+
+	if (!in.hasChannel(channel_in_)) {
+		LOG(ERROR) << "input channel missing";
+		return false;
+	}
+
+	in.download(channel_in_);
+	Mat im = in.get<Mat>(channel_in_);
+
+	track(im);
+
+	if ((n_frame_++ % detect_n_frames_ == 0) && (tracked_.size() < max_tracked_)) {
+		if (im.channels() == 1) {
+			gray_ = im;
+		}
+		else if (im.channels() == 4) {
+			cv::cvtColor(im, gray_, cv::COLOR_BGRA2GRAY);
+		}
+		else if (im.channels() == 3) {
+			cv::cvtColor(im, gray_, cv::COLOR_BGR2GRAY);
+		}
+		else {
+			LOG(ERROR) << "unsupported number of channels in input image";
+			return false;
+		}
+
+		detect(gray_);
+	}
+
+	// TODO: save results somewhere
+	std::vector<Rect2d> result;
+	result.reserve(tracked_.size());
+	for (auto const &tracked : tracked_) { result.push_back(tracked.object); }
+	in.create(ftl::codecs::Channel::Data, result);
+
+	// TODO: should be uploaded by operator which requires data on GPU
+	in.upload(channel_in_);
+
+	return true;
+}

components/rgbd-sources/src/sources/stereovideo/calibrate.hpp

@@ -36,8 +36,6 @@ class Calibrate : public ftl::Configurable {
 
 	/**
 	 * @brief	Rectify and undistort stereo pair images (CPU)
-	 * @todo	Uses same rectification maps as GPU version, according to OpenCV
-	 * 			documentation for remap(), fixed point versions faster for CPU
 	 */
 	void rectifyStereo(cv::Mat &l, cv::Mat &r);
 

components/rgbd-sources/src/sources/stereovideo/local.cpp

@@ -56,21 +56,24 @@ LocalSource::LocalSource(nlohmann::json &config)
 		camera_b_ = nullptr;
 		stereo_ = false;
 		LOG(WARNING) << "Not able to find second camera for stereo";
-	} else {
-		camera_a_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 640));
-		camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 480));
+	}
+	else {
 		camera_b_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 640));
 		camera_b_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 480));
 
-		Mat frame;
-		camera_a_->grab();
-		camera_a_->retrieve(frame);
-		LOG(INFO) << "Video size : " << frame.cols << "x" << frame.rows;
-		width_ = frame.cols;
-		height_ = frame.rows;
 		stereo_ = true;
 	}
 
+	camera_a_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 640));
+	camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 480));
+	
+	Mat frame;
+	camera_a_->grab();
+	camera_a_->retrieve(frame);
+	LOG(INFO) << "Video size : " << frame.cols << "x" << frame.rows;
+	width_ = frame.cols;
+	height_ = frame.rows;
+
 	dwidth_ = value("depth_width", width_);
 	dheight_ = value("depth_height", height_);
 
@@ -248,66 +251,30 @@ bool LocalSource::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 
 	if (!camera_a_) return false;
 
-	if (camera_b_ || !stereo_) {
-		// TODO: Use threads here?
-		if (!camera_a_->retrieve(lfull)) {
-			LOG(ERROR) << "Unable to read frame from camera A";
-			return false;
-		}
-		if (camera_b_ && !camera_b_->retrieve(rfull)) {
-			LOG(ERROR) << "Unable to read frame from camera B";
-			return false;
-		}
-	} else {
-		LOG(FATAL) << "Stereo video no longer supported";
-		/*Mat frame;
-		if (!camera_a_->retrieve(frame)) {
-			LOG(ERROR) << "Unable to read frame from video";
-			return false;
-		}
-
-		int resx = frame.cols / 2;
-		//if (flip_) {
-		//	r = Mat(frame, Rect(0, 0, resx, frame.rows));
-		//	l = Mat(frame, Rect(resx, 0, frame.cols-resx, frame.rows));
-		//} else {
-			l = Mat(frame, Rect(0, 0, resx, frame.rows));
-			r = Mat(frame, Rect(resx, 0, frame.cols-resx, frame.rows));
-		//}*/
+	// TODO: Use threads here?
+	if (!camera_a_->retrieve(lfull)) {
+		LOG(ERROR) << "Unable to read frame from camera A";
+		return false;
 	}
 
-	/*if (downsize_ != 1.0f) {
-		// cv::cuda::resize()
-
-		cv::resize(left_, left_, cv::Size((int)(left_.cols * downsize_), (int)(left_.rows * downsize_)),
-				0, 0, cv::INTER_LINEAR);
-		cv::resize(r, r, cv::Size((int)(r.cols * downsize_), (int)(r.rows * downsize_)),
-				0, 0, cv::INTER_LINEAR);
-	}*/
-
-	// Note: this seems to be too slow on CPU...
-	/*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
-	wb = cv::xphoto::createSimpleWB();
-	wb->balanceWhite(l, l);
-	wb->balanceWhite(r, r);*/
-
-	/*if (flip_v_) {
-		Mat tl, tr;
-		cv::flip(left_, tl, 0);
-		cv::flip(r, tr, 0);
-		left_ = tl;
-		r = tr;
-	}*/
+	if (camera_b_ && !camera_b_->retrieve(rfull)) {
+		LOG(ERROR) << "Unable to read frame from camera B";
+		return false;
+	}
 
-	c->rectifyStereo(lfull, rfull);
+	if (stereo_) {
+		c->rectifyStereo(lfull, rfull);
+		
+		// Need to resize
+		if (hasHigherRes()) {
+			// TODO: Use threads?
+			cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
+		}
+	}
 
-	// Need to resize
 	if (hasHigherRes()) {
-		// TODO: Use threads?
 		cv::resize(lfull, l, l.size(), 0.0, 0.0, cv::INTER_CUBIC);
-		cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
 		hres_out.upload(hres, stream);
-		//LOG(INFO) << "Early Resize: " << l.size() << " from " << lfull.size();
 	} else {
 		hres_out = cv::cuda::GpuMat();
 	}

components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp

@@ -18,6 +18,7 @@
 #include "ftl/operators/segmentation.hpp"
 #include "ftl/operators/disparity.hpp"
 #include "ftl/operators/mask.hpp"
+#include "ftl/operators/detectandtrack.hpp"
 
 #include "ftl/threads.hpp"
 #include "calibrate.hpp"
@@ -46,25 +47,6 @@ StereoVideoSource::~StereoVideoSource() {
 	delete lsrc_;
 }
 
-template<typename T>
-static std::pair<std::vector<int>, std::vector<T>> MatToVec(cv::Mat M) {
-	std::pair<std::vector<int>, std::vector<T>> res;
-	res.first = std::vector<int>(3);
-	res.first[0] = M.type();
-	res.first[1] = M.size().width;
-	res.first[2] = M.size().height;
-	res.second = std::vector<T>(M.begin<T>(), M.end<T>());
-	return res;
-}
-
-template<typename T>
-static cv::Mat VecToMat(std::pair<std::vector<int>, std::vector<T>> data) {
-	return cv::Mat(	data.first[1],
-					data.first[2],
-					data.first[0],
-					data.second.data());
-}
-
 void StereoVideoSource::init(const string &file) {
 	capabilities_ = kCapVideo | kCapStereo;
 
@@ -97,6 +79,7 @@ void StereoVideoSource::init(const string &file) {
 	#ifdef HAVE_OPTFLOW
 	pipeline_input_->append<ftl::operators::NVOpticalFlow>("optflow", Channel::Colour, Channel::Flow);
 	#endif
+	pipeline_input_->append<ftl::operators::DetectAndTrack>("facedetection")->set("enabled", false);
 
 	calib_ = ftl::create<Calibrate>(host_, "calibration", cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()), stream_);
 

components/streams/src/netstream.cpp

@@ -196,7 +196,9 @@ bool Net::begin() {
 				for (size_t i=0; i<size(); ++i) {
 					select(i, selected(i) + spkt.channel);
 				}
-				reqtally_[static_cast<int>(spkt.channel)] = static_cast<int>(pkt.frame_count)*kTallyScale;
+				if (static_cast<int>(spkt.channel) < 32) {
+					reqtally_[static_cast<int>(spkt.channel)] = static_cast<int>(pkt.frame_count)*kTallyScale;
+				}
 			} else {
 				select(spkt.frameSetID(), selected(spkt.frameSetID()) + spkt.channel);
 			}