//rotates points from one frame of reference to another using a rotation matrix public static void RotatePoints(Matrix3x3 rotMatrix, List <Vector3> bodyFramePoints, List <Vector3> referenceFramePoints) { for (int i = 0; i < referenceFramePoints.Count; i++) { bodyFramePoints[i] = Matrix3x3.Multiply(referenceFramePoints[i], rotMatrix); } }
//Runs the calibration algorithm void calibrateThreadFunction() { addMessage("Starting Calibration with " + compassSamples.Count.ToString() + " samples."); cal = new Calibration(this); bool calSuccess = false; if (threeAxisButton.Checked) { calSuccess = cal.calibrate3(compassSamples); } else { calSuccess = cal.calibrate2(compassSamples); } if (calSuccess) { //Calibration was successful - redraw all the samples in green. addMessage("Calibration successful."); if (localFieldTextBox.Text == "") { setLocalField(cal.estimatedLocalField.ToString("F5")); } for (int i = 0; i < compassSamples.Count; i++) { //apply calibration Vector3 rawData = new Vector3(compassSamples[i].data[0], compassSamples[i].data[1], compassSamples[i].data[2]); Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform); //apply local mag field - calibrate returns a unit (mag 1) field calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData); lock (p) { if (twoAxisButton.Checked) { p.addPoint(new Vector3( calibratedData.X, calibratedData.Y, -calibratedData.Z), Brushes.Green); } else { p.addPoint(new Vector3( calibratedData.X, calibratedData.Z, -calibratedData.Y), Brushes.Green); } } } calibrated = true; } else { //failed addMessage("Calibration failed, try again."); } setButtonEnabled(sampleDoneButton, true, "Start"); }
//computes a rotation matrix based on a previous rotation matrix and a series of angle rotations //better algorithm then nextRotMatrix - still need to keep rotation < 180 degrees //This uses the rectangular rule public static Matrix3x3 nextRotMatrix2(Matrix3x3 rotMatrix, Vector3 rotations) { //This uses C2 = C1( I + (sin(w)/w)B + ((1 - cos(w))/w)B^2 ) //where w is the total rotation, I is the identity matrix and B is the scew symmetric form of the rotation vector Matrix3x3 I = new Matrix3x3(); I.matrix[0, 0] = 1; I.matrix[1, 0] = 0; I.matrix[2, 0] = 0; I.matrix[0, 1] = 0; I.matrix[1, 1] = 1; I.matrix[2, 1] = 0; I.matrix[0, 2] = 0; I.matrix[1, 2] = 0; I.matrix[2, 2] = 1; Matrix3x3 B = new Matrix3x3(); B.matrix[0, 0] = 0; B.matrix[1, 0] = -rotations.Z; B.matrix[2, 0] = rotations.Y; B.matrix[0, 1] = rotations.Z; B.matrix[1, 1] = 0; B.matrix[2, 1] = -rotations.X; B.matrix[0, 2] = -rotations.Y; B.matrix[1, 2] = rotations.X; B.matrix[2, 2] = 0; double totalRotation = Vector3.Length(rotations); Matrix3x3 smallRot; //Don't divide by 0 if (totalRotation > 0) { smallRot = Matrix3x3.Add(Matrix3x3.Add( I, Matrix3x3.Multiply(Math.Sin(totalRotation) / totalRotation, B)), Matrix3x3.Multiply((1 - Math.Cos(totalRotation)) / (totalRotation * totalRotation), Matrix3x3.Multiply(B, B)) ); } else { smallRot = I; } Matrix3x3 newRotMatrix = Matrix3x3.Multiply(rotMatrix, smallRot); //If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking //The determinant should be 1 //double det = Matrix3x3.Determinant(newRotMatrix) //This should give an Identity matrix //Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(newRotMatrix), newRotMatrix); //Normalize to the the vectors Unit length //return newRotMatrix; return(Matrix3x3.Normalize(newRotMatrix)); //TODO: We should really be doing an orthonormalization }
//computes a rotation matrix based on a previous rotation matrix and a series of angle rotations public static Matrix3x3 nextRotMatrix(Matrix3x3 rotMatrix, Vector3 rotations) { //assuming C(t2) = C(t1)A(t1) where A(t1) is the rotation matrix relating the body frame between time t1 and t2 (I + B) //A(t1) = [ 1 y z ] for small angles (<180 degrees). x, y and z are rotations about the axes // [ -y 1 x ] // [ -z -x 1 ] Matrix3x3 A = new Matrix3x3(); A.matrix[0, 0] = 1; A.matrix[1, 0] = rotations.Y; A.matrix[2, 0] = rotations.Z; A.matrix[0, 1] = -rotations.Y; A.matrix[1, 1] = 1; A.matrix[2, 1] = rotations.X; A.matrix[0, 2] = -rotations.Z; A.matrix[1, 2] = -rotations.X; A.matrix[2, 2] = 1; //Normalized to keep the vectors unit length Matrix3x3 newRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(rotMatrix, A)); return(newRotMatrix); }
void spatial_SpatialData(object sender, Phidgets.Events.SpatialDataEventArgs e) { //This displays a vector representing the measured field in red, and after calibration the calibrated field in green if (e.spatialData[0].MagneticField.Length == 3) { lock (p) { if (twoAxisButton.Checked) { p.originalVertices[4] = new Vector3( e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], -e.spatialData[0].MagneticField[2]); if (calibrated) { Vector3 rawData = new Vector3(e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], e.spatialData[0].MagneticField[2]); Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform); //apply local mag field - calibrate returns a unit (mag 1) field calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData); p.originalVertices[5] = new Vector3( calibratedData.X, calibratedData.Y, -calibratedData.Z); } } else { p.originalVertices[4] = new Vector3( e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[2], -e.spatialData[0].MagneticField[1]); if (calibrated) { Vector3 rawData = new Vector3(e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], e.spatialData[0].MagneticField[2]); Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform); //apply local mag field - calibrate returns a unit (mag 1) field calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData); p.originalVertices[5] = new Vector3( calibratedData.X, calibratedData.Z, -calibratedData.Y); } } } } //This samples magnetic field data if (sampling) { lock (compassSamples) { if (sampling) { sampleCounter++; if (sampleCounter >= samplesPerSample) { sampleCounter = 0; if (e.spatialData[0].MagneticField.Length == 3) { compassSamples.Add( new compassDataPoint( e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], e.spatialData[0].MagneticField[2] ) ); //This draws the magnetic vectors as dots lock (p) { if (twoAxisButton.Checked) { p.addPoint(new Vector3( e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], -e.spatialData[0].MagneticField[2]), Brushes.Red); } else { p.addPoint(new Vector3( e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[2], -e.spatialData[0].MagneticField[1]), Brushes.Red); } } } } } } } }