Store calibration in euler angles, fix inverted pitch value

OpenVR-SpaceCalibrator/Calibration.cpp

@@ -25,8 +25,9 @@ struct CalibrationContext
 	CalibrationState state = None;
 	int referenceID, targetID;
 
-	vr::HmdQuaternion_t calibratedRotation;
-	vr::HmdVector3d_t calibratedTranslation;
+	Eigen::Vector3d calibratedRotation;
+	Eigen::Vector3d calibratedTranslation;
+
 	std::string calibratedTrackingSystem;
 
 	bool profileLoaded = false, validProfile = false;
@@ -151,12 +152,14 @@ bool EndsWith(const std::string &str, const std::string &suffix)
 	return str.compare(str.length() - suffix.length(), suffix.length(), suffix) == 0;
 }
 
-Eigen::Matrix3d CalibrateRotation(const std::vector<Sample> &samples)
+Eigen::Vector3d CalibrateRotation(const std::vector<Sample> &samples)
 {
 	std::vector<DSample> deltas;
 
-	for (size_t i = 0; i < samples.size(); i++) {
-		for (size_t j = 0; j < i; j++) {
+	for (size_t i = 0; i < samples.size(); i++)
+	{
+		for (size_t j = 0; j < i; j++)
+		{
 			auto delta = DeltaRotationSamples(samples[i], samples[j]);
 			if (delta.valid)
 				deltas.push_back(delta);
@@ -169,7 +172,8 @@ Eigen::Matrix3d CalibrateRotation(const std::vector<Sample> &samples)
 	Eigen::MatrixXd refPoints(deltas.size(), 3), targetPoints(deltas.size(), 3);
 	Eigen::Vector3d refCentroid(0,0,0), targetCentroid(0,0,0);
 
-	for (size_t i = 0; i < deltas.size(); i++) {
+	for (size_t i = 0; i < deltas.size(); i++)
+	{
 		refPoints.row(i) = deltas[i].ref;
 		refCentroid += deltas[i].ref;
 
@@ -180,7 +184,8 @@ Eigen::Matrix3d CalibrateRotation(const std::vector<Sample> &samples)
 	refCentroid /= (double) deltas.size();
 	targetCentroid /= (double) deltas.size();
 
-	for (size_t i = 0; i < deltas.size(); i++) {
+	for (size_t i = 0; i < deltas.size(); i++)
+	{
 		refPoints.row(i) -= refCentroid;
 		targetPoints.row(i) -= targetCentroid;
 	}
@@ -191,7 +196,8 @@ Eigen::Matrix3d CalibrateRotation(const std::vector<Sample> &samples)
 	auto svd = bdcsvd.compute(crossCV, Eigen::ComputeThinU | Eigen::ComputeThinV);
 
 	Eigen::Matrix3d i = Eigen::Matrix3d::Identity();
-	if ((svd.matrixU() * svd.matrixV().transpose()).determinant() < 0) {
+	if ((svd.matrixU() * svd.matrixV().transpose()).determinant() < 0)
+	{
 		i(2,2) = -1;
 	}
 
@@ -199,16 +205,20 @@ Eigen::Matrix3d CalibrateRotation(const std::vector<Sample> &samples)
 	rot.transposeInPlace();
 
 	Eigen::Vector3d euler = rot.eulerAngles(2, 1, 0) * 180.0 / EIGEN_PI;
-	printf("rotation yaw=%.2f pitch=%.2f roll=%.2f\n", euler[1], euler[0], euler[2]);
-	return rot;
+	euler[2] *= -1;
+
+	printf("rotation yaw=%.2f pitch=%.2f roll=%.2f\n", euler[1], euler[2], euler[0]);
+	return euler;
 }
 
 Eigen::Vector3d CalibrateTranslation(const std::vector<Sample> &samples)
 {
 	std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> deltas;
 
-	for (size_t i = 0; i < samples.size(); i++) {
-		for (size_t j = 0; j < i; j++) {
+	for (size_t i = 0; i < samples.size(); i++)
+	{
+		for (size_t j = 0; j < i; j++)
+		{
 			auto QAi = samples[i].ref.rot.transpose();
 			auto QAj = samples[j].ref.rot.transpose();
 			auto dQA = QAj - QAi;
@@ -226,19 +236,21 @@ Eigen::Vector3d CalibrateTranslation(const std::vector<Sample> &samples)
 	Eigen::VectorXd constants(deltas.size() * 3);
 	Eigen::MatrixXd coefficients(deltas.size() * 3, 3);
 
-	for (size_t i = 0; i < deltas.size(); i++) {
-		for (int axis = 0; axis < 3; axis++) {
+	for (size_t i = 0; i < deltas.size(); i++)
+	{
+		for (int axis = 0; axis < 3; axis++)
+		{
 			constants(i * 3 + axis) = deltas[i].first(axis);
 			coefficients.row(i * 3 + axis) = deltas[i].second.row(axis);
 		}
 	}
 
 	Eigen::Vector3d trans = coefficients.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(constants);
-	trans(0) *= -1;
+	trans(0) *= -1.0;
 	auto transcm = trans * 100.0;
 
 	printf("\ntranslation x=%.2f y=%.2f z=%.2f\n", transcm[0], transcm[1], transcm[2]);
-	return trans;
+	return transcm;
 }
 
 Sample CollectSample(const CalibrationContext &ctx)
@@ -334,13 +346,12 @@ void LoadProfile(CalibrationContext &ctx)
 
 	file
 		>> ctx.calibratedTrackingSystem
-		>> ctx.calibratedRotation.w
-		>> ctx.calibratedRotation.x
-		>> ctx.calibratedRotation.y
-		>> ctx.calibratedRotation.z
-		>> ctx.calibratedTranslation.v[0]
-		>> ctx.calibratedTranslation.v[1]
-		>> ctx.calibratedTranslation.v[2];
+		>> ctx.calibratedRotation(1) // yaw
+		>> ctx.calibratedRotation(2) // pitch
+		>> ctx.calibratedRotation(0) // roll
+		>> ctx.calibratedTranslation(0) // x
+		>> ctx.calibratedTranslation(1) // y
+		>> ctx.calibratedTranslation(2); // z
 
 	ctx.validProfile = true;
 }
@@ -352,18 +363,44 @@ void SaveProfile(CalibrationContext &ctx)
 	file
 		<< ctx.calibratedTrackingSystem << std::endl
 		<< std::setprecision(std::numeric_limits<double>::digits10 + 1)
-		<< ctx.calibratedRotation.w << " "
-		<< ctx.calibratedRotation.x << " "
-		<< ctx.calibratedRotation.y << " "
-		<< ctx.calibratedRotation.z << std::endl
-		<< ctx.calibratedTranslation.v[0] << " "
-		<< ctx.calibratedTranslation.v[1] << " "
-		<< ctx.calibratedTranslation.v[2] << std::endl;
+		<< ctx.calibratedRotation(1) << " " // yaw
+		<< ctx.calibratedRotation(2) << " " // pitch
+		<< ctx.calibratedRotation(0) << std::endl // roll
+		<< ctx.calibratedTranslation(0) << " " // x
+		<< ctx.calibratedTranslation(1) << " " // y
+		<< ctx.calibratedTranslation(2) << std::endl; //z
 
 	ctx.profileLoaded = true;
 	ctx.validProfile = true;
 }
 
+vr::HmdQuaternion_t VRRotationQuat(Eigen::Vector3d eulerdeg)
+{
+	auto euler = eulerdeg * EIGEN_PI / 180.0;
+
+	Eigen::Quaterniond rotQuat =
+		Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitZ()) *
+		Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) *
+		Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitX());
+
+	vr::HmdQuaternion_t vrRotQuat;
+	vrRotQuat.x = rotQuat.coeffs()[0];
+	vrRotQuat.y = rotQuat.coeffs()[1];
+	vrRotQuat.z = rotQuat.coeffs()[2];
+	vrRotQuat.w = rotQuat.coeffs()[3];
+	return vrRotQuat;
+}
+
+vr::HmdVector3d_t VRTranslationVec(Eigen::Vector3d transcm)
+{
+	auto trans = transcm * 0.01;
+	vr::HmdVector3d_t vrTrans;
+	vrTrans.v[0] = trans[0];
+	vrTrans.v[1] = trans[1];
+	vrTrans.v[2] = trans[2];
+	return vrTrans;
+}
+
 void ScanAndApplyProfile(const CalibrationContext &ctx)
 {
 	char buffer[vr::k_unMaxPropertyStringSize];
@@ -384,8 +421,12 @@ void ScanAndApplyProfile(const CalibrationContext &ctx)
 			if (trackingSystem == ctx.calibratedTrackingSystem)
 			{
 				//printf("setting calibration for %d (%s)\n", id, buffer);
-				InputEmulator.setWorldFromDriverRotationOffset(id, ctx.calibratedRotation);
-				InputEmulator.setWorldFromDriverTranslationOffset(id, ctx.calibratedTranslation);
+				auto vrRotQuat = VRRotationQuat(ctx.calibratedRotation);
+				InputEmulator.setWorldFromDriverRotationOffset(id, vrRotQuat);
+
+				auto vrTransVec = VRTranslationVec(ctx.calibratedTranslation);
+				InputEmulator.setWorldFromDriverTranslationOffset(id, vrTransVec);
+
 				InputEmulator.enableDeviceOffsets(id, true);
 			}
 		}
@@ -459,37 +500,25 @@ void CalibrationTick()
 	{
 		if (ctx.state == Rotation)
 		{
-			auto rot = CalibrateRotation(samples);
-			Eigen::Quaterniond rotQuat(rot);
-
-			vr::HmdQuaternion_t vrRotQuat;
-			vrRotQuat.x = rotQuat.coeffs()[0];
-			vrRotQuat.y = rotQuat.coeffs()[1];
-			vrRotQuat.z = rotQuat.coeffs()[2];
-			vrRotQuat.w = rotQuat.coeffs()[3];
+			ctx.calibratedRotation = CalibrateRotation(samples);
 
+			auto vrRotQuat = VRRotationQuat(ctx.calibratedRotation);
 			InputEmulator.setWorldFromDriverRotationOffset(ctx.targetID, vrRotQuat);
 			InputEmulator.enableDeviceOffsets(ctx.targetID, true);
 
-			ctx.calibratedRotation = vrRotQuat;
 			ctx.state = Translation;
 		}
 		else if (ctx.state == Translation)
 		{
-			auto trans = CalibrateTranslation(samples);
-
-			vr::HmdVector3d_t vrTrans;
-			vrTrans.v[0] = trans[0];
-			vrTrans.v[1] = trans[1];
-			vrTrans.v[2] = trans[2];
+			ctx.calibratedTranslation = CalibrateTranslation(samples);
 
+			auto vrTrans = VRTranslationVec(ctx.calibratedTranslation);
 			InputEmulator.setWorldFromDriverTranslationOffset(ctx.targetID, vrTrans);
 
-			ctx.calibratedTranslation = vrTrans;
-			ctx.state = None;
-
 			SaveProfile(ctx);
 			printf("finished calibration, profile saved\n");
+
+			ctx.state = None;
 		}
 
 		samples.clear();

README.md

@@ -0,0 +1,34 @@
+# OpenVR-SpaceCalibrator
+
+Use VR devices from one company with any other.
+
+This is **beta software** and may not work for you. A quick video demo is available at https://www.youtube.com/watch?v=Khzta0hgvrA
+
+## Usage
+
+0. Install [OpenVR-InputEmulator](https://github.com/matzman666/OpenVR-InputEmulator).
+1. Download and unzip the [latest release](https://github.com/pushrax/OpenVR-SpaceCalibrator/releases) of OpenVR-SpaceCalibrator.
+2. Run SteamVR, and turn on your Touch controllers and only one Vive device to use for calibration.
+    * Make sure your SteamVR and Oculus room setups are correct. If a lighthouse or sensor has moved since your last room setup, the calibration won't be calculated properly.
+3. Run OpenVR-SpaceCalibrator.
+4. Hold the left Touch controller and your Vive device in your left hand securely, like they're glued together.
+5. Click "Start Calibration"
+6. Wave your left hand around, like you're calibrating the compass on your phone. You want to get as many orientations as possible.
+7. Done! A profile will be saved automatically. You can turn on the rest of your Vive devices now.
+
+Next time you run SteamVR and OpenVR-InputEmulator it will load the calibration automatically and apply it to any Vive devices you turn on.
+
+## Manually editing the calibration
+
+If you'd like to make a manual change to the calibration, the values are in `openvr_space_calibration.txt` in the same folder as the exe.
+The first 4 numbers are the rotation (yaw, pitch, roll) in degrees, and the next 3 numbers are the translation (x, y, z) in centimeters.
+
+## The math
+
+See [math.pdf](https://github.com/pushrax/OpenVR-SpaceCalibrator/blob/master/math.pdf) for details.
+If you have some ideas for how to improve the calibration process, let me know!
+
+## Compiling your own build
+
+1. Install boost 1.63 to `lib/boost_1_63_0` (required for IPC to OpenVR-InputEmulator). https://sourceforge.net/projects/boost/files/boost-binaries/1.63.0/
+2. Open `OpenVR-SpaceCalibrator.sln` in Visual Studio 2015 and build.