From 432fb02871d7d4bf22c5dc78b611056e229e7f86 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 14 May 2020 18:37:10 +0300
Subject: [PATCH] Small fixes, add filestream reset

---
 components/operators/src/aruco.cpp                    |  2 +-
 components/operators/src/clipping.cpp                 |  5 ++++-
 components/operators/src/poser.cpp                    |  5 +++++
 components/streams/include/ftl/streams/filestream.hpp |  2 ++
 components/streams/src/filestream.cpp                 | 11 +++++++++++
 5 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/components/operators/src/aruco.cpp b/components/operators/src/aruco.cpp
index 2608ca1bb..34fcfca33 100644
--- a/components/operators/src/aruco.cpp
+++ b/components/operators/src/aruco.cpp
@@ -66,7 +66,7 @@ bool ArUco::apply(Frame &in, Frame &out, cudaStream_t stream) {
 	Frame *inptr = &in;
 	Frame *outptr = &out;
 
-	estimate_pose_ = config()->value("estimate_pose", false);
+	estimate_pose_ = config()->value("estimate_pose", true);
 	debug_ = config()->value("debug", false);
 	marker_size_ = config()->value("marker_size",0.1f);
 
diff --git a/components/operators/src/clipping.cpp b/components/operators/src/clipping.cpp
index 9d392b4f9..b3c5d9072 100644
--- a/components/operators/src/clipping.cpp
+++ b/components/operators/src/clipping.cpp
@@ -76,7 +76,10 @@ bool ClipScene::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStr
 
 			auto sclip = clip;
 			sclip.origin = sclip.origin.getInverse() * pose;
-			if (!no_clip) ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream);
+			if (!no_clip) {
+				f.create<cv::cuda::GpuMat>(Channel::Depth);  // Force reset.
+				ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream);
+			}
 		}
 	}
 
diff --git a/components/operators/src/poser.cpp b/components/operators/src/poser.cpp
index 0ed962f95..0c60db87b 100644
--- a/components/operators/src/poser.cpp
+++ b/components/operators/src/poser.cpp
@@ -24,6 +24,8 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
         std::vector<ftl::codecs::Shape3D> transforms;
         in.get(Channel::Shapes3D, transforms);
 
+		//LOG(INFO) << "Found shapes 3D global: " << (int)transforms.size();
+
         //for (auto &t : transforms) {
             //LOG(INFO) << "Have FS transform: " << t.id;
         //}
@@ -37,6 +39,8 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
                 std::vector<ftl::codecs::Shape3D> transforms;
                 f.get(Channel::Shapes3D, transforms);
 
+				//LOG(INFO) << "Found shapes 3D: " << (int)transforms.size();
+
                 for (auto &t : transforms) {
                     std::string idstr;
                     switch(t.type) {
@@ -58,6 +62,7 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
                     } else {
                         // TODO: Merge poses
                         if (!(*p).second.locked) (*p).second.pose = pose;
+						//LOG(INFO) << "POSE ID: " << idstr;
                     }
                 }
             }
diff --git a/components/streams/include/ftl/streams/filestream.hpp b/components/streams/include/ftl/streams/filestream.hpp
index bd6a580ea..92d0a8afe 100644
--- a/components/streams/include/ftl/streams/filestream.hpp
+++ b/components/streams/include/ftl/streams/filestream.hpp
@@ -29,6 +29,8 @@ class File : public Stream {
 	bool end() override;
 	bool active() override;
 
+	void reset() override;
+
 	/**
 	 * Automatically tick through the frames using a timer. Threads are used.
 	 */
diff --git a/components/streams/src/filestream.cpp b/components/streams/src/filestream.cpp
index 5650862dd..5ae915fdf 100644
--- a/components/streams/src/filestream.cpp
+++ b/components/streams/src/filestream.cpp
@@ -364,6 +364,17 @@ bool File::end() {
 	return true;
 }
 
+void File::reset() {
+	UNIQUE_LOCK(mutex_, lk);
+	data_.clear();
+	buffer_in_.reset();
+	buffer_in_.remove_nonparsed_buffer();
+	_open();
+
+	timestart_ = (ftl::timer::get_time() / ftl::timer::getInterval()) * ftl::timer::getInterval();
+	timestamp_ = timestart_;
+}
+
 bool File::active() {
 	return active_;
 }
-- 
GitLab