mirror of
https://github.com/hyblocker/OpenVR-SpaceCalibrator.git
synced 2026-10-14 10:00:51 +02:00
552 lines
15 KiB
C++
552 lines
15 KiB
C++
#include "stdafx.h"
|
|
#include "Calibration.h"
|
|
#include "CalibrationMetrics.h"
|
|
#include "Configuration.h"
|
|
#include "IPCClient.h"
|
|
#include "CalibrationCalc.h"
|
|
#include "VRState.h"
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
#include <iostream>
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
inline vr::HmdQuaternion_t operator*(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) {
|
|
return {
|
|
(lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z),
|
|
(lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y),
|
|
(lhs.w * rhs.y) + (lhs.y * rhs.w) + (lhs.z * rhs.x) - (lhs.x * rhs.z),
|
|
(lhs.w * rhs.z) + (lhs.z * rhs.w) + (lhs.x * rhs.y) - (lhs.y * rhs.x)
|
|
};
|
|
}
|
|
|
|
CalibrationContext CalCtx;
|
|
IPCClient Driver;
|
|
static protocol::DriverPoseShmem shmem;
|
|
|
|
namespace {
|
|
CalibrationCalc calibration;
|
|
|
|
inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const double(&vector)[3]) {
|
|
vr::HmdQuaternion_t vectorQuat = { 0.0, vector[0], vector[1] , vector[2] };
|
|
vr::HmdQuaternion_t conjugate = { quat.w, -quat.x, -quat.y, -quat.z };
|
|
auto rotatedVectorQuat = quat * vectorQuat * conjugate;
|
|
return { rotatedVectorQuat.x, rotatedVectorQuat.y, rotatedVectorQuat.z };
|
|
}
|
|
|
|
inline Eigen::Matrix3d quaternionRotateMatrix(const vr::HmdQuaternion_t& quat) {
|
|
return Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z).toRotationMatrix();
|
|
}
|
|
|
|
struct DSample
|
|
{
|
|
bool valid;
|
|
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));
|
|
}
|
|
|
|
double AngleFromRotationMatrix3(Eigen::Matrix3d rot)
|
|
{
|
|
return acos((rot(0, 0) + rot(1, 1) + rot(2, 2) - 1.0) / 2.0);
|
|
}
|
|
|
|
vr::HmdQuaternion_t VRRotationQuat(const Eigen::Quaterniond& rotQuat)
|
|
{
|
|
|
|
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::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());
|
|
|
|
return VRRotationQuat(rotQuat);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
DSample DeltaRotationSamples(Sample s1, Sample s2)
|
|
{
|
|
// Difference in rotation between samples.
|
|
auto dref = s1.ref.rot * s2.ref.rot.transpose();
|
|
auto dtarget = s1.target.rot * s2.target.rot.transpose();
|
|
|
|
// When stuck together, the two tracked objects rotate as a pair,
|
|
// therefore their axes of rotation must be equal between any given pair of samples.
|
|
DSample ds;
|
|
ds.ref = AxisFromRotationMatrix3(dref);
|
|
ds.target = AxisFromRotationMatrix3(dtarget);
|
|
|
|
// Reject samples that were too close to each other.
|
|
auto refA = AngleFromRotationMatrix3(dref);
|
|
auto targetA = AngleFromRotationMatrix3(dtarget);
|
|
ds.valid = refA > 0.4 && targetA > 0.4 && ds.ref.norm() > 0.01 && ds.target.norm() > 0.01;
|
|
|
|
ds.ref.normalize();
|
|
ds.target.normalize();
|
|
return ds;
|
|
}
|
|
|
|
Pose ConvertPose(const vr::DriverPose_t &driverPose) {
|
|
Eigen::Quaterniond driverToWorldQ(
|
|
driverPose.qWorldFromDriverRotation.w,
|
|
driverPose.qWorldFromDriverRotation.x,
|
|
driverPose.qWorldFromDriverRotation.y,
|
|
driverPose.qWorldFromDriverRotation.z
|
|
);
|
|
Eigen::Vector3d driverToWorldV(
|
|
driverPose.vecWorldFromDriverTranslation[0],
|
|
driverPose.vecWorldFromDriverTranslation[1],
|
|
driverPose.vecWorldFromDriverTranslation[2]
|
|
);
|
|
|
|
Eigen::Quaterniond driverRot = driverToWorldQ * Eigen::Quaterniond(
|
|
driverPose.qRotation.w,
|
|
driverPose.qRotation.x,
|
|
driverPose.qRotation.y,
|
|
driverPose.qRotation.z
|
|
);
|
|
|
|
Eigen::Vector3d driverPos = driverToWorldV + driverToWorldQ * Eigen::Vector3d(
|
|
driverPose.vecPosition[0],
|
|
driverPose.vecPosition[1],
|
|
driverPose.vecPosition[2]
|
|
);
|
|
|
|
Eigen::AffineCompact3d xform = Eigen::Translation3d(driverPos) * driverRot;
|
|
|
|
return Pose(xform);
|
|
}
|
|
|
|
bool CollectSample(const CalibrationContext& ctx)
|
|
{
|
|
vr::DriverPose_t reference, target;
|
|
reference.poseIsValid = false;
|
|
target.poseIsValid = false;
|
|
|
|
reference = ctx.devicePoses[ctx.referenceID];
|
|
target = ctx.devicePoses[ctx.targetID];
|
|
|
|
bool ok = true;
|
|
if (!reference.poseIsValid)
|
|
{
|
|
CalCtx.Log("Reference device is not tracking\n"); ok = false;
|
|
}
|
|
if (!target.poseIsValid)
|
|
{
|
|
CalCtx.Log("Target device is not tracking\n"); ok = false;
|
|
}
|
|
if (!ok)
|
|
{
|
|
if (CalCtx.state != CalibrationState::Continuous) {
|
|
CalCtx.Log("Aborting calibration!\n");
|
|
CalCtx.state = CalibrationState::None;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
calibration.PushSample(Sample(
|
|
ConvertPose(reference),
|
|
ConvertPose(target)
|
|
));
|
|
|
|
return true;
|
|
}
|
|
|
|
bool AssignTargets() {
|
|
auto state = VRState::Load();
|
|
|
|
if (CalCtx.referenceID < 0) {
|
|
CalCtx.referenceID = state.FindDevice(CalCtx.referenceStandby.trackingSystem, CalCtx.referenceStandby.model, CalCtx.referenceStandby.serial);
|
|
}
|
|
|
|
if (CalCtx.targetID < 0) {
|
|
CalCtx.targetID = state.FindDevice(CalCtx.targetStandby.trackingSystem, CalCtx.targetStandby.model, CalCtx.targetStandby.serial);
|
|
}
|
|
|
|
return CalCtx.referenceID >= 0 && CalCtx.targetID >= 0;
|
|
}
|
|
}
|
|
|
|
void InitCalibrator()
|
|
{
|
|
Driver.Connect();
|
|
shmem.Open(OPENVR_SPACECALIBRATOR_SHMEM_NAME);
|
|
}
|
|
|
|
void ResetAndDisableOffsets(uint32_t id)
|
|
{
|
|
vr::HmdVector3d_t zeroV;
|
|
zeroV.v[0] = zeroV.v[1] = zeroV.v[2] = 0;
|
|
|
|
vr::HmdQuaternion_t zeroQ;
|
|
zeroQ.x = 0; zeroQ.y = 0; zeroQ.z = 0; zeroQ.w = 1;
|
|
|
|
protocol::Request req(protocol::RequestSetDeviceTransform);
|
|
req.setDeviceTransform = { id, false, zeroV, zeroQ, 1.0 };
|
|
Driver.SendBlocking(req);
|
|
}
|
|
|
|
static_assert(vr::k_unTrackedDeviceIndex_Hmd == 0, "HMD index expected to be 0");
|
|
|
|
void ScanAndApplyProfile(CalibrationContext &ctx)
|
|
{
|
|
std::unique_ptr<char[]> buffer_array(new char
|
|
[vr::k_unMaxPropertyStringSize]);
|
|
char* buffer = buffer_array.get();
|
|
ctx.enabled = ctx.validProfile;
|
|
|
|
protocol::Request setParamsReq(protocol::RequestSetAlignmentSpeedParams);
|
|
setParamsReq.setAlignmentSpeedParams = ctx.alignmentSpeedParams;
|
|
Driver.SendBlocking(setParamsReq);
|
|
|
|
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);
|
|
ResetAndDisableOffsets(id);
|
|
continue;
|
|
}*/
|
|
|
|
if (!ctx.enabled)
|
|
{
|
|
ResetAndDisableOffsets(id);
|
|
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)
|
|
{
|
|
ResetAndDisableOffsets(id);
|
|
continue;
|
|
}
|
|
|
|
std::string trackingSystem(buffer);
|
|
|
|
if (id == vr::k_unTrackedDeviceIndex_Hmd)
|
|
{
|
|
//auto p = ctx.devicePoses[id].mDeviceToAbsoluteTracking.m;
|
|
//printf("HMD %d: %f %f %f\n", id, p[0][3], p[1][3], p[2][3]);
|
|
|
|
if (trackingSystem != ctx.referenceTrackingSystem)
|
|
{
|
|
// Currently using an HMD with a different tracking system than the calibration.
|
|
ctx.enabled = false;
|
|
}
|
|
|
|
ResetAndDisableOffsets(id);
|
|
continue;
|
|
}
|
|
|
|
if (trackingSystem != ctx.targetTrackingSystem)
|
|
{
|
|
ResetAndDisableOffsets(id);
|
|
continue;
|
|
}
|
|
|
|
protocol::Request req(protocol::RequestSetDeviceTransform);
|
|
req.setDeviceTransform = {
|
|
id,
|
|
true,
|
|
VRTranslationVec(ctx.calibratedTranslation),
|
|
VRRotationQuat(ctx.calibratedRotation),
|
|
ctx.calibratedScale
|
|
};
|
|
req.setDeviceTransform.lerp = CalCtx.state == CalibrationState::Continuous;
|
|
req.setDeviceTransform.quash = CalCtx.state == CalibrationState::Continuous && id == CalCtx.targetID && CalCtx.quashTargetInContinuous;
|
|
|
|
Driver.SendBlocking(req);
|
|
}
|
|
|
|
if (ctx.enabled && ctx.chaperone.valid && ctx.chaperone.autoApply)
|
|
{
|
|
uint32_t quadCount = 0;
|
|
vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);
|
|
|
|
// Heuristic: when SteamVR resets to a blank-ish chaperone, it uses empty geometry,
|
|
// but manual adjustments (e.g. via a play space mover) will not touch geometry.
|
|
if (quadCount != ctx.chaperone.geometry.size())
|
|
{
|
|
ApplyChaperoneBounds();
|
|
}
|
|
}
|
|
}
|
|
|
|
void StartCalibration()
|
|
{
|
|
CalCtx.state = CalibrationState::Begin;
|
|
CalCtx.wantedUpdateInterval = 0.0;
|
|
CalCtx.messages.clear();
|
|
calibration.Clear();
|
|
Metrics::WriteLogAnnotation("StartCalibration");
|
|
}
|
|
|
|
void StartContinuousCalibration() {
|
|
CalCtx.calibrationSpeed = CalibrationContext::FAST;
|
|
StartCalibration();
|
|
CalCtx.state = CalibrationState::Continuous;
|
|
CalCtx.Log("Collecting initial samples...");
|
|
Metrics::WriteLogAnnotation("StartContinuousCalibration");
|
|
}
|
|
|
|
void EndContinuousCalibration() {
|
|
CalCtx.state = CalibrationState::None;
|
|
SaveProfile(CalCtx);
|
|
Metrics::WriteLogAnnotation("EndContinuousCalibration");
|
|
}
|
|
|
|
void CalibrationTick(double time)
|
|
{
|
|
if (!vr::VRSystem())
|
|
return;
|
|
|
|
auto &ctx = CalCtx;
|
|
if ((time - ctx.timeLastTick) < 0.05)
|
|
return;
|
|
|
|
if (ctx.state == CalibrationState::Continuous || ctx.state == CalibrationState::ContinuousStandby) {
|
|
ctx.ClearLogOnMessage();
|
|
}
|
|
|
|
ctx.timeLastTick = time;
|
|
shmem.ReadNewPoses([&](const protocol::DriverPoseShmem::AugmentedPose& augmented_pose) {
|
|
if (augmented_pose.deviceId >= 0 && augmented_pose.deviceId <= vr::k_unMaxTrackedDeviceCount) {
|
|
ctx.devicePoses[augmented_pose.deviceId] = augmented_pose.pose;
|
|
}
|
|
});
|
|
|
|
if (ctx.state == CalibrationState::None || ctx.state == CalibrationState::ContinuousStandby
|
|
|| (ctx.state == CalibrationState::Continuous && !calibration.isValid()))
|
|
{
|
|
if ((time - ctx.timeLastScan) >= 1.0)
|
|
{
|
|
ScanAndApplyProfile(ctx);
|
|
ctx.timeLastScan = time;
|
|
}
|
|
}
|
|
|
|
if (ctx.state == CalibrationState::ContinuousStandby) {
|
|
if (AssignTargets()) {
|
|
StartContinuousCalibration();
|
|
}
|
|
else {
|
|
ctx.wantedUpdateInterval = 0.5;
|
|
ctx.Log("Waiting for devices...");
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (ctx.state == CalibrationState::None) {
|
|
ctx.wantedUpdateInterval = 1.0;
|
|
return;
|
|
}
|
|
|
|
if (ctx.state == CalibrationState::Editing)
|
|
{
|
|
ctx.wantedUpdateInterval = 0.1;
|
|
|
|
if ((time - ctx.timeLastScan) >= 0.1)
|
|
{
|
|
ScanAndApplyProfile(ctx);
|
|
ctx.timeLastScan = time;
|
|
}
|
|
return;
|
|
}
|
|
|
|
bool ok = true;
|
|
|
|
if (ctx.referenceID == -1 || ctx.referenceID >= vr::k_unMaxTrackedDeviceCount) {
|
|
CalCtx.Log("Missing reference device\n");
|
|
ok = false;
|
|
}
|
|
if (ctx.targetID == -1 || ctx.targetID >= vr::k_unMaxTrackedDeviceCount)
|
|
{
|
|
CalCtx.Log("Missing target device\n");
|
|
ok = false;
|
|
}
|
|
|
|
if (ctx.state == CalibrationState::Begin)
|
|
{
|
|
|
|
char referenceSerial[256], targetSerial[256];
|
|
referenceSerial[0] = targetSerial[0] = 0;
|
|
vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.referenceID, vr::Prop_SerialNumber_String, referenceSerial, 256);
|
|
vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.targetID, vr::Prop_SerialNumber_String, targetSerial, 256);
|
|
|
|
char buf[256];
|
|
snprintf(buf, sizeof buf, "Reference device ID: %d, serial: %s\n", ctx.referenceID, referenceSerial);
|
|
CalCtx.Log(buf);
|
|
snprintf(buf, sizeof buf, "Target device ID: %d, serial %s\n", ctx.targetID, targetSerial);
|
|
CalCtx.Log(buf);
|
|
|
|
ScanAndApplyProfile(ctx);
|
|
|
|
if (!CalCtx.ReferencePoseIsValid())
|
|
{
|
|
CalCtx.Log("Reference device is not tracking\n"); ok = false;
|
|
}
|
|
|
|
if (!CalCtx.TargetPoseIsValid())
|
|
{
|
|
CalCtx.Log("Target device is not tracking\n"); ok = false;
|
|
}
|
|
|
|
if (ok) {
|
|
//ResetAndDisableOffsets(ctx.targetID);
|
|
ctx.state = CalibrationState::Rotation;
|
|
ctx.wantedUpdateInterval = 0.0;
|
|
|
|
CalCtx.Log("Starting calibration...\n");
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (!ok)
|
|
{
|
|
if (ctx.state != CalibrationState::Continuous) {
|
|
ctx.state = CalibrationState::None;
|
|
|
|
CalCtx.Log("Aborting calibration!\n");
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!CollectSample(ctx))
|
|
{
|
|
return;
|
|
}
|
|
|
|
CalCtx.Progress(calibration.SampleCount(), (int)CalCtx.SampleCount());
|
|
|
|
while (calibration.SampleCount() > CalCtx.SampleCount()) calibration.ShiftSample();
|
|
|
|
if (calibration.SampleCount() >= CalCtx.SampleCount())
|
|
{
|
|
LARGE_INTEGER start_time;
|
|
QueryPerformanceCounter(&start_time);
|
|
|
|
bool ok, lerp = false;
|
|
|
|
if (CalCtx.state == CalibrationState::Continuous) {
|
|
CalCtx.messages.clear();
|
|
calibration.enableStaticRecalibration = CalCtx.enableStaticRecalibration;
|
|
ok = calibration.ComputeIncremental(lerp, CalCtx.continuousCalibrationThreshold);
|
|
}
|
|
else {
|
|
calibration.enableStaticRecalibration = false;
|
|
ok = calibration.ComputeOneshot();
|
|
}
|
|
|
|
if (calibration.isValid()) {
|
|
ctx.calibratedRotation = calibration.EulerRotation();
|
|
ctx.calibratedTranslation = calibration.Transformation().translation() * 100.0; // convert to cm units for profile storage
|
|
|
|
auto vrTrans = VRTranslationVec(ctx.calibratedTranslation);
|
|
auto vrRot = VRRotationQuat(Eigen::Quaterniond(calibration.Transformation().rotation()));
|
|
|
|
ctx.validProfile = true;
|
|
SaveProfile(ctx);
|
|
|
|
ScanAndApplyProfile(ctx);
|
|
|
|
CalCtx.Log("Finished calibration, profile saved\n");
|
|
}
|
|
else
|
|
{
|
|
CalCtx.Log("Calibration failed.\n");
|
|
}
|
|
|
|
LARGE_INTEGER end_time;
|
|
QueryPerformanceCounter(&end_time);
|
|
LARGE_INTEGER freq;
|
|
QueryPerformanceFrequency(&freq);
|
|
double duration = (end_time.QuadPart - start_time.QuadPart) / (double)freq.QuadPart;
|
|
Metrics::computationTime.Push(duration * 1000.0);
|
|
|
|
Metrics::WriteLogEntry();
|
|
|
|
if (CalCtx.state != CalibrationState::Continuous) {
|
|
ctx.state = CalibrationState::None;
|
|
calibration.Clear();
|
|
}
|
|
else {
|
|
for (int i = 0; i < 10; i++) calibration.ShiftSample();
|
|
}
|
|
}
|
|
}
|
|
|
|
void LoadChaperoneBounds()
|
|
{
|
|
vr::VRChaperoneSetup()->RevertWorkingCopy();
|
|
|
|
uint32_t quadCount = 0;
|
|
vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);
|
|
|
|
CalCtx.chaperone.geometry.resize(quadCount);
|
|
vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], &quadCount);
|
|
vr::VRChaperoneSetup()->GetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
|
|
vr::VRChaperoneSetup()->GetWorkingPlayAreaSize(&CalCtx.chaperone.playSpaceSize.v[0], &CalCtx.chaperone.playSpaceSize.v[1]);
|
|
CalCtx.chaperone.valid = true;
|
|
}
|
|
|
|
void ApplyChaperoneBounds()
|
|
{
|
|
vr::VRChaperoneSetup()->RevertWorkingCopy();
|
|
vr::VRChaperoneSetup()->SetWorkingCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], (uint32_t)CalCtx.chaperone.geometry.size());
|
|
vr::VRChaperoneSetup()->SetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
|
|
vr::VRChaperoneSetup()->SetWorkingPlayAreaSize(CalCtx.chaperone.playSpaceSize.v[0], CalCtx.chaperone.playSpaceSize.v[1]);
|
|
vr::VRChaperoneSetup()->CommitWorkingCopy(vr::EChaperoneConfigFile_Live);
|
|
}
|
|
|
|
void DebugApplyRandomOffset() {
|
|
protocol::Request req(protocol::RequestDebugOffset);
|
|
Driver.SendBlocking(req);
|
|
} |