From ec564d70daa8c1a66018f9606b02b873ae792c84 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:03:37 -0700 Subject: Start work on determining rotation using quaternions only Rotation was previously approximated using axis/angle This change starts down the path of using quaternions exclusively. This change appears to give at least as good as answers as the axis/angle model in basic cases (also only tested with 1 lighthouse), but it is currently much slower and runs in unpredictable time. --- calibrate.c | 66 +++++++++++++++++++--- redist/linmath.c | 36 ++++++++++++ redist/linmath.h | 2 + src/poser_turveytori.c | 150 ++++++++++++++++++++++++++++++++++++++++--------- 4 files changed, 218 insertions(+), 36 deletions(-) diff --git a/calibrate.c b/calibrate.c index 8fb0986..cfd3e17 100644 --- a/calibrate.c +++ b/calibrate.c @@ -207,9 +207,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } // line for the (0,-y) to (0,+y)) @@ -233,11 +233,36 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } + // Small line to indicate (0,+y) + { + FLT tmp1[3]; + FLT tmp1out[3]; + FLT tmp2[3]; + FLT tmp2out[3]; + + tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); + tmp1[2] = 0; + tmp1[0] = ((pose.Pos[2] * 40.0)) * 0.3; + + quatrotatevector(tmp1out, pose.Rot, tmp1); + + tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)*0.7); + tmp2[2] = 0; + tmp2[0] = -(((pose.Pos[2] * 40.0)) * 0.3); + + quatrotatevector(tmp2out, pose.Rot, tmp2); + + CNFGTackSegment( + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + } // Small line to indicate (+x,0) { FLT tmp1[3]; @@ -245,13 +270,38 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) FLT tmp2[3]; FLT tmp2out[3]; - tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); tmp1[2] = 0; tmp1[1] = tmp1[0] * 0.3; quatrotatevector(tmp1out, pose.Rot, tmp1); - tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7); + tmp2[2] = 0; + tmp2[1] = -(tmp1[0] * 0.3); + + quatrotatevector(tmp2out, pose.Rot, tmp2); + + CNFGTackSegment( + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + } + // Small line to indicate (+x,0) + { + FLT tmp1[3]; + FLT tmp1out[3]; + FLT tmp2[3]; + FLT tmp2out[3]; + + tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7); + tmp1[2] = 0; + tmp1[1] = tmp1[0] * 0.3; + + quatrotatevector(tmp1out, pose.Rot, tmp1); + + tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); tmp2[2] = 0; tmp2[1] = -(tmp1[0] * 0.3); @@ -259,9 +309,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } diff --git a/redist/linmath.c b/redist/linmath.c index 5d51708..1d70ee1 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) } +void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) +{ + // this way might be fine, too. + //FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + // + //*angle = 2 * FLT_ATAN2(dist, q[0]); + + //axis[0] = q[1] / dist; + //axis[1] = q[2] / dist; + //axis[2] = q[3] / dist; + + + // Good mathematical foundation for this algorithm found here: + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm + + FLT tmp[4] = { q[0], q[1], q[2], q[3] }; + + quatnormalize(tmp, q); + + if (FLT_FABS(q[0] - 1) < FLT_EPSILON) + { + // we have a degenerate case where we're rotating approx. 0 degrees + *angle = 0; + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + return; + } + + axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0])); + axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0])); + axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0])); + + *angle = 2 * FLT_ACOS(tmp[0]); +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. diff --git a/redist/linmath.h b/redist/linmath.h index 8d6cf05..1876b34 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b ); void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); +void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat); + //Quaternion things... diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ae4592d..fed5762 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -87,7 +87,8 @@ typedef struct int lastAxis[NUM_LIGHTHOUSES]; Point lastLhPos[NUM_LIGHTHOUSES]; - FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; +// FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; + FLT lastLhRotQuat[NUM_LIGHTHOUSES][4]; } ToriData; @@ -774,6 +775,21 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + +// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!! + { + FLT axisAngle[4]; + + axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion); + + FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + int a = throwaway; + return throwaway; + } + + + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { @@ -1015,23 +1031,61 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } if (ttDebug) printf(" Ri=%d ", i); } -static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//{ +// FLT reverseRotation[4] = { rotation[0], rotation[1], rotation[2], -rotation[3] }; +// FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; +// +// //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); +// quatrotatevector(objPoint, rotation, objPoint); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +//} +static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; + + FLT inverseRotation[4]; + + quatgetreciprocal(inverseRotation, rotation); + + FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - quatrotatevector(objPoint, rotation, objPoint); - if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + quatrotatevector(posOut, inverseRotation, posOut); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } +//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) +//{ +// posOut[0] = -lhPoint.x; +// posOut[1] = -lhPoint.y; +// posOut[2] = -lhPoint.z; +// +// rotatearoundaxis(posOut, posOut, rotation, rotation[3]); +// +// if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); +//} static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + //{ + // FLT axisAngle[4]; + + // axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate); + + // FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + // int a = throwaway; + //} + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable @@ -1108,8 +1162,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; - if (ttDebug) printf("+"); - WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + printf("+"); + //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else { @@ -1117,12 +1171,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - if (ttDebug) printf("-"); + printf("-"); } } - if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1133,7 +1187,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; FLT quat1[4]; quatfromaxisangle(quat1, zAxis, theta); @@ -1152,6 +1206,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } +void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) + // not correcting for phi, but that's less important. + + + // Step 2, optimize the axis/ angle to match the data. + //RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + + //// Step 2, optimize the quaternion to match the data. + RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); + + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); + +} + static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) { @@ -1286,17 +1366,25 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + FLT rotQuat[4]; // this is a quaternion! // if we've already guessed at the rotation of the lighthouse, // then let's use that as a starting guess, because it's probably // going to make convergence happen much faster. - if (toriData->lastLhRotAxisAngle[lh][0] != 0) - { - rot[0] = toriData->lastLhRotAxisAngle[lh][0]; - rot[1] = toriData->lastLhRotAxisAngle[lh][1]; - rot[2] = toriData->lastLhRotAxisAngle[lh][2]; - rot[3] = toriData->lastLhRotAxisAngle[lh][3]; - } + //if (toriData->lastLhRotAxisAngle[lh][0] != 0) + //{ + // rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + // rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + // rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + // rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + //} + //if (toriData->lastLhRotQuat[lh][0] != 0) + //{ + // rotQuat[0] = toriData->lastLhRotQuat[lh][0]; + // rotQuat[1] = toriData->lastLhRotQuat[lh][1]; + // rotQuat[2] = toriData->lastLhRotQuat[lh][2]; + // rotQuat[3] = toriData->lastLhRotQuat[lh][3]; + //} // Given the relative position of the lighthouse // to the tracked object, in the tracked object's coordinate @@ -1304,22 +1392,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // tracked object's coordinate system. // TODO: I believe this could be radically improved // using an SVD. + SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + FLT objPos2[3]; - { - toriData->lastLhRotAxisAngle[lh][0] = rot[0]; - toriData->lastLhRotAxisAngle[lh][1] = rot[1]; - toriData->lastLhRotAxisAngle[lh][2] = rot[2]; - toriData->lastLhRotAxisAngle[lh][3] = rot[3]; - } + //{ + // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; + // toriData->lastLhRotQuat[lh][1] = rotQuat[1]; + // toriData->lastLhRotQuat[lh][2] = rotQuat[2]; + // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; + //} + + WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd); + WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); - WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat2[4]; + FLT rot2[4]; - FLT rotQuat[4]; + quatfromaxisangle(rotQuat2, rot, rot[3]); + axisanglefromquat(&(rot2[3]), rot2, rotQuat); - quatfromaxisangle(rotQuat, rot, rot[3]); //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; -- cgit v1.2.3