aruco tags; headset position adjustment

applications/vision/src/main.cpp

@@ -188,11 +188,11 @@ static void run(ftl::Configurable *root) {
 	root->on("quiet", quiet, false);
 
 	auto *pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
+	pipeline->append<ftl::operators::ArUco>("aruco")->value("enabled", false);
 	pipeline->append<ftl::operators::DetectAndTrack>("facedetection")->value("enabled", false);
 	pipeline->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel
 	//pipeline->append<ftl::operators::ClipScene>("clipping")->value("enabled", false);
 	pipeline->restore("vision_pipeline", { "clipping" });
-	pipeline->append<ftl::operators::ArUco>("aruco")->value("enabled", false);
 
 	auto h = creator->onFrameSet([sender,outstream,&stats_count,&latency,&frames,&stats_time,pipeline,&encodable,&previous_encodable](const ftl::data::FrameSetPtr &fs) {
 

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

@@ -121,6 +121,7 @@ class ArUco : public ftl::operators::Operator {
 
 	inline Operator::Type type() const override { return Operator::Type::OneToOne; }
 	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) override;
+	virtual void wait(cudaStream_t) override;
 
 	ftl::codecs::Channel channel_in_;
 	ftl::codecs::Channel channel_out_;
@@ -129,6 +130,7 @@ class ArUco : public ftl::operators::Operator {
 	bool estimate_pose_;
 	float marker_size_;
 	cv::Mat tmp_;
+	std::future<void> job_;
 
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> params_;

components/operators/src/aruco.cpp

@@ -40,17 +40,24 @@ static Eigen::Matrix4d matrix(cv::Vec3d &rvec, cv::Vec3d &tvec) {
 }
 
 ArUco::ArUco(ftl::operators::Graph *g, ftl::Configurable *cfg) : ftl::operators::Operator(g, cfg) {
-	dictionary_ = cv::aruco::getPredefinedDictionary(cfg->value("dictionary", 0));
+	dictionary_ = cv::aruco::getPredefinedDictionary(
+		cfg->value("dictionary", int(cv::aruco::DICT_4X4_50)));
 	params_ = cv::aruco::DetectorParameters::create();
 	params_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;
-	params_->cornerRefinementMinAccuracy = 0.01;
-	params_->cornerRefinementMaxIterations = 20;
+	params_->cornerRefinementMinAccuracy = 0.05;
+	params_->cornerRefinementMaxIterations = 10;
+
+	// default values 13, 23, 10, for speed just one thresholding window size
+	params_->adaptiveThreshWinSizeMin = 13;
+	params_->adaptiveThreshWinSizeMax = 23;
+	params_->adaptiveThreshWinSizeStep = 10;
 
 	channel_in_ = Channel::Colour;
 	channel_out_ = Channel::Shapes3D;
 
 	cfg->on("dictionary", [this,cfg]() {
-		dictionary_ = cv::aruco::getPredefinedDictionary(cfg->value("dictionary", 0));
+		dictionary_ = cv::aruco::getPredefinedDictionary(
+			cfg->value("dictionary", 0));
 	});
 }
 
@@ -60,42 +67,50 @@ bool ArUco::apply(Frame &in, Frame &out, cudaStream_t) {
 	estimate_pose_ = config()->value("estimate_pose", true);
 	marker_size_ = config()->value("marker_size", 0.1f);
 
-	std::vector<Vec3d> rvecs;
-	std::vector<Vec3d> tvecs;
-	std::vector<std::vector<cv::Point2f>> corners;
-	std::vector<int> ids;
+	job_ = ftl::pool.push([this, &in, &out](int) {
+		std::vector<Vec3d> rvecs;
+		std::vector<Vec3d> tvecs;
+		std::vector<std::vector<cv::Point2f>> corners;
+		std::vector<int> ids;
 
-	{
-		FTL_Profile("ArUco", 0.02);
-		cv::cvtColor(in.get<cv::Mat>(channel_in_), tmp_, cv::COLOR_BGRA2GRAY);
+		{
+			FTL_Profile("ArUco", 0.02);
+			cv::cvtColor(in.get<cv::Mat>(channel_in_), tmp_, cv::COLOR_BGRA2GRAY);
 
-		const Mat K = in.getLeftCamera().getCameraMatrix();
-		const Mat dist;
+			const Mat K = in.getLeftCamera().getCameraMatrix(tmp_.size());
+			const Mat dist;
 
-		cv::aruco::detectMarkers(tmp_, dictionary_,
-								corners, ids, params_, cv::noArray(), K, dist);
+			cv::aruco::detectMarkers(tmp_, dictionary_,
+									corners, ids, params_, cv::noArray(), K, dist);
 
-		if (estimate_pose_) {
-			cv::aruco::estimatePoseSingleMarkers(corners, marker_size_, K, dist, rvecs, tvecs);
+			if (estimate_pose_) {
+				cv::aruco::estimatePoseSingleMarkers(corners, marker_size_, K, dist, rvecs, tvecs);
+			}
 		}
-	}
 
-	list<Shape3D> result;
-	if (out.hasChannel(channel_out_)) {
-		result = out.get<list<Shape3D>>(channel_out_);
-	}
+		list<Shape3D> result;
+		if (out.hasChannel(channel_out_)) {
+			result = out.get<list<Shape3D>>(channel_out_);
+		}
 
-	for (size_t i = 0; i < rvecs.size(); i++) {
-		if (estimate_pose_) {
-			auto &t = result.emplace_back();
-			t.id = ids[i];
-			t.type = ftl::codecs::Shape3DType::ARUCO;
-			t.pose = (in.getPose() * matrix(rvecs[i], tvecs[i])).cast<float>();
-			t.size = Eigen::Vector3f(1.0f, 1.0f, 0.0f)*marker_size_;
-			t.label = "Aruco-" + std::to_string(ids[i]);
+		for (size_t i = 0; i < rvecs.size(); i++) {
+			if (estimate_pose_) {
+				auto &t = result.emplace_back();
+				t.id = ids[i];
+				t.type = ftl::codecs::Shape3DType::ARUCO;
+				t.pose = (in.getPose() * matrix(rvecs[i], tvecs[i])).cast<float>();
+				t.size = Eigen::Vector3f(1.0f, 1.0f, 0.0f)*marker_size_;
+				t.label = "Aruco-" + std::to_string(ids[i]);
+			}
 		}
-	}
 
-	out.create<list<Shape3D>>(channel_out_).list = result;
+		out.create<list<Shape3D>>(channel_out_).list = result;
+	});
 	return true;
 }
+
+void ArUco::wait(cudaStream_t) {
+	if (job_.valid()) {
+		job_.wait();
+	}
+}
\ No newline at end of file

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

@@ -64,14 +64,14 @@ struct __align__(16) Camera {
 	/**
 	 * Convert screen plus depth into camera coordinates.
 	 */
-	__host__ __device__ float3 screenToCam(int ux, int uy, float depth) const; 
+	__host__ __device__ float3 screenToCam(int ux, int uy, float depth) const;
 
 	//Eigen::Vector4f eigenScreenToCam(int ux, int uy, float depth) const;
 
 	/**
 	 * Convert screen plus depth into camera coordinates.
 	 */
-	__host__ __device__ float3 screenToCam(uint ux, uint uy, float depth) const; 
+	__host__ __device__ float3 screenToCam(uint ux, uint uy, float depth) const;
 
 	/**
 	 * Convert screen plus depth into camera coordinates.
@@ -86,8 +86,8 @@ struct __align__(16) Camera {
 	 * Make a camera struct from a configurable.
 	 */
 	static Camera from(ftl::Configurable*);
-	
-	cv::Mat getCameraMatrix() const;
+
+	cv::Mat getCameraMatrix(const cv::Size& sz={0, 0}) const;
 	#endif
 };
 
@@ -104,18 +104,18 @@ inline float3 ftl::rgbd::Camera::project<ftl::rgbd::Projection::EQUIRECTANGULAR>
     //inverse formula for spherical projection, reference Szeliski book "Computer Vision: Algorithms and Applications" p439.
     const float theta = atan2(ray3d.y,sqrt(ray3d.x*ray3d.x+ray3d.z*ray3d.z));
 	const float phi = atan2(ray3d.x, ray3d.z);
-	
+
 	const float pi = 3.14159265f;
 
     //get 2D point on equirectangular map
-    float x_sphere = (((phi*width)/pi+width)/2.0f); 
+    float x_sphere = (((phi*width)/pi+width)/2.0f);
     float y_sphere = (theta+ pi/2.0f)*height/pi;
 
     return make_float3(x_sphere,y_sphere, l);
 };
 
 template <> __device__ __host__
-inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::EQUIRECTANGULAR>(const float3 &equi) const {	
+inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::EQUIRECTANGULAR>(const float3 &equi) const {
 	const float pi = 3.14159265f;
 
 	float phi = (equi.x * 2.0f - float(width)) * pi / float(width);
@@ -131,7 +131,7 @@ inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::EQUIRECTANGULA
 template <> __device__ __host__
 inline float3 ftl::rgbd::Camera::project<ftl::rgbd::Projection::PERSPECTIVE>(const float3 &pos) const {
 	return make_float3(
-		static_cast<float>(pos.x*fx/pos.z - cx),			
+		static_cast<float>(pos.x*fx/pos.z - cx),
 		static_cast<float>(pos.y*fy/pos.z - cy),
 		pos.z
 	);
@@ -147,7 +147,7 @@ inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::PERSPECTIVE>(c
 template <> __device__ __host__
 inline float3 ftl::rgbd::Camera::project<ftl::rgbd::Projection::ORTHOGRAPHIC>(const float3 &pos) const {
 	return make_float3(
-		static_cast<float>(pos.x*fx - cx),			
+		static_cast<float>(pos.x*fx - cx),
 		static_cast<float>(pos.y*fy - cy),
 		pos.z
 	);
@@ -163,7 +163,7 @@ inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::ORTHOGRAPHIC>(
 template <> __device__ __host__
 inline float2 ftl::rgbd::Camera::camToScreen<float2>(const float3 &pos) const {
 	return make_float2(
-		static_cast<float>(pos.x*fx/pos.z - cx),			
+		static_cast<float>(pos.x*fx/pos.z - cx),
 		static_cast<float>(pos.y*fy/pos.z - cy)
 	);
 }

components/rgbd-sources/src/camera.cpp

@@ -35,13 +35,18 @@ Camera Camera::from(ftl::Configurable *cfg) {
 	return r;
 }
 
-cv::Mat Camera::getCameraMatrix() const {
-	cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
-	K.at<double>(0,0) = fx;
-	K.at<double>(0,2) = -cx;
-	K.at<double>(1,1) = fy;
-	K.at<double>(1,2) = -cy;
-	return K;
+cv::Mat Camera::getCameraMatrix(const cv::Size& sz) const {
+	if (sz == cv::Size{0, 0}) {
+		cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
+		K.at<double>(0,0) = fx;
+		K.at<double>(0,2) = -cx;
+		K.at<double>(1,1) = fy;
+		K.at<double>(1,2) = -cy;
+		return K;
+	}
+	else {
+		return scaled(sz.width, sz.height).getCameraMatrix();
+	}
 }
 
 /*

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

@@ -196,10 +196,10 @@ cv::Mat StereoRectification::cameraMatrix(Channel c) {
 	}
 	else {
 		if (c == Channel::Left) {
-			return calib_left_.intrinsic.matrix();
+			return CalibrationData::Intrinsic(calib_left_.intrinsic, image_resolution_).matrix();
 		}
 		else if (c == Channel::Right) {
-			return calib_right_.intrinsic.matrix();
+			return CalibrationData::Intrinsic(calib_right_.intrinsic, image_resolution_).matrix();
 		}
 	}
 	throw ftl::exception("Invalid channel, expected Left or Right");

components/streams/src/renderers/openvr_render.cpp

@@ -329,6 +329,11 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
 
 				if (headset_origin.size() > 0) {
 					ftl::operators::Poser::get(headset_origin, horigin);
+					double headset_offset = host_->value("headset_offset_z", 0.0);
+					// move z-axis by offset
+					Eigen::Vector3d offset =
+						horigin.block<3, 3>(0, 0)*Eigen::Vector3d(0.0, 0.0, headset_offset);
+					horigin.block<3, 1>(0, 3) -= offset;
 				}
 				Eigen::Matrix4d new_pose = horigin*viewPose.inverse();