Fixing the room setup yaw is no longer necessary

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Justin Li
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

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;
@@ -103,6 +104,22 @@ struct DSample
 	Eigen::Vector3d ref, target;
 };
 
+bool StartsWith(const std::string &str, const std::string &prefix)
+{
+	if (str.length() < prefix.length())
+		return false;
+
+	return str.compare(0, prefix.length(), prefix) == 0;
+}
+
+bool EndsWith(const std::string &str, const std::string &suffix)
+{
+	if (str.length() < suffix.length())
+		return false;
+
+	return str.compare(str.length() - suffix.length(), suffix.length(), suffix) == 0;
+}
+
 Eigen::Vector3d AxisFromRotationMatrix3(Eigen::Matrix3d rot)
 {
 	return Eigen::Vector3d(rot(2,1) - rot(1,2), rot(0,2) - rot(2,0), rot(1,0) - rot(0,1));
@@ -135,41 +152,28 @@ DSample DeltaRotationSamples(Sample s1, Sample s2)
 	return ds;
 }
 
-bool StartsWith(const std::string &str, const std::string &prefix)
-{
-	if (str.length() < prefix.length())
-		return false;
-
-	return str.compare(0, prefix.length(), prefix) == 0;
-}
-
-bool EndsWith(const std::string &str, const std::string &suffix)
-{
-	if (str.length() < suffix.length())
-		return false;
-
-	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);
 		}
 	}
-	printf("\ngot %zd samples with %zd delta samples\n", samples.size(), deltas.size());
+	printf("got %zd samples with %zd delta samples\n", samples.size(), deltas.size());
 
 	// Kabsch algorithm
 
 	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,19 @@ 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;
+
+	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 +235,20 @@ 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;
 	auto transcm = trans * 100.0;
 
-	printf("\ntranslation x=%.2f y=%.2f z=%.2f\n", transcm[0], transcm[1], transcm[2]);
-	return trans;
+	printf("translation x=%.2f y=%.2f z=%.2f\n", transcm[0], transcm[1], transcm[2]);
+	return transcm;
 }
 
 Sample CollectSample(const CalibrationContext &ctx)
@@ -247,7 +257,7 @@ Sample CollectSample(const CalibrationContext &ctx)
 	reference.bPoseIsValid = false;
 	target.bPoseIsValid = false;
 
-	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0.0f, devicePoses, vr::k_unMaxTrackedDeviceCount);
+	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.0f, devicePoses, vr::k_unMaxTrackedDeviceCount);
 
 	reference = devicePoses[ctx.referenceID];
 	target = devicePoses[ctx.targetID];
@@ -269,7 +279,7 @@ bool PickDevices(CalibrationContext &ctx)
 	char buffer[vr::k_unMaxPropertyStringSize];
 
 	vr::TrackedDevicePose_t devicePoses[vr::k_unMaxTrackedDeviceCount];
-	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0.0f, devicePoses, vr::k_unMaxTrackedDeviceCount);
+	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.0f, devicePoses, vr::k_unMaxTrackedDeviceCount);
 
 	for (uint32_t id = 0; id < vr::k_unMaxTrackedDeviceCount; ++id)
 	{
@@ -334,13 +344,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,42 +361,92 @@ 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];
 
+	vr::TrackedDevicePose_t devicePoses[vr::k_unMaxTrackedDeviceCount];
+	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.0f, devicePoses, vr::k_unMaxTrackedDeviceCount);
+
 	for (uint32_t id = 0; id < vr::k_unMaxTrackedDeviceCount; ++id)
 	{
 		auto deviceClass = vr::VRSystem()->GetTrackedDeviceClass(id);
 		if (deviceClass == vr::TrackedDeviceClass_Invalid)
 			continue;
 
+		/*if (deviceClass == vr::TrackedDeviceClass_HMD) // for debugging unexpected universe switches
+		{
+			vr::ETrackedPropertyError err = vr::TrackedProp_Success;
+			auto universeId = vr::VRSystem()->GetUint64TrackedDeviceProperty(id, vr::Prop_CurrentUniverseId_Uint64, &err);
+			printf("uid %d err %d\n", universeId, err);
+			continue;
+		}*/
+
 		vr::ETrackedPropertyError err = vr::TrackedProp_Success;
 		vr::VRSystem()->GetStringTrackedDeviceProperty(id, vr::Prop_TrackingSystemName_String, buffer, vr::k_unMaxPropertyStringSize, &err);
 
-		if (err == vr::TrackedProp_Success)
-		{
-			std::string trackingSystem(buffer);
+		if (err != vr::TrackedProp_Success)
+			continue;
 
-			if (trackingSystem == ctx.calibratedTrackingSystem)
-			{
-				//printf("setting calibration for %d (%s)\n", id, buffer);
-				InputEmulator.setWorldFromDriverRotationOffset(id, ctx.calibratedRotation);
-				InputEmulator.setWorldFromDriverTranslationOffset(id, ctx.calibratedTranslation);
-				InputEmulator.enableDeviceOffsets(id, true);
-			}
+		std::string trackingSystem(buffer);
+
+		if (trackingSystem != ctx.calibratedTrackingSystem)
+			continue;
+
+		if (deviceClass == vr::TrackedDeviceClass_TrackingReference)
+		{
+			// TODO(pushrax): detect zero reference switches and adjust calibration automatically
+			//auto p = devicePoses[id].mDeviceToAbsoluteTracking.m;
+			//printf("%d: %f %f %f\n", id, p[0][3], p[1][3], p[2][3]);
+		}
+		else
+		{
+			//printf("setting calibration for %d (%s)\n", id, buffer);
+			auto vrRotQuat = VRRotationQuat(ctx.calibratedRotation);
+			InputEmulator.setWorldFromDriverRotationOffset(id, vrRotQuat);
+
+			auto vrTransVec = VRTranslationVec(ctx.calibratedTranslation);
+			InputEmulator.setWorldFromDriverTranslationOffset(id, vrTransVec);
+
+			InputEmulator.enableDeviceOffsets(id, true);
 		}
 	}
 }
@@ -457,39 +516,28 @@ void CalibrationTick()
 
 	if (samples.size() == totalSamples)
 	{
+		printf("\n");
 		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,52 @@
+# OpenVR-SpaceCalibrator
+
+Use VR devices from one company with any other.
+
+This is **beta software** and may not work for you. A quick video walkthrough of the calibration process is available at https://www.youtube.com/watch?v=W3TnQd9JMl4
+
+## Usage
+
+### Calibration
+
+If you don't already run a setup with multiple device types, see [Setting up SteamVR](#setting-up-steamvr).
+
+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.
+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.
+
+### Setting up SteamVR
+
+Every hardware designer with their own tracking system needs to make a "driver"
+so SteamVR can see their tracking data. SteamVR by default will load only one driver at a time,
+but we can tell it to load all of them by editing the vrsettings config file.
+
+Open `Steam\steamapps\common\SteamVR\resources\settings\default.vrsettings` in a text editor,
+look for the line that has `"activateMultipleDrivers": false` and change the `false` to `true`.
+In some guides online you may also see references to `requireHmd`, this does _not_ need to be changed
+unless you truly want to be able to run without any kind of HMD.
+
+Sometimes when SteamVR updates this file seems to get wiped, if your setup stops working you might
+have to edit the config again.
+
+### 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 3 numbers are the rotation (yaw, pitch, roll) in degrees, and the next 3 numbers are the translation (x, y, z) in centimeters.
+
+### 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.
+
+
+## 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!