tracking: Build kalman things as a single TU

src/xrt/auxiliary/CMakeLists.txt

@@ -24,8 +24,8 @@ set(OS_SOURCE_FILES
 	)
 
 set(TRACKING_SOURCE_FILES
-	tracking/t_calibration.cpp
 	tracking/t_calibration_opencv.h
+	tracking/t_calibration.cpp
 	tracking/t_convert.cpp
 	tracking/t_debug_hsv_filter.cpp
 	tracking/t_debug_hsv_picker.cpp
@@ -34,8 +34,9 @@ set(TRACKING_SOURCE_FILES
 	tracking/t_fusion.h
 	tracking/t_hsv_filter.c
 	tracking/t_imu_fusion.h
-	tracking/t_imu.cpp
 	tracking/t_imu.h
+	tracking/t_kalman.cpp
+	tracking/t_tracker_psmv_fusion.h
 	tracking/t_tracker_psmv.cpp
 	tracking/t_tracker_psvr.cpp
 	tracking/t_tracking.h

src/xrt/auxiliary/meson.build

@@ -103,8 +103,8 @@ tracking_srcs = [
 	'tracking/t_fusion.h',
 	'tracking/t_hsv_filter.c',
 	'tracking/t_imu_fusion.h',
-	'tracking/t_imu.cpp',
 	'tracking/t_imu.h',
+	'tracking/t_kalman.cpp',
 	'tracking/t_tracker_psmv.cpp',
 	'tracking/t_tracker_psvr.cpp',
 	'tracking/t_tracking.h',

src/xrt/auxiliary/tracking/t_imu.cpp

@@ -2,7 +2,8 @@
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
- * @brief  IMU fusion.
+ * @brief  IMU fusion implementation - for inclusion into the single
+ * kalman-incuding translation unit.
  * @author Ryan Pavlik <ryan.pavlik@collabora.com>
  * @ingroup aux_tracking
  */

src/xrt/auxiliary/tracking/t_kalman.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Single compiled file for all kalman filter using source.
+ *
+ * Centralizes the build cost in one place.
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#include "t_imu.cpp"
+#include "t_tracker_psmv_fusion.cpp"

src/xrt/auxiliary/tracking/t_tracker_psmv.cpp

@@ -12,8 +12,7 @@
 
 #include "tracking/t_tracking.h"
 #include "tracking/t_calibration_opencv.h"
-#include "tracking/t_fusion.h"
-#include "tracking/t_imu_fusion.h"
+#include "tracking/t_tracker_psmv_fusion.h"
 
 #include "util/u_var.h"
 #include "util/u_misc.h"
@@ -22,25 +21,13 @@
 #include "util/u_format.h"
 
 #include "math/m_api.h"
-#include "math/m_eigen_interop.h"
 
 #include "os/os_threading.h"
 
-
-#include "flexkalman/AbsoluteOrientationMeasurement.h"
-#include "flexkalman/FlexibleKalmanFilter.h"
-#include "flexkalman/FlexibleUnscentedCorrect.h"
-#include "flexkalman/PoseSeparatelyDampedConstantVelocity.h"
-#include "flexkalman/PoseState.h"
-
 #include <stdio.h>
 #include <assert.h>
 #include <pthread.h>
 
-using State = flexkalman::pose_externalized_rotation::State;
-using ProcessModel =
-    flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel;
-
 /*!
  * Single camera.
  */
@@ -59,7 +46,6 @@ struct View
 
 struct TrackerPSMV
 {
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	struct xrt_tracked_psmv base = {};
 	struct xrt_frame_sink sink = {};
 	struct xrt_frame_node node = {};
@@ -97,10 +83,7 @@ struct TrackerPSMV
 
 	cv::Ptr<cv::SimpleBlobDetector> sbd;
 
-	State filter_state;
-	ProcessModel process_model;
-
-	xrt_fusion::SimpleIMUFusion imu;
+	std::unique_ptr<xrt_fusion::PSMVFusionInterface> filter;
 
 	xrt_vec3 tracked_object_position;
 };
@@ -252,20 +235,6 @@ world_point_from_blobs(cv::Point2f left,
 	return world_point;
 }
 
-static void
-reset_filter(TrackerPSMV &t)
-{
-
-	t.filter_state = State{};
-	t.tracked = false;
-}
-
-static void
-reset_filter_and_imu(TrackerPSMV &t)
-{
-	reset_filter(t);
-	t.imu = xrt_fusion::SimpleIMUFusion{};
-}
 /*!
  * @brief Perform tracking computations on a frame of video data.
  */
@@ -363,9 +332,10 @@ process(TrackerPSMV &t, struct xrt_frame *xf)
 	if (nearest_world.got_one) {
 		cv::Point3f world_point = nearest_world.best;
 		// update internal state
-
-		map_vec3(t.tracked_object_position) =
-		    Eigen::Map<Eigen::Vector3f>(&world_point.x);
+		memcpy(&t.tracked_object_position, &world_point.x,
+		       sizeof(t.tracked_object_position));
+	} else {
+		t.filter->clear_position_tracked_flag();
 	}
 
 	if (t.debug.frame != NULL) {
@@ -376,51 +346,27 @@ process(TrackerPSMV &t, struct xrt_frame *xf)
 
 	xrt_frame_reference(&xf, NULL);
 	xrt_frame_reference(&t.debug.frame, NULL);
-	bool corrected = false;
 	if (nearest_world.got_one) {
-		Eigen::Vector3f position = map_vec3(t.tracked_object_position);
-
-		auto measurement =
-		    xrt_fusion::AbsolutePositionLeverArmMeasurement{
-		        position.cast<double>(),
-		        //! @todo something less arbitrary for the lever arm?
-		        //! This puts the origin approximately under the PS
-		        //! button.
-		        Eigen::Vector3d(0., 0.09, 0.),
-		        //! @todo this should depend on distance
-		        // Weirdly, this is where *not* applying the
-		        // disparity-to-distance/rectification/etc would
-		        // simplify things, since the measurement variance is
-		        // related to the image sensor. 1.e-4 means 1cm std dev.
-		        // Not sure how to estimate the depth variance without
-		        // some research.
-		        Eigen::Vector3d(1.e-4, 1.e-4, 4.e-4)};
-		double resid = measurement.getResidual(t.filter_state).norm();
-
-		if (resid > 15) {
-			// Residual arbitrarily "too large"
-			fprintf(stderr,
-			        "Warning - measurement residual is %f\n",
-			        resid);
-		}
-		//! @todo predict and use actual dt - right now only IMU updates
-		//! this time.
-		// flexkalman::predict(t.filter_state, t.process_model, 1.0
-		// / 60.);
-		if (flexkalman::correctUnscented(t.filter_state, measurement)) {
-			corrected = true;
-			t.tracked = true;
-		} else {
-			fprintf(stderr,
-			        "Got non-finite something when filtering "
-			        "tracker - resetting filter!\n");
-			t.filter_state = State{};
-		}
-	}
-	if (!corrected) {
-		// We don't have anything to correct with, so we must manually
-		// run post-correct.
-		t.filter_state.postCorrect();
+#if 0
+		//! @todo something less arbitrary for the lever arm?
+		//! This puts the origin approximately under the PS
+		//! button.
+		xrt_vec3 lever_arm{0.f, 0.09f, 0.f};
+		//! @todo this should depend on distance
+		// Weirdly, this is where *not* applying the
+		// disparity-to-distance/rectification/etc would
+		// simplify things, since the measurement variance is
+		// related to the image sensor. 1.e-4 means 1cm std dev.
+		// Not sure how to estimate the depth variance without
+		// some research.
+		xrt_vec3 variance{1.e-4f, 1.e-4f, 4.e-4f};
+#endif
+		t.filter->process_3d_vision_data(
+		    0, &t.tracked_object_position, NULL, NULL,
+		    //! @todo tune cutoff for residual arbitrarily "too large"
+		    15);
+	} else {
+		t.filter->clear_position_tracked_flag();
 	}
 }
 
@@ -496,27 +442,7 @@ get_pose(TrackerPSMV &t,
 		return;
 	}
 
-	if (!t.tracked) {
-		out_relation->relation_flags = (enum xrt_space_relation_flags)0;
-		os_thread_helper_unlock(&t.oth);
-		return;
-	}
-
-	auto predicted_state =
-	    flexkalman::getPrediction(t.filter_state, t.process_model,
-	                              /*! @todo compute dt here */ 0.024);
-	// out_relation->pose.position = t.fusion.pos;
-	map_vec3(out_relation->pose.position) =
-	    predicted_state.position().cast<float>();
-	map_quat(out_relation->pose.orientation) =
-	    predicted_state.getQuaternion().cast<float>();
-
-	//! @todo assuming that orientation is actually currently tracked.
-	out_relation->relation_flags = (enum xrt_space_relation_flags)(
-	    XRT_SPACE_RELATION_POSITION_VALID_BIT |
-	    XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
-	    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
-	    XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	t.filter->get_prediction(when_ns, out_relation);
 
 	os_thread_helper_unlock(&t.oth);
 }
@@ -533,29 +459,7 @@ imu_data(TrackerPSMV &t,
 		os_thread_helper_unlock(&t.oth);
 		return;
 	}
-
-	float dt = time_ns_to_s(delta_ns);
-	t.imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(), dt);
-	t.imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(), dt);
-	t.imu.postCorrect();
-
-	//! @todo use better measurements instead of the above "simple fusion"
-	flexkalman::predict(t.filter_state, t.process_model, dt);
-	auto meas = flexkalman::AbsoluteOrientationMeasurement{
-	    Eigen::Quaterniond(
-	        Eigen::AngleAxisd(EIGEN_PI, Eigen::Vector3d::UnitY())) *
-	        t.imu.getQuat(),
-	    Eigen::Vector3d::Constant(0.01)};
-	if (!flexkalman::correctUnscented(t.filter_state, meas)) {
-		fprintf(stderr,
-		        "Got non-finite something when filtering IMU - "
-		        "resetting filter and IMU fusion!\n");
-		reset_filter_and_imu(t);
-
-	} else {
-		map_quat(t.fusion.rot) =
-		    t.filter_state.getQuaternion().cast<float>();
-	}
+	t.filter->process_imu_data(delta_ns, sample, NULL);
 
 	os_thread_helper_unlock(&t.oth);
 }
@@ -689,6 +593,7 @@ t_psmv_create(struct xrt_frame_context *xfctx,
 	t.fusion.rot.y = 0.0f;
 	t.fusion.rot.z = 0.0f;
 	t.fusion.rot.w = 1.0f;
+	t.filter = xrt_fusion::PSMVFusionInterface::create();
 
 	ret = os_thread_helper_init(&t.oth);
 	if (ret != 0) {

src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  PS Move tracker code that is expensive to compile.
+ *
+ * Typically built as a part of t_kalman.cpp to reduce incremental build times.
+ *
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ * @author Pete Black <pblack@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#include "t_tracker_psmv_fusion.h"
+
+#include "tracking/t_fusion.h"
+#include "tracking/t_imu_fusion.h"
+
+#include "math/m_api.h"
+#include "math/m_eigen_interop.h"
+
+#include "util/u_misc.h"
+
+#include "flexkalman/AbsoluteOrientationMeasurement.h"
+#include "flexkalman/FlexibleKalmanFilter.h"
+#include "flexkalman/FlexibleUnscentedCorrect.h"
+#include "flexkalman/PoseSeparatelyDampedConstantVelocity.h"
+#include "flexkalman/PoseState.h"
+
+
+using State = flexkalman::pose_externalized_rotation::State;
+using ProcessModel =
+    flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel;
+
+namespace xrt_fusion {
+
+struct TrackingInfo
+{
+	bool valid{false};
+	bool tracked{false};
+};
+namespace {
+	class PSMVFusion : public PSMVFusionInterface
+	{
+	public:
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+		void
+		clear_position_tracked_flag() override;
+
+		void
+		process_imu_data(time_duration_ns delta_ns,
+		                 const struct xrt_tracking_sample *sample,
+		                 const struct xrt_vec3
+		                     *orientation_variance_optional) override;
+		void
+		process_3d_vision_data(
+		    time_duration_ns delta_ns,
+		    const struct xrt_vec3 *position,
+		    const struct xrt_vec3 *variance_optional,
+		    const struct xrt_vec3 *lever_arm_optional,
+		    float residual_limit) override;
+
+		void
+		get_prediction(
+		    timepoint_ns when_ns,
+		    struct xrt_space_relation *out_relation) override;
+
+	private:
+		void
+		reset_filter();
+		void
+		reset_filter_and_imu();
+
+		State filter_state;
+		ProcessModel process_model;
+
+		xrt_fusion::SimpleIMUFusion imu;
+
+		bool tracked{false};
+		TrackingInfo orientation_state;
+		TrackingInfo position_state;
+	};
+
+
+
+	void
+	PSMVFusion::clear_position_tracked_flag()
+	{
+		position_state.tracked = false;
+	}
+
+	void
+	PSMVFusion::reset_filter()
+	{
+		filter_state = State{};
+		tracked = false;
+		position_state = TrackingInfo{};
+	}
+	void
+	PSMVFusion::reset_filter_and_imu()
+	{
+		reset_filter();
+		orientation_state = TrackingInfo{};
+		imu = SimpleIMUFusion{};
+	}
+
+	void
+	PSMVFusion::process_imu_data(
+	    time_duration_ns delta_ns,
+	    const struct xrt_tracking_sample *sample,
+	    const struct xrt_vec3 *orientation_variance_optional)
+	{
+
+		float dt = time_ns_to_s(delta_ns);
+		Eigen::Vector3d variance = Eigen::Vector3d::Constant(0.01);
+		if (orientation_variance_optional) {
+			variance = map_vec3(*orientation_variance_optional)
+			               .cast<double>();
+		}
+		imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(),
+		                dt);
+		imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(),
+		               dt);
+		imu.postCorrect();
+
+		//! @todo use better measurements instead of the above "simple
+		//! fusion"
+		flexkalman::predict(filter_state, process_model, dt);
+		auto meas = flexkalman::AbsoluteOrientationMeasurement{
+		    // Must rotate by 180 to align
+		    Eigen::Quaterniond(
+		        Eigen::AngleAxisd(EIGEN_PI, Eigen::Vector3d::UnitY())) *
+		        imu.getQuat(),
+		    variance};
+		if (flexkalman::correctUnscented(filter_state, meas)) {
+			orientation_state.tracked = true;
+			orientation_state.valid = true;
+		} else {
+			fprintf(stderr,
+			        "Got non-finite something when filtering IMU - "
+			        "resetting filter and IMU fusion!\n");
+			reset_filter_and_imu();
+		}
+	}
+
+	void
+	PSMVFusion::process_3d_vision_data(
+	    time_duration_ns delta_ns,
+	    const struct xrt_vec3 *position,
+	    const struct xrt_vec3 *variance_optional,
+	    const struct xrt_vec3 *lever_arm_optional,
+	    float residual_limit)
+	{
+		Eigen::Vector3f pos = map_vec3(*position);
+		Eigen::Vector3d variance{1.e-4, 1.e-4, 4.e-4};
+		if (variance_optional) {
+			variance = map_vec3(*variance_optional).cast<double>();
+		}
+		Eigen::Vector3d lever_arm{0, 0.09, 0};
+		if (lever_arm_optional) {
+			lever_arm =
+			    map_vec3(*lever_arm_optional).cast<double>();
+		}
+		auto measurement =
+		    xrt_fusion::AbsolutePositionLeverArmMeasurement{
+		        pos.cast<double>(), lever_arm, variance};
+		double resid = measurement.getResidual(filter_state).norm();
+
+		if (resid > residual_limit) {
+			// Residual arbitrarily "too large"
+			fprintf(
+			    stderr,
+			    "Warning - measurement residual is %f, resetting "
+			    "filter state\n",
+			    resid);
+			reset_filter();
+			return;
+		}
+		if (flexkalman::correctUnscented(filter_state, measurement)) {
+			tracked = true;
+			position_state.valid = true;
+			position_state.tracked = true;
+		} else {
+			fprintf(stderr,
+			        "Got non-finite something when filtering "
+			        "tracker - resetting filter!\n");
+			reset_filter();
+		}
+	}
+
+	void
+	PSMVFusion::get_prediction(timepoint_ns when_ns,
+	                           struct xrt_space_relation *out_relation)
+	{
+		if (out_relation == NULL) {
+			return;
+		}
+		// Clear to sane values
+		U_ZERO(out_relation);
+		out_relation->pose.orientation.w = 1;
+		if (!tracked) {
+			return;
+		}
+		auto predicted_state = flexkalman::getPrediction(
+		    filter_state, process_model,
+		    /*! @todo compute dt here */ 0.024);
+
+		map_vec3(out_relation->pose.position) =
+		    predicted_state.position().cast<float>();
+		map_quat(out_relation->pose.orientation) =
+		    predicted_state.getQuaternion().cast<float>();
+		map_vec3(out_relation->linear_velocity) =
+		    predicted_state.velocity().cast<float>();
+		map_vec3(out_relation->angular_velocity) =
+		    predicted_state.angularVelocity().cast<float>();
+
+		uint64_t flags = 0;
+		if (position_state.valid) {
+			flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT;
+			flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
+			if (position_state.tracked) {
+				flags |=
+				    XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
+			}
+		}
+		if (orientation_state.valid) {
+			flags |= XRT_SPACE_RELATION_ORIENTATION_VALID_BIT;
+			flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
+			if (orientation_state.tracked) {
+				flags |=
+				    XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
+			}
+		}
+		out_relation->relation_flags = (xrt_space_relation_flags)flags;
+	}
+} // namespace
+
+
+std::unique_ptr<PSMVFusionInterface>
+PSMVFusionInterface::create()
+{
+	auto ret = std::make_unique<PSMVFusion>();
+	return ret;
+}
+} // namespace xrt_fusion
\ No newline at end of file

src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  PS Move tracker code.
+ * @author Pete Black <pblack@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#include "xrt/xrt_defines.h"
+#include "xrt/xrt_tracking.h"
+
+#include "util/u_time.h"
+
+#include <memory>
+
+namespace xrt_fusion {
+class PSMVFusionInterface
+{
+public:
+	static std::unique_ptr<PSMVFusionInterface>
+	create();
+	virtual ~PSMVFusionInterface() = default;
+
+	/*!
+	 * @brief If you've lost sight of the position tracking and won't even
+	 * enter another function in this class.
+	 */
+	virtual void
+	clear_position_tracked_flag() = 0;
+
+	virtual void
+	process_imu_data(
+	    time_duration_ns delta_ns,
+	    const struct xrt_tracking_sample *sample,
+	    const struct xrt_vec3 *orientation_variance_optional) = 0;
+	virtual void
+	process_3d_vision_data(time_duration_ns delta_ns,
+	                       const struct xrt_vec3 *position,
+	                       const struct xrt_vec3 *variance_optional,
+	                       const struct xrt_vec3 *lever_arm_optional,
+	                       float residual_limit) = 0;
+
+	virtual void
+	get_prediction(timepoint_ns when_ns,
+	               struct xrt_space_relation *out_relation) = 0;
+};
+} // namespace xrt_fusion
\ No newline at end of file