#include #include "survive_config.h" #include #include #include #include #include #include "linmath.h" #include #include #include #if defined(__FreeBSD__) || defined(__APPLE__) #include #else #include //for alloca #include #endif static int ttDebug = 0; #define PointToFlts(x) ((FLT*)(x)) typedef struct { FLT x; FLT y; FLT z; } Point; void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {} void updateHeader(FILE * file) {} void writeAxes(FILE * file) {} void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {} void writePcdHeader(FILE * file) {} void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {} void markPointWithStar(FILE *file, Point point, unsigned int color) {} typedef struct { Point point; // location of the sensor on the tracked object; Point normal; // unit vector indicating the normal for the sensor double theta; // "horizontal" angular measurement from lighthouse radians double phi; // "vertical" angular measurement from lighthouse in radians. } TrackedSensor; typedef struct { size_t numSensors; TrackedSensor sensor[0]; } TrackedObject; #ifndef M_PI #define M_PI 3.14159265358979323846264338327 #endif #define SQUARED(x) ((x)*(x)) typedef union { struct { unsigned char Blue; unsigned char Green; unsigned char Red; unsigned char Alpha; }; uint32_t long_value; } RGBValue; static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 }; static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 }; static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 }; static const double WORLD_BOUNDS = 100; #define MAX_TRACKED_POINTS 40 static const float DefaultPointsPerOuterDiameter = 60; typedef struct { FLT down[3]; // populated by the IMU for posing //Stuff #define OLD_ANGLES_BUFF_LEN 3 FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. int lastAxis[NUM_LIGHTHOUSES]; Point lastLhPos[NUM_LIGHTHOUSES]; // FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; FLT lastLhRotQuat[NUM_LIGHTHOUSES][4]; } ToriData; static FLT distance(Point a, Point b) { FLT x = a.x - b.x; FLT y = a.y - b.y; FLT z = a.z - b.z; return FLT_SQRT(x*x + y*y + z*z); } Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) { Matrix3x3 result; FLT v1[3] = { 0, 0, 1 }; FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; normalize3d(v2, v2); rotation_between_vecs_to_m3(&result, v1, v2); // Useful for debugging... //FLT v2b[3]; //rotate_vec(v2b, v1, result); return result; } typedef struct { Point a; Point b; FLT angle; FLT tanAngle; // tangent of angle Matrix3x3 rotation; Matrix3x3 invRotation; // inverse of rotation char ai; char bi; } PointsAndAngle; Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) { Point q; double pf[3] = { p.x, p.y, p.z }; q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x; q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y; q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z; return q; } double angleFromPoints(Point p1, Point p2, Point center) { Point v1, v2, v1norm, v2norm; v1.x = p1.x - center.x; v1.y = p1.y - center.y; v1.z = p1.z - center.z; v2.x = p2.x - center.x; v2.y = p2.y - center.y; v2.z = p2.z - center.z; double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); v1norm.x = v1.x / v1mag; v1norm.y = v1.y / v1mag; v1norm.z = v1.z / v1mag; double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); v2norm.x = v2.x / v2mag; v2norm.y = v2.y / v2mag; v2norm.z = v2.z / v2mag; double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; double angle = acos(res); return angle; } Point midpoint(Point a, Point b) { Point m; m.x = (a.x + b.x) / 2; m.y = (a.y + b.y) / 2; m.z = (a.z + b.z) / 2; return m; } // What we're doing here is: // * Given a point in space // * And points and a lighthouse angle that implicitly define a torus // * for that torus, what is the toroidal angle of the plane that will go through that point in space // * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? void estimateToroidalAndPoloidalAngleOfPoint( PointsAndAngle *pna, Point point, double *toroidalSin, double *toroidalCos, double *poloidalAngle, double *poloidalSin) { // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus. // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. Matrix3x3 rot = pna->invRotation; Point origin; origin.x = 0; origin.y = 0; origin.z = 0; Point m = midpoint(pna->a, pna->b); // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them // This will be their representation in the "friendly" coordinate system Point pointF; // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. pointF.x = point.x - m.x; pointF.y = point.y - m.y; pointF.z = point.z - m.z; // now we'll undo the rotation part. pointF = RotateAndTranslatePoint(pointF, rot, origin); // hooray, now pointF is in our more-friendly coordinate system. // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the // angle between a vector to pointF and a vector along the x axis. FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x)); *toroidalSin = pointF.y / toroidalHyp; *toroidalCos = pointF.x / toroidalHyp; //*toroidalAngle = atan(pointF.y / pointF.x); //if (pointF.x < 0) //{ // *toroidalAngle += M_PI; //} //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001); //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001); // SCORE!! We've got the toroidal angle. We're half done! // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle // so that the point we're focusin on will lie on the x/z plane. We then should translate the // torus so that the center of the poloidal circle is at the origin. At that point, it will // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a // vector from the origin to the point. // okay, instead of rotating the torus & point by the toroidal angle to get the point on // the xz plane, we're going to take advantage of the radial symmetry of the torus // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent // this new coordinate system Point pointG; pointG.z = pointF.z; pointG.y = 0; pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, // our next step will be to shift it down so that the center of the poloidal circle is at the origin. // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat // this as a 2D problem. I think we're getting close... // I stole these lines from the torus generator. Gonna need the poloidal radius. double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance. double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. // The shift along the X axis will be the toroidal radius. Point pointH; pointH.z = pointG.z; pointH.y = pointG.y; pointH.x = pointG.x - toroidalRadius; // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, // that will be the poloidal angle we're looking for. (crosses fingers) FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x)); *poloidalSin = pointH.z / poloidalHyp; *poloidalAngle = atan(pointH.z / pointH.x); if (pointH.x < 0) { *poloidalAngle += M_PI; } //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); // Wow, that ended up being not so much code, but a lot of interesting trig. // can't remember the last time I spent so much time working through each line of code. return; } #define MAX_POINT_PAIRS 100 FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) { FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); return angle; } // This provides a pretty good estimate of the angle above, probably better // the further away the lighthouse is. But, it's not crazy-precise. // It's main advantage is speed. FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) { FLT p = (a->phi - b->phi); FLT d = (a->theta - b->theta); FLT adjd = FLT_SIN((a->phi + b->phi) / 2); FLT adjP = FLT_SIN((a->theta + b->theta) / 2); FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); return pythAngle; } Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin) { Point result; FLT distanceBetweenPoints = distance(pna->a, pna->b); Point m = midpoint(pna->a, pna->b); Matrix3x3 rot = pna->rotation; FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; result.z = poloidalRadius*poloidalSin; result = RotateAndTranslatePoint(result, rot, m); return result; } FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) { double toroidalSin = 0; double toroidalCos = 0; double poloidalAngle = 0; double poloidalSin = 0; estimateToroidalAndPoloidalAngleOfPoint( pna, pointIn, &toroidalSin, &toroidalCos, &poloidalAngle, &poloidalSin); Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin); FLT dist = distance(pointIn, torusPoint); // This is some voodoo black magic. This is here to solve the problem that the origin // (which is near the center of all the tori) erroniously will rank as a good match. // through a lot of empiracle testing on how to compensate for this, the "fudge factor" // below ended up being the best fit. As simple as it is, I have a strong suspicion // that there's some crazy complex thesis-level math that could be used to derive this // but it works so we'll run with it. // Note that this may be resulting in a skewing of the found location by several millimeters. // it is not clear if this is actually removing existing skew (to get a more accurate value) // or if it is introducing an undesirable skew. double fudge = FLT_SIN((poloidalAngle - M_PI) / 2); dist = dist / fudge; return dist; } int compareFlts(const void * a, const void * b) { FLT a2 = *(const FLT*)a; FLT b2 = *(const FLT*)b; return (a2 > b2) - (a2 < b2); } FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) { FLT fitness; FLT resultSum = 0; FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); FLT worstFitness = 0; for (size_t i = 0; i < pnaCount; i++) { fitness = getPointFitnessForPna(pointIn, &(pna[i])); if (worstFitness < fitness) { worstFitness = fitness; } fitnesses[i] = FLT_FABS(fitness); if (0) { printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); } } qsort(fitnesses, pnaCount, sizeof(FLT), compareFlts); //printf("wf[%f]\n", worstFitness); // Note that we're only using the best 70% of the tori. // This is to remove any "bad" outliers. // TODO: better algorithms exist. for (size_t i = 0; i < (size_t)(pnaCount * 0.70); i++) { resultSum += SQUARED(fitnesses[i]); } return 1 / FLT_SQRT(resultSum); } // TODO: Use a central point instead of separate "minus" points for each axis. This will reduce // the number of fitness calls by 1/3. Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) { Point result; FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0); Point tmpXplus = pointIn; Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude) { FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); FLT scale = desiredMagnitude / distanceIn; Point result = vectorIn; result.x *= scale; result.y *= scale; result.z *= scale; return result; } Point getAvgPoints(Point a, Point b) { Point result; result.x = (a.x + b.x) / 2; result.y = (a.y + b.y) / 2; result.z = (a.z + b.z) / 2; return result; } // This is modifies the basic gradient descent algorithm to better handle the shallow valley case, // which appears to be typical of this convergence. static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) { int i = 0; FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0); Point lastPoint = initialEstimate; // The values below are somewhat magic, and definitely tunable // The initial vlue of g will represent the biggest step that the gradient descent can take at first. // bigger values may be faster, especially when the initial guess is wildly off. // The downside to a bigger starting guess is that if we've picked a good guess at the local minima // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. // That's fairly unlikely with the lighthouse problem, from expereince. // The other downside is that if it's too big, we may have to spend a few iterations before it gets down // to a size that doesn't jump us out of our minima. // The terminal value of g represents how close we want to get to the local minima before we're "done" // The change in value of g for each iteration is intentionally very close to 1. // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; Point point1 = lastPoint; // let's get 3 iterations of gradient descent here. Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); Point gradientN1 = getNormalizedAndScaledVector(gradient1, g); Point point2; point2.x = point1.x + gradientN1.x; point2.y = point1.y + gradientN1.y; point2.z = point1.z + gradientN1.z; Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); Point gradientN2 = getNormalizedAndScaledVector(gradient2, g); Point point3; point3.x = point2.x + gradientN2.x; point3.y = point2.y + gradientN2.y; point3.z = point2.z + gradientN2.z; // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging // converging gradient descent makes. Instead of using the gradient as the best indicator of // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically // following *that* vector. As it turns out, this works *amazingly* well. Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4); Point point4; point4.x = point3.x + specialGradient.x; point4.y = point3.y + specialGradient.y; point4.z = point3.z + specialGradient.z; FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0); if (newMatchFitness > lastMatchFitness) { if (logFile) { writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); } lastMatchFitness = newMatchFitness; lastPoint = point4; #ifdef TORI_DEBUG printf("+"); #endif } else { #ifdef TORI_DEBUG printf("-"); #endif g *= 0.7; } // from empiracle evidence, we're probably "good enough" at this point. // So, even though we could still improve, we're likely to be improving // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. if (i > 120) { //printf("i got big"); break; } } if (ttDebug) printf(" i=%3d ", i); return lastPoint; } // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormH, t1H, t2H); normalize3d(tNormH, tNormH); // Now do the same for the vertical sweep // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormV, t1V, t2V); normalize3d(tNormV, tNormV); // First, where is the sensor in the object's reference frame? FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; // Where is the point, in the reference frame of the lighthouse? // This has two steps, first we translate from the object's location being the // origin to the lighthouse being the origin. // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. // We need an arbitrary vector from the plane to the point. // Since the plane goes through the origin, this is trivial. // The sensor point itself is such a vector! // And go calculate the distances! // TODO: don't need to ABS these because we square them below. FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); fitness += SQUARED(dH); fitness += SQUARED(dV); } fitness = FLT_SQRT(fitness); return fitness; } FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj) { // For this fitness calculator, we're going to use the rotation information to figure out where // we expect to see the tracked object sensors, and we'll do a sum of squares to grade // the quality of the guess formed by the AxisAngle; FLT fitness = 0; // for each point in the tracked object for (int i=0; i< obj->numSensors; i++) { // let's see... we need to figure out where this sensor should be in the LH reference frame. FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z}; // And this puppy needs to be rotated... rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]); // Now, the vector indicating the position of the sensor, as seen by the lighthouse is: FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)}; // and the vector we're calculating given the rotation passed in is the same as the sensor location: FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]}; FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh ); fitness += SQUARED(angleBetween); } return 1/FLT_SQRT(fitness); } // This figures out how far away from the scanned planes each point is, then does a sum of squares // for the fitness. // // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormH, t1H, t2H); normalize3d(tNormH, tNormH); // Now do the same for the vertical sweep // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormV, t1V, t2V); normalize3d(tNormV, tNormV); // First, where is the sensor in the object's reference frame? FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; // Where is the point, in the reference frame of the lighthouse? // This has two steps, first we translate from the object's location being the // origin to the lighthouse being the origin. // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. // We need an arbitrary vector from the plane to the point. // Since the plane goes through the origin, this is trivial. // The sensor point itself is such a vector! // And go calculate the distances! // TODO: don't need to ABS these because we square them below. FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); fitness += SQUARED(dH); fitness += SQUARED(dV); } fitness = FLT_SQRT(fitness); return 1/fitness; } // interesting-- this is one place where we could use any sensors that are only hit by // 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); return throwaway; } FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormH, t1H, t2H); normalize3d(tNormH, tNormH); // Now do the same for the vertical sweep // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; // the normal is the cross of two vectors on the plane. cross3d(tNormV, t1V, t2V); normalize3d(tNormV, tNormV); // First, where is the sensor in the object's reference frame? FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; // Where is the point, in the reference frame of the lighthouse? // This has two steps, first we translate from the object's location being the // origin to the lighthouse being the origin. // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. // We need an arbitrary vector from the plane to the point. // Since the plane goes through the origin, this is trivial. // The sensor point itself is such a vector! // And go calculate the distances! // TODO: don't need to ABS these because we square them below. FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); fitness += SQUARED(dH); fitness += SQUARED(dV); } fitness = FLT_SQRT(fitness); return 1/fitness; } void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) { FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj); FLT tmp0plus[4]; quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness; FLT tmp1plus[4]; quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness; FLT tmp2plus[4]; quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness; FLT tmp3plus[4]; quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness; return; } void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) { FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj); FLT tmp0plus[4]; quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness; FLT tmp1plus[4]; quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness; FLT tmp2plus[4]; quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness; FLT tmp3plus[4]; quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness; return; } //void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) //{ // quatnormalize(vectorToScale, vectorToScale); // quatscale(vectorToScale, vectorToScale, desiredMagnitude); // return; //} void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) { quatnormalize(vectorToScale, vectorToScale); quatscale(vectorToScale, vectorToScale, desiredMagnitude); //vectorToScale[3] = desiredMagnitude; return; } 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 RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj); quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable // The initial vlue of g will represent the biggest step that the gradient descent can take at first. // bigger values may be faster, especially when the initial guess is wildly off. // The downside to a bigger starting guess is that if we've picked a good guess at the local minima // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. // That's fairly unlikely with the lighthouse problem, from expereince. // The other downside is that if it's too big, we may have to spend a few iterations before it gets down // to a size that doesn't jump us out of our minima. // The terminal value of g represents how close we want to get to the local minima before we're "done" // The change in value of g for each iteration is intentionally very close to 1. // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) { i++; FLT point1[4]; quatcopy(point1, rotOut); // let's get 3 iterations of gradient descent here. FLT gradient1[4]; normalize3d(point1, point1); getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient1,g); FLT point2[4]; quatadd(point2, gradient1, point1); //quatnormalize(point2,point2); normalize3d(point1, point1); FLT gradient2[4]; getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient2,g); FLT point3[4]; quatadd(point3, gradient2, point2); normalize3d(point1, point1); //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging // converging gradient descent makes. Instead of using the gradient as the best indicator of // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically // following *that* vector. As it turns out, this works *amazingly* well. FLT specialGradient[4]; quatsub(specialGradient,point3,point1); // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. getNormalizedAndScaledRotationGradient(specialGradient,g/4); FLT point4[4]; quatadd(point4, specialGradient, point3); //quatnormalize(point4,point4); normalize3d(point1, point1); FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; } else { //#ifdef TORI_DEBUG //printf("- , %f\n", point4[3]); //#endif g *= 0.7; } if (i > 998) { //printf("Ri got big"); break; } } if (ttDebug) printf(" Ri=%d ", i); } //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) { 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(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 // The initial vlue of g will represent the biggest step that the gradient descent can take at first. // bigger values may be faster, especially when the initial guess is wildly off. // The downside to a bigger starting guess is that if we've picked a good guess at the local minima // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. // That's fairly unlikely with the lighthouse problem, from expereince. // The other downside is that if it's too big, we may have to spend a few iterations before it gets down // to a size that doesn't jump us out of our minima. // The terminal value of g represents how close we want to get to the local minima before we're "done" // The change in value of g for each iteration is intentionally very close to 1. // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. for (FLT g = 0.1; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; quatcopy(point1, rotOut); // let's get 3 iterations of gradient descent here. FLT gradient1[4]; //normalize3d(point1, point1); getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient1,g); FLT point2[4]; quatadd(point2, gradient1, point1); quatnormalize(point2,point2); //normalize3d(point1, point1); FLT gradient2[4]; getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient2,g); FLT point3[4]; quatadd(point3, gradient2, point2); //normalize3d(point1, point1); quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging // converging gradient descent makes. Instead of using the gradient as the best indicator of // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically // following *that* vector. As it turns out, this works *amazingly* well. FLT specialGradient[4]; quatsub(specialGradient,point3,point1); // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. getNormalizedAndScaledRotationGradient(specialGradient,g/4); FLT point4[4]; quatadd(point4, specialGradient, point3); quatnormalize(point4,point4); //normalize3d(point1, point1); FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.04; //printf("+"); //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else { //#ifdef TORI_DEBUG //printf("- , %f\n", point4[3]); //#endif g *= 0.7; //printf("-"); //printf("%3f", lastMatchFitness); } } if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } void SolveForRotation(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); // TODO: Need to use the quaternion version here!!! //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); //WhereIsTheTrackedObjectQuaternion(rotOut, 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, PoserData *pd, char doLogOutput, SurvivePose *additionalTx, const int lh, const int setLhCalibration) { ToriData *toriData = so->PoserData; //printf("Solving for Lighthouse\n"); //printf("obj->numSensors = %d;\n", obj->numSensors); //for (int i=0; i < obj->numSensors; i++) //{ // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); //} PointsAndAngle pna[MAX_POINT_PAIRS]; volatile size_t sizeNeeded = sizeof(pna); Point avgNorm = { 0 }; FLT smallestAngle = 20.0; FLT largestAngle = 0; size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) { for (unsigned int j = 0; j < i; j++) { if (pnaCount < MAX_POINT_PAIRS) { pna[pnaCount].a = obj->sensor[i].point; pna[pnaCount].b = obj->sensor[j].point; pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]); //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); if (pna[pnaCount].angle < smallestAngle) { smallestAngle = pna[pnaCount].angle; } if (pna[pnaCount].angle > largestAngle) { largestAngle = pna[pnaCount].angle; } double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); pna[pnaCount].ai = i; pna[pnaCount].bi = j; pnaCount++; } } avgNorm.x += obj->sensor[i].normal.x; avgNorm.y += obj->sensor[i].normal.y; avgNorm.z += obj->sensor[i].normal.z; } avgNorm.x = avgNorm.x / obj->numSensors; avgNorm.y = avgNorm.y / obj->numSensors; avgNorm.z = avgNorm.z / obj->numSensors; FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z }; FILE *logFile = NULL; if (doLogOutput) { logFile = fopen("pointcloud2.pcd", "wb"); writePcdHeader(logFile); writeAxes(logFile); } // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); // arbitrarily picking a value of 8 meters out to start from. // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse // since this is least likely to pick the incorrect "mirror" point that would send us // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) Point p1 = getNormalizedAndScaledVector(avgNorm, 8); // if the last lighthouse position has been populated (extremely rare it would be 0) if (toriData->lastLhPos[lh].x != 0) { p1.x = toriData->lastLhPos[lh].x; p1.y = toriData->lastLhPos[lh].y; p1.z = toriData->lastLhPos[lh].z; } // refinedEstimateGd is the estimate for the location of the lighthouse in the tracked // object's local coordinate system. Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; // here we're checking the direction of the found point against the average direction of the // normal direction of the sensors that saw the light pulse. // This is because there are two possible points of convergence for the tori. One is the correct // location of the lighthouse. The other is in almost exactly the opposite direction. // The easiest way to determine that we've converged correctly is to see if the sensors' normal // are pointing in the direction of the point we've converged on. // if we have converged on the wrong point, we can try to converge one more time, using a starting estimate of // the point we converged on rotated to be directly opposite of its current position. Such a point // is guaranteed, in practice, to converge on the other location. // Note: in practice, we pretty much always converge on the correct point in the first place, // but this check just makes extra sure. FLT a1 = anglebetween3d(pf1, avgNormF); if (a1 > M_PI / 2) { Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z }; refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); } FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //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->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 // system, find the rotation of the lighthouse, again in the // 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->lastLhRotQuat[lh][0] = rotQuat[0]; // toriData->lastLhRotQuat[lh][1] = rotQuat[1]; // toriData->lastLhRotQuat[lh][2] = rotQuat[2]; // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; //} FLT rotQuat2[4]; FLT rot2[4]; quatfromaxisangle(rotQuat2, rot, rot[3]); axisanglefromquat(&(rot2[3]), rot2, rotQuat); // WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions. // WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); <--------------This is hte one we need to use, might need to be fixed. //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; //quatrotatevector(tmpPos, rotQuat, tmpPos); //} //static int foo = 0; //if (0 == foo) if (setLhCalibration) { //foo = 1; if (so->ctx->bsd[lh].PositionSet) { printf("Warning: resetting base station calibration data"); } SurvivePose lighthousePose; FLT invRot[4]; quatgetreciprocal(lighthousePose.Rot, rotQuat); lighthousePose.Pos[0] = refinedEstimateGd.x; lighthousePose.Pos[1] = refinedEstimateGd.y; lighthousePose.Pos[2] = refinedEstimateGd.z; SurvivePose assumedObj = {0}; FLT negZ[3] = {0, 0, 1}; quatfrom2vectors(assumedObj.Rot, toriData->down, negZ); PoserData_lighthouse_pose_func(pd, so, lh, additionalTx, &lighthousePose, &assumedObj); } FLT wcPos[3]; // position in wold coordinates // Take the position of the tracked object from the lighthouse's // frame of reference, and then rotate it in the reverse // direction of the orientation of the lighthouse // in the world reference freame. // Then, change the offset based on the position of the lighthouse // in the world reference frame. // The result is the position of the tracked object // in the world reference frame. quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; FLT newOrientation[4]; //quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot); // turns the wrong way quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, rotQuat); // turns the wrong way FLT invRot[4]; quatgetreciprocal(invRot, rotQuat); //quatrotateabout(newOrientation, invRot, so->ctx->bsd[lh].Pose.Rot); // turns correctly, rotations not aligned //quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, invRot); // turns correctly, rotations not aligned FLT invPoseRot[4]; quatgetreciprocal(invPoseRot, so->ctx->bsd[lh].Pose.Rot); //quatrotateabout(newOrientation, rotQuat, invPoseRot); // turns the wrong way, rotations not aligned //quatrotateabout(newOrientation, invPoseRot, rotQuat); // turns the wrong way, rotations not aligned //FLT invRot[4]; //quatgetreciprocal(invRot, rotQuat); //quatrotateabout(newOrientation, invRot, invPoseRot); // turns correctly, rotations aligned <-- This seems to be the best. //quatrotateabout(newOrientation, invPoseRot, invRot); // turns correctly, rotations aligned, (x & y flipped?) quatnormalize(newOrientation, newOrientation); so->OutPose.Pos[0] = wcPos[0]; so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; so->OutPose.Rot[0] = newOrientation[0]; so->OutPose.Rot[1] = newOrientation[1]; so->OutPose.Rot[2] = newOrientation[2]; so->OutPose.Rot[3] = newOrientation[3]; so->FromLHPose[lh].Pos[0] = so->OutPose.Pos[0]; so->FromLHPose[lh].Pos[1] = so->OutPose.Pos[1]; so->FromLHPose[lh].Pos[2] = so->OutPose.Pos[2]; so->FromLHPose[lh].Rot[0] = so->OutPose.Rot[0]; so->FromLHPose[lh].Rot[1] = so->OutPose.Rot[1]; so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2]; so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3]; if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); posOut[0] = wcPos[0]; posOut[1] = wcPos[1]; posOut[2] = wcPos[2]; quatOut[0] = so->OutPose.Rot[0]; quatOut[1] = so->OutPose.Rot[1]; quatOut[2] = so->OutPose.Rot[2]; quatOut[3] = so->OutPose.Rot[3]; if (logFile) { updateHeader(logFile); fclose(logFile); } toriData->lastLhPos[lh].x = refinedEstimateGd.x; toriData->lastLhPos[lh].y = refinedEstimateGd.y; toriData->lastLhPos[lh].z = refinedEstimateGd.z; return refinedEstimateGd; } static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalTx, int lh) { ToriData * td = so->PoserData; if (! so->ctx->bsd[lh].PositionSet) { // we don't know where we are! Augh!!! return; } //for (int i=0; i < so->sensor_ct; i++) //{ // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", // // i, // // x0, // // y0, // // x1, // // y1 // // ); // printf("%2d: %8.8f, %8.8f \n", // i, // x0, // y0 // ); //} //printf("\n"); TrackedObject *to; to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); { int sensorCount = 0; //// TODO: remove, for debug purposes only! //FLT downQuat[4]; //FLT negZ[3] = { 0,0,-1 }; ////quatfrom2vectors(downQuat, negZ, td->down); //quatfrom2vectors(downQuat, td->down, negZ); //// end TODO for (int i = 0; i < so->sensor_ct; i++) { int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) { FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; // TODO: remove these two lines!!! //quatrotatevector(norm, downQuat, norm); //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) sensorCount++; } } to->numSensors = sensorCount; if (sensorCount > 4) { SurvivePose pose; // TODO: This countdown stuff is a total hack! // it basically ignores all the logic to find the most reliable data points // and just grabs a sample and uses it. //static int countdown = 5; //if (countdown > 0 && so->ctx->objs[0] == so) //{ // SolveForLighthouse(pos, quat, to, so, 0, lh, 1); // countdown--; //} //else //{ // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); //} SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); if (so->ctx->poseproc) { so->ctx->poseproc(so, lh, &pose); } if (ttDebug) printf("!\n"); } } free(to); } int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; ToriData * td = so->PoserData; static int firstRun = 1; if (firstRun) { ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0); firstRun = 0; } if (!td) { so->PoserData = td = malloc(sizeof(ToriData)); memset(td, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { PoserDataIMU * tmpImu = (PoserDataIMU*)poserData; // store off data we can use for figuring out what direction is down when doing calibration. //TODO: looks like the data mask isn't getting set correctly. //if (tmpImu->datamask & 1) // accelerometer data is present { td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0]; td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1]; td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2]; } //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] ); //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] ); break; } case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)poserData; if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) { // should never happen. Famous last words... break; } int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); SurvivePose additionalTx; if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { if (0 == l->lh && axis) // only once per full cycle... { static unsigned int counter = 1; counter++; // let's just do this occasionally for now... if (counter % 4 == 0) QuickPose(so, poserData, &additionalTx, 0); } if (1 == l->lh && axis) // only once per full cycle... { static unsigned int counter = 1; counter++; // let's just do this occasionally for now... if (counter % 4 == 0) QuickPose(so, poserData, &additionalTx, 1); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; // and clear out the data. for (int i=0; i < SENSORS_PER_OBJECT; i++) { td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; } td->lastAxis[l->lh] = axis; } td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; PoserDataFullScene * fs = (PoserDataFullScene*)poserData; to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary // to being the down direction as defined by the accelerometer, then when we have come up // with world coordinate system, it will have Z oriented correctly. // let's get the quaternion that represents this rotation. FLT downQuat[4]; FLT negZ[3] = { 0,0,1 }; //quatfrom2vectors(downQuat, negZ, td->down); quatfrom2vectors(downQuat, td->down, negZ); FLT angle; FLT axis[3]; angleaxisfrom2vect(&angle, axis, td->down, negZ); //angleaxisfrom2vect(&angle, &axis, negZ, td->down); SurvivePose additionalTx; { int sensorCount = 0; for (int i = 0; i < so->sensor_ct; i++) { if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 { FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; quatrotatevector(norm, downQuat, norm); quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; FLT out[2]; survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out); to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) sensorCount++; } } to->numSensors = sensorCount; FLT pos[3], quat[4]; SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 0, 1); } { int sensorCount = 0; int lh = 1; for (int i = 0; i < so->sensor_ct; i++) { if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) { FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; quatrotatevector(norm, downQuat, norm); quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; FLT out[2]; survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out); to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } to->numSensors = sensorCount; FLT pos[3], quat[4]; SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 1, 1); } // This code block rotates the lighthouse fixes to account for any time the tracked object // is oriented other than +z = up //This REALLY DOESN'T WORK!!! //{ // for (int lh = 0; lh < 2; lh++) // { // quatrotatevector(&(so->ctx->bsd[lh].Pose.Pos[0]), downQuat, &(so->ctx->bsd[lh].Pose.Pos[0])); // //quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), &(so->ctx->bsd[lh].Pose.Rot[0]), downQuat); // quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), downQuat, &(so->ctx->bsd[lh].Pose.Rot[0])); // } //} for (int i=0; i < ctx->activeLighthouses; i++) { printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", i, ctx->bsd[i].Pose.Pos[0], ctx->bsd[i].Pose.Pos[1], ctx->bsd[i].Pose.Pos[2], ctx->bsd[i].Pose.Rot[0], ctx->bsd[i].Pose.Rot[1], ctx->bsd[i].Pose.Rot[2], ctx->bsd[i].Pose.Rot[3]); } config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); config_save(ctx, "config.json"); free(to); //printf( "Full scene data.\n" ); break; } case POSERDATA_DISASSOCIATE: { free( so->PoserData ); so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; } } return 0; } REGISTER_LINKTIME( PoserTurveyTori );