oculus1

annotate libovr/Src/Util/Util_MagCalibration.cpp @ 3:b069a5c27388

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