oculus1

annotate libovr/Src/Util/Util_MagCalibration.cpp @ 1:e2f9e4603129

find changesets by author, revision, files, or words in the commit message
added LibOVR and started a simple vr wrapper.
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 14 Sep 2013 16:14:59 +0300
parents
children b069a5c27388
rev   line source
nuclear@1 1 /************************************************************************************
nuclear@1 2
nuclear@1 3 Filename : Util_MagCalibration.cpp
nuclear@1 4 Content : Procedures for calibrating the magnetometer
nuclear@1 5 Created : April 16, 2013
nuclear@1 6 Authors : Steve LaValle, Andrew Reisse
nuclear@1 7
nuclear@1 8 Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
nuclear@1 9
nuclear@1 10 Use of this software is subject to the terms of the Oculus license
nuclear@1 11 agreement provided at the time of installation or download, or which
nuclear@1 12 otherwise accompanies this software in either electronic or hard copy form.
nuclear@1 13
nuclear@1 14 *************************************************************************************/
nuclear@1 15
nuclear@1 16 #include "Util_MagCalibration.h"
nuclear@1 17
nuclear@1 18 namespace OVR { namespace Util {
nuclear@1 19
nuclear@1 20 void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
nuclear@1 21 {
nuclear@1 22 Stat = Mag_AutoCalibrating;
nuclear@1 23 // This is a "hard" reset of the mag, so need to clear stored values
nuclear@1 24 sf.ClearMagCalibration();
nuclear@1 25 SampleCount = 0;
nuclear@1 26
nuclear@1 27 // reset the statistics
nuclear@1 28 MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
nuclear@1 29 MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
nuclear@1 30 MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
nuclear@1 31 MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
nuclear@1 32 }
nuclear@1 33
nuclear@1 34 unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
nuclear@1 35 {
nuclear@1 36 if (Stat != Mag_AutoCalibrating)
nuclear@1 37 return Stat;
nuclear@1 38
nuclear@1 39 Quatf q = sf.GetOrientation();
nuclear@1 40 Vector3f m = sf.GetMagnetometer();
nuclear@1 41
nuclear@1 42 InsertIfAcceptable(q, m);
nuclear@1 43
nuclear@1 44 if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
nuclear@1 45 {
nuclear@1 46 //LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z);
nuclear@1 47 //LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w);
nuclear@1 48 SetCalibration(sf);
nuclear@1 49 }
nuclear@1 50
nuclear@1 51 return Stat;
nuclear@1 52
nuclear@1 53 }
nuclear@1 54
nuclear@1 55 void MagCalibration::BeginManualCalibration(SensorFusion& sf)
nuclear@1 56 {
nuclear@1 57 Stat = Mag_ManuallyCalibrating;
nuclear@1 58 sf.ClearMagCalibration();
nuclear@1 59 SampleCount = 0;
nuclear@1 60 }
nuclear@1 61
nuclear@1 62 bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
nuclear@1 63 {
nuclear@1 64 switch (SampleCount)
nuclear@1 65 {
nuclear@1 66 // Initial sample is always acceptable
nuclear@1 67 case 0:
nuclear@1 68 return true;
nuclear@1 69 break;
nuclear@1 70 case 1:
nuclear@1 71 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@1 72 ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);
nuclear@1 73 break;
nuclear@1 74 case 2:
nuclear@1 75 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@1 76 (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@1 77 ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&
nuclear@1 78 ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);
nuclear@1 79 break;
nuclear@1 80 case 3:
nuclear@1 81 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@1 82 (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@1 83 (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&
nuclear@1 84 ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||
nuclear@1 85 (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||
nuclear@1 86 (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||
nuclear@1 87 (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));
nuclear@1 88 }
nuclear@1 89
nuclear@1 90 return false;
nuclear@1 91 }
nuclear@1 92
nuclear@1 93
nuclear@1 94 bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
nuclear@1 95 {
nuclear@1 96 // Update some statistics
nuclear@1 97 if (m.x < MinMagValues.x)
nuclear@1 98 MinMagValues.x = m.x;
nuclear@1 99 if (m.y < MinMagValues.y)
nuclear@1 100 MinMagValues.y = m.y;
nuclear@1 101 if (m.z < MinMagValues.z)
nuclear@1 102 MinMagValues.z = m.z;
nuclear@1 103 if (m.x > MaxMagValues.x)
nuclear@1 104 MaxMagValues.x = m.x;
nuclear@1 105 if (m.y > MaxMagValues.y)
nuclear@1 106 MaxMagValues.y = m.y;
nuclear@1 107 if (m.z > MaxMagValues.z)
nuclear@1 108 MaxMagValues.z = m.z;
nuclear@1 109 if (q.x < MinQuatValues.x)
nuclear@1 110 MinQuatValues.x = q.x;
nuclear@1 111 if (q.y < MinQuatValues.y)
nuclear@1 112 MinQuatValues.y = q.y;
nuclear@1 113 if (q.z < MinQuatValues.z)
nuclear@1 114 MinQuatValues.z = q.z;
nuclear@1 115 if (q.w < MinQuatValues.w)
nuclear@1 116 MinQuatValues.w = q.w;
nuclear@1 117 if (q.x > MaxQuatValues.x)
nuclear@1 118 MaxQuatValues.x = q.x;
nuclear@1 119 if (q.y > MaxQuatValues.y)
nuclear@1 120 MaxQuatValues.y = q.y;
nuclear@1 121 if (q.z > MaxQuatValues.z)
nuclear@1 122 MaxQuatValues.z = q.z;
nuclear@1 123 if (q.w > MaxQuatValues.w)
nuclear@1 124 MaxQuatValues.w = q.w;
nuclear@1 125 MagSpread = MaxMagValues - MinMagValues;
nuclear@1 126 QuatSpread = MaxQuatValues - MinQuatValues;
nuclear@1 127
nuclear@1 128 if (IsAcceptableSample(q, m))
nuclear@1 129 {
nuclear@1 130 MagSamples[SampleCount] = m;
nuclear@1 131 QuatSamples[SampleCount] = q;
nuclear@1 132 SampleCount++;
nuclear@1 133 return true;
nuclear@1 134 }
nuclear@1 135
nuclear@1 136 return false;
nuclear@1 137 }
nuclear@1 138
nuclear@1 139 Matrix4f MagCalibration::GetMagCalibration() const
nuclear@1 140 {
nuclear@1 141 Matrix4f calMat = Matrix4f();
nuclear@1 142 calMat.M[0][3] = -MagCenter.x;
nuclear@1 143 calMat.M[1][3] = -MagCenter.y;
nuclear@1 144 calMat.M[2][3] = -MagCenter.z;
nuclear@1 145 return calMat;
nuclear@1 146 }
nuclear@1 147
nuclear@1 148 bool MagCalibration::SetCalibration(SensorFusion& sf)
nuclear@1 149 {
nuclear@1 150 if (SampleCount < 4)
nuclear@1 151 return false;
nuclear@1 152
nuclear@1 153 MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
nuclear@1 154 Matrix4f calMat = GetMagCalibration();
nuclear@1 155 sf.SetMagCalibration(calMat);
nuclear@1 156 Stat = Mag_Calibrated;
nuclear@1 157 //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
nuclear@1 158
nuclear@1 159 return true;
nuclear@1 160 }
nuclear@1 161
nuclear@1 162
nuclear@1 163 // Calculate the center of a sphere that passes through p1, p2, p3, p4
nuclear@1 164 Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
nuclear@1 165 const Vector3f& p3, const Vector3f& p4)
nuclear@1 166 {
nuclear@1 167 Matrix4f A;
nuclear@1 168 int i;
nuclear@1 169 Vector3f p[4];
nuclear@1 170 p[0] = p1;
nuclear@1 171 p[1] = p2;
nuclear@1 172 p[2] = p3;
nuclear@1 173 p[3] = p4;
nuclear@1 174
nuclear@1 175 for (i = 0; i < 4; i++)
nuclear@1 176 {
nuclear@1 177 A.M[i][0] = p[i].x;
nuclear@1 178 A.M[i][1] = p[i].y;
nuclear@1 179 A.M[i][2] = p[i].z;
nuclear@1 180 A.M[i][3] = 1.0f;
nuclear@1 181 }
nuclear@1 182 float m11 = A.Determinant();
nuclear@1 183 OVR_ASSERT(m11 != 0.0f);
nuclear@1 184
nuclear@1 185 for (i = 0; i < 4; i++)
nuclear@1 186 {
nuclear@1 187 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@1 188 A.M[i][1] = p[i].y;
nuclear@1 189 A.M[i][2] = p[i].z;
nuclear@1 190 A.M[i][3] = 1.0f;
nuclear@1 191 }
nuclear@1 192 float m12 = A.Determinant();
nuclear@1 193
nuclear@1 194 for (i = 0; i < 4; i++)
nuclear@1 195 {
nuclear@1 196 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@1 197 A.M[i][1] = p[i].x;
nuclear@1 198 A.M[i][2] = p[i].z;
nuclear@1 199 A.M[i][3] = 1.0f;
nuclear@1 200 }
nuclear@1 201 float m13 = A.Determinant();
nuclear@1 202
nuclear@1 203 for (i = 0; i < 4; i++)
nuclear@1 204 {
nuclear@1 205 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@1 206 A.M[i][1] = p[i].x;
nuclear@1 207 A.M[i][2] = p[i].y;
nuclear@1 208 A.M[i][3] = 1.0f;
nuclear@1 209 }
nuclear@1 210 float m14 = A.Determinant();
nuclear@1 211
nuclear@1 212 float c = 0.5f / m11;
nuclear@1 213 return Vector3f(c*m12, -c*m13, c*m14);
nuclear@1 214 }
nuclear@1 215
nuclear@1 216 // Distance from p4 to the nearest point on a plane through p1, p2, p3
nuclear@1 217 float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
nuclear@1 218 const Vector3f& p3, const Vector3f& p4)
nuclear@1 219 {
nuclear@1 220 Vector3f v1 = p1 - p2;
nuclear@1 221 Vector3f v2 = p1 - p3;
nuclear@1 222 Vector3f planeNormal = v1.Cross(v2);
nuclear@1 223 planeNormal.Normalize();
nuclear@1 224 return (fabs((planeNormal * p4) - planeNormal * p1));
nuclear@1 225 }
nuclear@1 226
nuclear@1 227 }}