From 7b51f53ce5b63091b4894f660b6b4695bed785c5 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Tue, 16 Jun 2020 17:25:31 +0300
Subject: [PATCH] Re add vision node stats and pipeline

---
 applications/vision/src/main.cpp              | 30 ++++++++++++++-----
 components/streams/src/receiver.cpp           |  2 +-
 .../structures/include/ftl/data/new_frame.hpp |  6 ++--
 components/structures/src/new_frame.cpp       |  3 +-
 4 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index a406e5af4..5cc212675 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -169,8 +169,17 @@ static void run(ftl::Configurable *root) {
 		return true;
 	});*/
 
+	int stats_count = 0;
+	int frames = 0;
+	float latency = 0.0f;
+
+	auto *pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
+	pipeline->append<ftl::operators::DetectAndTrack>("facedetection")->value("enabled", false);
+	pipeline->append<ftl::operators::ArUco>("aruco")->value("enabled", false);
+	pipeline->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel
+
 	// Callback for when a source has populated a frame with data
-	auto h = source->onFrame([sender](ftl::data::Frame &f) {
+	auto h = source->onFrame([sender,&stats_count,&latency,&frames,pipeline](ftl::data::Frame &f) {
 		// Make a frameset first
 		auto fs = ftl::data::FrameSet::fromFrame(f);
 
@@ -179,14 +188,25 @@ static void run(ftl::Configurable *root) {
 		sender->post(*fs, ftl::codecs::Channel::Colour);
 
 		// Do pipeline here...
+		pipeline->apply(*fs, *fs);
+		if (fs->firstFrame().has(ftl::codecs::Channel::Depth)) sender->post(*fs, ftl::codecs::Channel::Depth);
+
+		++frames;
+		latency += float(ftl::timer::get_time() - f.timestamp());
+
+		if (--stats_count <= 0) {
+			latency /= float(frames);
+			LOG(INFO) << "Frame rate: " << frames << ", Latency: " << latency;
+			stats_count = 20;
+			frames = 0;
+			latency = 0.0f;
+		}
 		return true;
 	});
 
 	// Start the timed generation of frames
 	creator.start();
 
-	int stats_count = 0;
-
 	/*grp->onFrameSet([sender,&stats_count](ftl::rgbd::FrameSet &fs) {
 		fs.id = 0;
 		sender->post(fs);
@@ -206,10 +226,6 @@ static void run(ftl::Configurable *root) {
 		return true;
 	});
 	
-	auto pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
-	pipeline->append<ftl::operators::DetectAndTrack>("facedetection")->value("enabled", false);
-	pipeline->append<ftl::operators::ArUco>("aruco")->value("enabled", false);
-	pipeline->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel
 	grp->addPipeline(pipeline);*/
 	
 	net->start();
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index 5ebab5c28..ad42fd54c 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -330,7 +330,7 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 
 		// Add channel to frame and allocate memory if required
 		const cv::Size size = cv::Size(width, height);
-		auto &buf = frame.createChange<ftl::rgbd::VideoFrame>(spkt.channel, ftl::data::ChangeType::FOREIGN).createGPU();
+		auto &buf = frame.createChange<ftl::rgbd::VideoFrame>(spkt.channel, ftl::data::ChangeType::FOREIGN, pkt).createGPU();
 		buf.create(size, ftl::codecs::type(spkt.channel)); //(isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4));
 
 		cv::Rect roi((i % tx)*width, (i / tx)*height, width, height);
diff --git a/components/structures/include/ftl/data/new_frame.hpp b/components/structures/include/ftl/data/new_frame.hpp
index 64543d8e3..4bc435277 100644
--- a/components/structures/include/ftl/data/new_frame.hpp
+++ b/components/structures/include/ftl/data/new_frame.hpp
@@ -186,7 +186,7 @@ class Frame {
 	ftl::data::Aggregator<T> createChange(ftl::codecs::Channel c, ftl::data::ChangeType t);
 
 	template <typename T>
-	T &createChange(ftl::codecs::Channel c, ftl::data::ChangeType t, ftl::codecs::Packet &data);
+	T &createChange(ftl::codecs::Channel c, ftl::data::ChangeType t, const ftl::codecs::Packet &data);
 
 	const std::list<ftl::codecs::Packet> &getEncoded(ftl::codecs::Channel c) const;
 
@@ -290,7 +290,7 @@ class Frame {
 	protected:
 	std::any &createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t);
 
-	std::any &createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t, ftl::codecs::Packet &data);
+	std::any &createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t, const ftl::codecs::Packet &data);
 
 	std::any &createAny(ftl::codecs::Channel c);
 
@@ -464,7 +464,7 @@ ftl::data::Aggregator<T> ftl::data::Frame::create(ftl::codecs::Channel c) {
 }
 
 template <typename T>
-T &ftl::data::Frame::createChange(ftl::codecs::Channel c, ftl::data::ChangeType type,ftl::codecs::Packet &data) {
+T &ftl::data::Frame::createChange(ftl::codecs::Channel c, ftl::data::ChangeType type, const ftl::codecs::Packet &data) {
 	if (!bool(is_list<T>{}) && isAggregate(c)) throw FTL_Error("Aggregate channels must be of list type");
 
 	ftl::data::verifyChannelType<T>(c);
diff --git a/components/structures/src/new_frame.cpp b/components/structures/src/new_frame.cpp
index 7c8f10814..6d0da508e 100644
--- a/components/structures/src/new_frame.cpp
+++ b/components/structures/src/new_frame.cpp
@@ -120,7 +120,7 @@ std::any &Frame::createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t
 	}
 }
 
-std::any &Frame::createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t, ftl::codecs::Packet &data) {
+std::any &Frame::createAnyChange(ftl::codecs::Channel c, ftl::data::ChangeType t, const ftl::codecs::Packet &data) {
 	if (status_ != FrameStatus::CREATED) throw FTL_Error("Cannot apply change after store " << static_cast<int>(status_));
 
 	ftl::data::Frame::ChannelData *d;
@@ -310,6 +310,7 @@ void Frame::swapChannels(ftl::codecs::Channel c1, ftl::codecs::Channel c2) {
 void Frame::reset() {
 	for (auto &d : data_) {
 		d.second.status = ChannelStatus::INVALID;
+		d.second.encoded.clear();
 	}
 	changed_.clear();
 	status_ = FrameStatus::CREATED;
-- 
GitLab