From ae522f8a06848d467c835d87772580fa7cceb5cd Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Feb 2017 11:42:46 -0700 Subject: Replaced rotation algorithm & cleanup --- tools/lighthousefind_tori/torus_localizer.c | 830 ++++++++++++++-------------- 1 file changed, 422 insertions(+), 408 deletions(-) (limited to 'tools/lighthousefind_tori/torus_localizer.c') diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c index 43dd9a2..22a0ce2 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -9,201 +9,197 @@ static double distance(Point a, Point b) { - double x = a.x - b.x; - double y = a.y - b.y; - double z = a.z - b.z; - return sqrt(x*x + y*y + z*z); + double x = a.x - b.x; + double y = a.y - b.y; + double z = a.z - b.z; + return 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 }; + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; - normalize_v3(v2); + normalize3d(v2,v2); - rotation_between_vecs_to_mat3(result.val, v1, v2); + rotation_between_vecs_to_m3(&result, v1, v2); - FLT result2[9]; + // Useful for debugging... + FLT v2b[3]; + rotate_vec(v2b, v1, result); - rotation_between_vecs_to_m3(result2, v1, v2); - - result.val[0][0] = result2[0]; - result.val[0][1] = result2[1]; - result.val[0][2] = result2[2]; - result.val[1][0] = result2[3]; - result.val[1][1] = result2[4]; - result.val[1][2] = result2[5]; - result.val[2][0] = result2[6]; - result.val[2][1] = result2[7]; - result.val[2][2] = result2[8]; - - return result; + return result; } +//void TestGetRotationMatrixForTorus() +//{ +// // a={x=0.079967096447944641 y=0.045225199311971664 z=0.034787099808454514 } +// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } +// +// // a={x=0.050822000950574875 y=0.052537899464368820 z=0.033285100013017654 } +// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } +// +//} + Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) { - Point q; + 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; + 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; + 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; + 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; + 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 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 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 res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; - double angle = acos(res); + double angle = acos(res); - return angle; + 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; + 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; + return m; } -// This is the second incarnation of the torus generator. It is intended to differ from the initial torus generator by -// producing a point cloud of a torus where the points density is more uniform across the torus. This will allow -// us to be more efficient in finding a solution. +// This torus generator creates a point cloud of the given torus, and attempts to keep the +// density of the points fairly uniform across the surface of the torus. void partialTorusGenerator( - Point p1, - Point p2, - double toroidalStartAngle, - double toroidalEndAngle, - double poloidalStartAngle, - double poloidalEndAngle, - double lighthouseAngle, - double toroidalPrecision, - Point **pointCloud) + Point p1, + Point p2, + double toroidalStartAngle, + double toroidalEndAngle, + double poloidalStartAngle, + double poloidalEndAngle, + double lighthouseAngle, + double toroidalPrecision, + Point **pointCloud) { - double poloidalRadius = 0; - double toroidalRadius = 0; + double poloidalRadius = 0; + double toroidalRadius = 0; - Point m = midpoint(p1, p2); - double distanceBetweenPoints = distance(p1, p2); + Point m = midpoint(p1, p2); + double distanceBetweenPoints = distance(p1, p2); - // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally - // thinking the tori converge at the location of the tracked object instead of at the lighthouse. - double centralAngleToIgnore = lighthouseAngle * 3; + // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally + // thinking the tori converge at the location of the tracked object instead of at the lighthouse. + double centralAngleToIgnore = lighthouseAngle * 3; - Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); + Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); - toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); + toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); - poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); + poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2)); - double poloidalPrecision = M_PI * 2 / toroidalPrecision; + double poloidalPrecision = M_PI * 2 / toroidalPrecision; - //unsigned int pointCount = toroidalPrecision * toroidalPrecision / 2 * (toroidalEndAngle - toroidalStartAngle) / (M_PI * 2) * (poloidalEndAngle - poloidalStartAngle) / (M_PI * 1); - //unsigned int pointCount = (unsigned int)(toroidalPrecision * ((M_PI - lighthouseAngle) * 2 / poloidalPrecision + 1) + 1); - // TODO: This calculation of the number of points that we will generate is excessively large (probably by about a factor of 2 or more) We can do better. - //float pointEstimate = (pointCount + 1000) * sizeof(Point) * 2 * M_PI / (toroidalEndAngle - toroidalStartAngle); - unsigned int pointCount = 0; + unsigned int pointCount = 0; - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + // This loop tries to (cheaply) figure out an upper bound on the number of points we'll have in our point cloud + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - double step_distance = 2 * M_PI / steps; + double step_distance = 2 * M_PI / steps; - pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); - } + pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); + } - *pointCloud = malloc(pointCount * sizeof(Point) ); + *pointCloud = malloc(pointCount * sizeof(Point) ); - assert(0 != *pointCloud); + assert(0 != *pointCloud); - (*pointCloud)[pointCount - 1].x = -1000; - (*pointCloud)[pointCount - 1].y = -1000; - (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. + (*pointCloud)[pointCount - 1].x = -1000; + (*pointCloud)[pointCount - 1].y = -1000; + (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. - size_t currentPoint = 0; + size_t currentPoint = 0; - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; + for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) + { + // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle + // We do this so our point cloud will have a more even distribution of points across the surface of the torus. + double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - double step_distance = 2 * M_PI / steps; + double step_distance = 2 * M_PI / steps; - //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) - for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) - { - if (currentPoint >= pointCount - 1) - { - int a = 0; - } - assert(currentPoint < pointCount - 1); - (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); - (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); - (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); - (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); + //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) + for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) + { + if (currentPoint >= pointCount - 1) + { + int a = 0; + } + assert(currentPoint < pointCount - 1); + (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); + (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); + (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); + (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); - // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up - // and hence we know that nothing with a negative z value is a possible lightouse location. - // Before this code can go live, we'll have to take the normals into account and remove this hack. - if ((*pointCloud)[currentPoint].z > 0) - { - currentPoint++; - } - } - } + // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up + // and hence we know that nothing with a negative z value is a possible lightouse location. + // Before this code can go live, we'll have to take the normals into account and remove this hack. + if ((*pointCloud)[currentPoint].z > 0) + { + currentPoint++; + } + } + } #ifdef TORI_DEBUG - printf("%d / %d\n", currentPoint, pointCount); + printf("%d / %d\n", currentPoint, pointCount); #endif - (*pointCloud)[currentPoint].x = -1000; - (*pointCloud)[currentPoint].y = -1000; - (*pointCloud)[currentPoint].z = -1000; + (*pointCloud)[currentPoint].x = -1000; + (*pointCloud)[currentPoint].y = -1000; + (*pointCloud)[currentPoint].z = -1000; } void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud) { - double centralAngleToIgnore = lighthouseAngle * 6; + double centralAngleToIgnore = lighthouseAngle * 6; - centralAngleToIgnore = 20.0 / 180.0 * M_PI; + centralAngleToIgnore = 20.0 / 180.0 * M_PI; - partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); + partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); - return; + return; } @@ -218,163 +214,163 @@ void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointClo // That way, the caller doesn't have to draw the entire torus in high resolution, just the part of the torus // that is most likely to contain the best solution. void estimateToroidalAndPoloidalAngleOfPoint( - Point torusP1, - Point torusP2, - double lighthouseAngle, - Point point, - double *toroidalAngle, - double *poloidalAngle) + Point torusP1, + Point torusP2, + double lighthouseAngle, + Point point, + double *toroidalAngle, + double *poloidalAngle) { - // this is the rotation matrix that shows how to rotate the torus from being in a simple "default" orientation - // into the coordinate system of the tracked object - Matrix3x3 rot = GetRotationMatrixForTorus(torusP1, torusP2); - - // 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. - rot = inverseM33(rot); - Point origin; - origin.x = 0; - origin.y = 0; - origin.z = 0; - - Point m = midpoint(torusP1, torusP2); - - // 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. - - *toroidalAngle = atan(pointF.y / pointF.x); - if (pointF.x < 0) - { - *toroidalAngle += M_PI; - } - - // 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(torusP1, torusP2); // we don't care about the coordinate system of these points because we're just getting distance. - double toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); - double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 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) - - *poloidalAngle = atan(pointH.z / pointH.x); - if (pointH.x < 0) - { - *poloidalAngle += M_PI; - } - - // 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; + // this is the rotation matrix that shows how to rotate the torus from being in a simple "default" orientation + // into the coordinate system of the tracked object + Matrix3x3 rot = GetRotationMatrixForTorus(torusP1, torusP2); + + // 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. + rot = inverseM33(rot); + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(torusP1, torusP2); + + // 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. + + *toroidalAngle = atan(pointF.y / pointF.x); + if (pointF.x < 0) + { + *toroidalAngle += M_PI; + } + + // 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(torusP1, torusP2); // we don't care about the coordinate system of these points because we're just getting distance. + double toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); + double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 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) + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + // 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; } double FindSmallestDistance(Point p, Point* cloud) { - Point *cp = cloud; - double smallestDistance = 10000000000000.0; - - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); - if (distance < smallestDistance) - { - smallestDistance = distance; - } - cp++; - } - smallestDistance = sqrt(smallestDistance); - return smallestDistance; + Point *cp = cloud; + double smallestDistance = 10000000000000.0; + + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); + if (distance < smallestDistance) + { + smallestDistance = distance; + } + cp++; + } + smallestDistance = sqrt(smallestDistance); + return smallestDistance; } // Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds. Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) { - Point bestMatch = { 0 }; - double bestDistance = 10000000000000.0; - Point *cp = masterCloud; - int point = 0; - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - point++; + Point bestMatch = { 0 }; + double bestDistance = 10000000000000.0; + Point *cp = masterCloud; + int point = 0; + while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) + { + point++; #ifdef TORI_DEBUG - if (point % 100 == 0) - { - printf("."); - } + if (point % 100 == 0) + { + printf("."); + } #endif - double currentDistance = 0; - for (int i = 0; i < numClouds; i++) - { - if (clouds[i] == masterCloud) - { - continue; - } - Point* cloud = clouds[i]; - currentDistance += FindSmallestDistance(*cp, cloud); - } - - if (currentDistance < bestDistance) - { - bestDistance = currentDistance; - bestMatch = *cp; - } - cp++; - } - - return bestMatch; + double currentDistance = 0; + for (int i = 0; i < numClouds; i++) + { + if (clouds[i] == masterCloud) + { + continue; + } + Point* cloud = clouds[i]; + currentDistance += FindSmallestDistance(*cp, cloud); + } + + if (currentDistance < bestDistance) + { + bestDistance = currentDistance; + bestMatch = *cp; + } + cp++; + } + + return bestMatch; } @@ -382,197 +378,215 @@ Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) typedef struct { - Point a; - Point b; - double angle; + Point a; + Point b; + double angle; } PointsAndAngle; double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) { - double angle = acos(cos(a->phi - b->phi)*cos(a->theta - b->theta)); - double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta)); + double angle = acos(cos(a->phi - b->phi)*cos(a->theta - b->theta)); + double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta)); - return angle; + return angle; } double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) { - double p = (a->phi - b->phi); - double d = (a->theta - b->theta); + double p = (a->phi - b->phi); + double d = (a->theta - b->theta); - double adjd = sin((a->phi + b->phi) / 2); - double adjP = sin((a->theta + b->theta) / 2); - double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); - return pythAngle; + double adjd = sin((a->phi + b->phi) / 2); + double adjP = sin((a->theta + b->theta) / 2); + double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; } Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { - PointsAndAngle pna[MAX_POINT_PAIRS]; - //Point lh = { 10, 0, 200 }; - - 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 = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); - - double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); - - //double tmp = angleFromPoints(pna[pnaCount].a, pna[pnaCount].b, lh); - - pnaCount++; - } - } - } - - //Point **pointCloud = malloc(sizeof(Point*)* pnaCount); - Point **pointCloud = malloc(sizeof(void*)* pnaCount); - - FILE *f = NULL; - if (doLogOutput) - { - f = fopen("pointcloud2.pcd", "wb"); - writePcdHeader(f); - writeAxes(f); - } - - for (unsigned int i = 0; i < pnaCount; i++) - { - torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); - if (doLogOutput) - { - writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]); - } - - } - - Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); - - if (doLogOutput) - { - markPointWithStar(f, bestMatchA, 0xFF0000); - } + PointsAndAngle pna[MAX_POINT_PAIRS]; + + 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 = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pnaCount++; + } + } + } + + Point **pointCloud = malloc(sizeof(void*)* pnaCount); + + FILE *f = NULL; + if (doLogOutput) + { + f = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(f); + writeAxes(f); + } + + for (unsigned int i = 0; i < pnaCount; i++) + { + torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); + if (doLogOutput) + { + writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]); + } + + } + + Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud[i]); + pointCloud[i] = NULL; + } + + if (doLogOutput) + { + markPointWithStar(f, bestMatchA, 0xFF0000); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); + printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); #endif - // Now, let's add an extra patch or torus near the point we just found. + // Now, let's add an extra patch or torus near the point we just found. + + double toroidalAngle = 0; + double poloidalAngle = 0; - double toroidalAngle = 0; - double poloidalAngle = 0; + Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); - Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchA, + &toroidalAngle, + &poloidalAngle); - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - pna[i].a, - pna[i].b, - pna[i].angle, - bestMatchA, - &toroidalAngle, - &poloidalAngle); + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.1, toroidalAngle + 0.1, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.2, toroidalAngle + 0.2, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); + if (doLogOutput) + { + writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]); + } - if (doLogOutput) - { - writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]); - } + } - } + Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); - Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); - if (doLogOutput) - { - markPointWithStar(f, bestMatchB, 0x00FF00); - } + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud2[i]); + pointCloud2[i] = NULL; + } + + if (doLogOutput) + { + markPointWithStar(f, bestMatchB, 0x00FF00); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); + printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); #endif - Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); + Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); + + for (unsigned int i = 0; i < pnaCount; i++) + { + estimateToroidalAndPoloidalAngleOfPoint( + pna[i].a, + pna[i].b, + pna[i].angle, + bestMatchB, + &toroidalAngle, + &poloidalAngle); + + partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i])); - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - pna[i].a, - pna[i].b, - pna[i].angle, - bestMatchB, - &toroidalAngle, - &poloidalAngle); + if (doLogOutput) + { + writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]); + } - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.05, poloidalAngle + 0.05, pna[i].angle, 3000, &(pointCloud3[i])); + } - if (doLogOutput) - { - writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]); - } + Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); - } + for (unsigned int i = 0; i < pnaCount; i++) + { + free(pointCloud3[i]); + pointCloud3[i] = NULL; + } - Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); - if (doLogOutput) - { - markPointWithStar(f, bestMatchC, 0xFFFFFF); - } + if (doLogOutput) + { + markPointWithStar(f, bestMatchC, 0xFFFFFF); + } #ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); + printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); #endif - if (doLogOutput) - { - updateHeader(f); - fclose(f); - } + if (doLogOutput) + { + updateHeader(f); + fclose(f); + } + + - return bestMatchC; + return bestMatchC; } static Point makeUnitPoint(Point *p) { - Point newP; - double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); - newP.x = p->x / r; - newP.y = p->y / r; - newP.z = p->z / r; + Point newP; + double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); + newP.x = p->x / r; + newP.y = p->y / r; + newP.z = p->z / r; - return newP; + return newP; } static double getPhi(Point p) { - // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); - // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); - double phi = atan(p.x / p.z); - return phi; + // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); + // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); + double phi = atan(p.x / p.z); + return phi; } static double getTheta(Point p) { - //double theta = atan(p.y / p.x); - double theta = atan(p.x / p.y); - return theta; + //double theta = atan(p.y / p.x); + double theta = atan(p.x / p.y); + return theta; } // subtraction static Point PointSub(Point a, Point b) { - Point newPoint; + Point newPoint; - newPoint.x = a.x - b.x; - newPoint.y = a.y - b.y; - newPoint.z = a.z - b.z; + newPoint.x = a.x - b.x; + newPoint.y = a.y - b.y; + newPoint.z = a.z - b.z; - return newPoint; + return newPoint; } -- cgit v1.2.3