Use slow preset; save relative transform

1. Use SLOW preset for continuous calibration. This should
   increase its accuracy with expense of increased CPU usage
   before fully calibrated.

2. Save/load relative transform, so new calibration doesn't have
  to start from scratch.

OpenVR-SpaceCalibrator/Calibration.cpp

@@ -330,15 +330,17 @@ void StartCalibration()
 }
 
 void StartContinuousCalibration() {
-	CalCtx.calibrationSpeed = CalibrationContext::FAST;
+	CalCtx.calibrationSpeed = CalibrationContext::SLOW;
 	StartCalibration();
 	CalCtx.state = CalibrationState::Continuous;
+	calibration.setRelativeTransformation(CalCtx.refToTargetPose, CalCtx.relativePosCalibrated);
 	CalCtx.Log("Collecting initial samples...");
 	Metrics::WriteLogAnnotation("StartContinuousCalibration");
 }
 
 void EndContinuousCalibration() {
 	CalCtx.state = CalibrationState::None;
+	CalCtx.relativePosCalibrated = false;
 	SaveProfile(CalCtx);
 	Metrics::WriteLogAnnotation("EndContinuousCalibration");
 }
@@ -488,6 +490,8 @@ void CalibrationTick(double time)
 		if (calibration.isValid()) {
 			ctx.calibratedRotation = calibration.EulerRotation();
 			ctx.calibratedTranslation = calibration.Transformation().translation() * 100.0; // convert to cm units for profile storage
+			ctx.refToTargetPose = calibration.RelativeTransformation();
+			ctx.relativePosCalibrated = calibration.isRelativeTransformationCalibrated();
 
 			auto vrTrans = VRTranslationVec(ctx.calibratedTranslation);
 			auto vrRot = VRRotationQuat(Eigen::Quaterniond(calibration.Transformation().rotation()));
@@ -518,7 +522,7 @@ void CalibrationTick(double time)
 			calibration.Clear();
 		}
 		else {
-			for (int i = 0; i < 10; i++) calibration.ShiftSample();
+			for (int i = 0; i < 50; i++) calibration.ShiftSample();
 		}
 	}
 }

OpenVR-SpaceCalibrator/Calibration.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <Windows.h>
 #include <openvr.h>
 #include <vector>
@@ -50,6 +51,9 @@ struct CalibrationContext
 	protocol::AlignmentSpeedParams alignmentSpeedParams;
 	bool enableStaticRecalibration;
 
+	Eigen::AffineCompact3d refToTargetPose = Eigen::AffineCompact3d::Identity();
+	bool relativePosCalibrated = false;
+
 	enum Speed
 	{
 		FAST = 0,
@@ -111,6 +115,8 @@ struct CalibrationContext
 		targetTrackingSystem = "";
 		enabled = false;
 		validProfile = false;
+		refToTargetPose = Eigen::AffineCompact3d::Identity();
+		relativePosCalibrated = true;
 	}
 
 	size_t SampleCount()

OpenVR-SpaceCalibrator/CalibrationCalc.cpp

@@ -547,7 +547,7 @@ bool CalibrationCalc::ComputeIncremental(bool &lerp, double threshold) {
         Metrics::posOffset_byRelPose.Push(relPosOffset * 1000);
         Metrics::error_byRelPose.Push(relPoseError * 1000);
 
-		if (relPoseError < 0.010 || m_relativePosCalibrated && relPoseError < 0.025) {
+		if (relPoseError < 0.015 || m_relativePosCalibrated && relPoseError < 0.035) {
 			newCalibrationValid = true;
 			usingRelPose = true;
 			newError = relPoseError;

OpenVR-SpaceCalibrator/CalibrationCalc.h

@@ -73,6 +73,22 @@ class CalibrationCalc {
 	bool isValid() const {
 		return m_isValid;
 	}
+
+	const Eigen::AffineCompact3d RelativeTransformation() const 
+	{
+		return m_refToTargetPose;
+	}
+
+	bool isRelativeTransformationCalibrated() const
+	{
+		return m_relativePosCalibrated;
+	}
+
+	void setRelativeTransformation(const Eigen::AffineCompact3d transform, bool calibrated)
+	{
+		m_refToTargetPose = transform;
+		m_relativePosCalibrated = calibrated;
+	}
 
 	void PushSample(const Sample& sample);
 	void Clear();

OpenVR-SpaceCalibrator/Configuration.cpp

@@ -151,6 +151,31 @@ static void ParseProfile(CalibrationContext &ctx, std::istream &stream)
 			ctx.chaperone.valid = true;
 		}
 	}
+	if (obj["relative_pos_calibrated"].is<bool>()) {
+		ctx.relativePosCalibrated = obj["relative_pos_calibrated"].get<bool>();
+	}
+	if (obj["relative_transform"].is<picojson::object>()) {
+		auto relTransform = obj["relative_transform"].get<picojson::object>();
+		Eigen::Vector3d refToTragetRoation;
+		Eigen::Vector3d refToTargetTranslation;
+
+		refToTragetRoation(0) = relTransform["roll"].get<double>();
+		refToTragetRoation(1) = relTransform["yaw"].get<double>();
+		refToTragetRoation(2) = relTransform["pitch"].get<double>();
+		refToTargetTranslation(0) = relTransform["x"].get<double>();
+		refToTargetTranslation(1) = relTransform["y"].get<double>();
+		refToTargetTranslation(2) = relTransform["z"].get<double>();
+
+        Eigen::Matrix3d rotationMatrix;
+        rotationMatrix =
+            Eigen::AngleAxisd(refToTragetRoation[0], Eigen::Vector3d::UnitX()) *
+            Eigen::AngleAxisd(refToTragetRoation[1], Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(refToTragetRoation[2], Eigen::Vector3d::UnitZ());
+
+		ctx.refToTargetPose = Eigen::AffineCompact3d::Identity();
+        ctx.refToTargetPose.linear() = rotationMatrix;
+        ctx.refToTargetPose.translation() = refToTargetTranslation;
+	}
 
 	ctx.validProfile = true;
 }
@@ -212,6 +237,18 @@ static void WriteProfile(CalibrationContext &ctx, std::ostream &out)
 		profile["chaperone"].set<picojson::object>(chaperone);
 	}
 
+	Eigen::Vector3d refToTragetRoation = ctx.refToTargetPose.rotation().eulerAngles(0, 1, 2);
+	Eigen::Vector3d refToTargetTranslation = ctx.refToTargetPose.translation();
+	picojson::object refToTarget;
+	refToTarget["x"].set<double>(refToTargetTranslation(0));
+	refToTarget["y"].set<double>(refToTargetTranslation(1));
+	refToTarget["z"].set<double>(refToTargetTranslation(2));
+	refToTarget["roll"].set<double>(refToTragetRoation(0));
+	refToTarget["yaw"].set<double>(refToTragetRoation(1));
+	refToTarget["pitch"].set<double>(refToTragetRoation(2));
+	profile["relative_pos_calibrated"].set<bool>(ctx.relativePosCalibrated);
+	profile["relative_transform"].set<picojson::object>(refToTarget);
+
 	picojson::value profileV;
 	profileV.set<picojson::object>(profile);