diff --git a/Makefile b/Makefile index f20a4557..b66f535e 100644 --- a/Makefile +++ b/Makefile @@ -82,9 +82,9 @@ lint: test: $(BIN) $(BSC) $(TEST_BIN) $(TEST_BIN) - # bash ./test/scripts/pyramid-incorrect.sh - bash ./test/scripts/readme-examples-test.sh - bash ./test/scripts/random-crap.sh + # ./test/scripts/pyramid-incorrect.sh + ./test/scripts/readme-examples-test.sh + ./test/scripts/random-crap.sh $(TEST_BIN): $(TEST_OBJS) $(CXX) $(LDFLAGS) -o $(TEST_BIN) $(TEST_OBJS) $(LIBS) diff --git a/src/io.cpp b/src/io.cpp index a4a61a2e..ac8e3942 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -1712,13 +1712,13 @@ void PipelineComparison(const PipelineInputList &expected, } while (0) if (values.plotRawInput != "") { - LOST_PIPELINE_COMPARE(expected[0]->InputImage() && expected.size() == 1, + LOST_PIPELINE_COMPARE(expected.size() == 1 && expected[0]->InputImage(), "--plot-raw-input requires exactly 1 input image, but " + std::to_string(expected.size()) + " many were provided.", PipelineComparatorPlotRawInput, values.plotRawInput, true); } if (values.plotInput != "") { - LOST_PIPELINE_COMPARE(expected[0]->InputImage() && expected.size() == 1 && expected[0]->InputStars(), + LOST_PIPELINE_COMPARE(expected.size() == 1 && expected[0]->InputImage() && expected[0]->InputStars(), "--plot-input requires exactly 1 input image, and for centroids to be available on that input image. " + std::to_string(expected.size()) + " many input images were provided.", PipelineComparatorPlotInput, values.plotInput, true); } diff --git a/src/star-id-private.hpp b/src/star-id-private.hpp index c910f898..ae361870 100644 --- a/src/star-id-private.hpp +++ b/src/star-id-private.hpp @@ -7,6 +7,7 @@ #include #include +#include "star-utils.hpp" #include "star-id.hpp" #include "databases.hpp" @@ -59,6 +60,97 @@ int IdentifyRemainingStarsPairDistance(StarIdentifiers *, const Camera &, float tolerance); -} +// PYRAMID + +/** + * The best pyramid "starting from" a certain star. The "other" stars are ordered by their distance + * from the main star for this struct. + * + * "Distance" in this class is angular distance, as usual. + * + * If distancesSum is nonpositive, no suitable pyramid exists for this star. + */ +class BestPyramidAtStar { +public: + int16_t centroidIndices[4]; + + float distancesSum; + + BestPyramidAtStar(int16_t centroidIndex0, int16_t centroidIndex1, int16_t centroidIndex2, int16_t centroidIndex3, + float distancesSum) + : distancesSum(distancesSum) { + centroidIndices[0] = centroidIndex0; + centroidIndices[1] = centroidIndex1; + centroidIndices[2] = centroidIndex2; + centroidIndices[3] = centroidIndex3; + } + + // "no suitable pyramid" constructor + explicit BestPyramidAtStar(int16_t mainCentroidIndex) + : distancesSum(-1) { + centroidIndices[0] = mainCentroidIndex; + centroidIndices[1] = -1; + centroidIndices[2] = -1; + centroidIndices[3] = -1; + } + + bool operator<(const BestPyramidAtStar &other) const { + return distancesSum > 0 && distancesSum < other.distancesSum; + } + + bool isNull() const { + return distancesSum <= 0; + } +}; + +/** + * Keep finding the best pyramid to attempt to identify next. + * + * Rough strategy is to find the overall best pyramid first, then remove all the stars in that + * pyramid and find the next best one (the idea being that if identification failed for the first + * pyramid, there is likely a false star in it, so we want to try and identify different stars next + * for highest chance of success). + * + * Of course, it's possible that we'll exhaust all stars using this strategy and still won't have + * found a valid pyramid, even though valid pyramids totally exist. In that case, we'll just resort + * to doing something worse TODO. + */ +class PyramidIterator { +public: + /// Please ensure that `centroids` outlives the PyramidIterator! Also, minDistance and maxDistance are exact, we don't offset by tolerance, you probably want to do that! + PyramidIterator(const std::vector ¢roidSpatials, float minDistance, float maxDistance); + + /// Returns the next best pyramid, or a "no pyramid" pyramid. + BestPyramidAtStar Next(); + +private: + float minDistance, maxDistance; + // once length of this is less than 4, we switch to alternate strategy: + const std::vector &allCentroidSpatials; + std::vector untriedCentroidIndices; +}; + +enum class IdentifyPyramidResult { + /// Won't even attempt identification of the given pyramid, not possible to identify for some reason (eg inter-star distances out of k-vector range) + CannotPossiblyIdentify = -1, + NoMatch = 0, /// Did not find any match + MatchedUniquely = 1, /// Matched uniquely, this is the only "successful" result + MatchedAmbiguously = 2, /// Multiple matches, you shouldn't use any of them +}; + +/** + * Try to identify a pattern of four stars. + * + * Pass it four 3D spatials, and it will try to find 4 catalog stars which match. + * @param a,b,c,d Catalog indices of the identified stars, if uniquely matched. + * @return see IdentifyPyramidResult. You can also sorta treat it like a number, though: "how many matches" were there? + */ +IdentifyPyramidResult IdentifyPyramid(const PairDistanceKVectorDatabase &, + const Catalog &, + float tolerance, + const Vec3 &, const Vec3 &, const Vec3 &, const Vec3 &, + int *a, int *b, int *c, int *d); + +} // namespace lost #endif diff --git a/src/star-id.cpp b/src/star-id.cpp index 4e803d3c..c8f0e9f6 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -4,7 +4,8 @@ #include #include #include -#include +#include +#include #include "star-id.hpp" #include "star-id-private.hpp" @@ -255,8 +256,12 @@ std::vector ConsumeInvolvingIterator(PairDistanceInvolvingIterator it) * The resulting map is "symmetrical" in the sense that if a star B is in the map for star A, then * star A is also in the map for star B. */ -std::unordered_multimap PairDistanceQueryToMap(const int16_t *pairs, const int16_t *end) { - std::unordered_multimap result; +std::multimap PairDistanceQueryToMap(const int16_t *pairs, const int16_t *end) { +#if LOST_DEBUG > 3 + std::cerr << "Building map from " << (end-pairs)/2 << " pairs" << std::endl; +#endif + std::multimap result; + // result.reserve(end - pairs); // equals number of pairs times two, which is correct. for (const int16_t *p = pairs; p != end; p += 2) { result.emplace(p[0], p[1]); result.emplace(p[1], p[0]); @@ -567,200 +572,314 @@ int IdentifyRemainingStarsPairDistance(StarIdentifiers *identifiers, return numExtraIdentifiedStars; } -StarIdentifiers PyramidStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { - - StarIdentifiers identified; - MultiDatabase multiDatabase(database); - const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(PairDistanceKVectorDatabase::kMagicValue); - if (databaseBuffer == NULL || stars.size() < 4) { - std::cerr << "Not enough stars, or database missing." << std::endl; - return identified; +// allCentroidSpatials should be unit vectors +std::vector ComputeBestPyramids(const std::vector &allCentroidSpatials, + const std::vector ¢roidIndices, + float minDistance, + float maxDistance) { + assert(centroidIndices.size() >= 4); + assert(allCentroidSpatials.size() >= centroidIndices.size()); + assert(minDistance < maxDistance); + if (allCentroidSpatials.size() > 0) { + // the only way this can happen by chance is if it's right on the boresight. Some test will catch it :) + assert(abs(allCentroidSpatials[0].Magnitude() - 1) < 0.0001); } - PairDistanceKVectorDatabase vectorDatabase(databaseBuffer); - // smallest normal single-precision float is around 10^-38 so we should be all good. See - // Analytic_Star_Pattern_Probability on the HSL wiki for details. - float expectedMismatchesConstant = pow(numFalseStars, 4) * pow(tolerance, 5) / 2 / pow(M_PI, 2); + std::vector result; + result.reserve(centroidIndices.size()); + float maxCos = cos(minDistance); + float minCos = cos(maxDistance); + // Find the 3 closest centroids to this centroid + std::vector> cosines; // allocating outside of the loop avoids repeated heap allocation + cosines.reserve(centroidIndices.size()); + for (int i = 0; i < (int)centroidIndices.size(); i++) { + cosines.resize(0); + + // TODO: optimize this using a sorted centroids list + for (int j = 0; j < (int)centroidIndices.size(); j++) { + if (i == j) { + continue; + } + float curCos = allCentroidSpatials[centroidIndices[i]] * allCentroidSpatials[centroidIndices[j]]; + // float curDistance = (allCentroids[centroidIndices[i]].position - allCentroids[centroidIndices[j]].position).Magnitude(); + if (minCos <= curCos && curCos <= maxCos) { + assert(centroidIndices[i] != centroidIndices[j]); + // emplace the NEGATIVE cosine so that the sort will be smallest angle first. + assert(centroidIndices[i] != centroidIndices[j]); + cosines.emplace_back(-curCos, centroidIndices[j]); + } + } - // this iteration technique is described in the Pyramid paper. Briefly: i will always be the - // lowest index, then dj and dk are how many indexes ahead the j-th star is from the i-th, and - // k-th from the j-th. In addition, we here add some other numbers so that the pyramids are not - // weird lines in wide FOV images. TODO: Select the starting points to ensure that the first pyramids are all within measurement tolerance. - int numStars = (int)stars.size(); - // the idea is that the square root is about across the FOV horizontally - int across = floor(sqrt(numStars))*2; - int halfwayAcross = floor(sqrt(numStars)/2); - long totalIterations = 0; - - int jMax = numStars - 3; - for (int jIter = 0; jIter < jMax; jIter++) { - int dj = 1+(jIter+halfwayAcross)%jMax; - - int kMax = numStars-dj-2; - for (int kIter = 0; kIter < kMax; kIter++) { - int dk = 1+(kIter+across)%kMax; - - int rMax = numStars-dj-dk-1; - for (int rIter = 0; rIter < rMax; rIter++) { - int dr = 1+(rIter+halfwayAcross)%rMax; - - int iMax = numStars-dj-dk-dr-1; - for (int iIter = 0; iIter <= iMax; iIter++) { - int i = (iIter + iMax/2)%(iMax+1); // start near the center of the photo - - // identification failure due to cutoff - if (++totalIterations > cutoff) { - std::cerr << "Cutoff reached." << std::endl; - return identified; - } + if (cosines.size() < 3) { + // Not enough centroids to make a pyramid, add a "null" best pyramid + result.emplace_back(i); + continue; + } - int j = i+dj; - int k = j+dk; - int r = k+dr; + std::partial_sort(cosines.begin(), cosines.begin()+3, cosines.end()); // operator< is defined on pairs to sort by first element - assert(i != j && j != k && k != r && i != k && i != r && j != r); + // Compute the sum of the distances between the 3 closest centroids + float distancesSum = 0; + for (int j = 0; j < 3; j++) { + distancesSum += acos(-cosines[j].first); + } - // TODO: move this out of the loop? - Vec3 iSpatial = camera.CameraToSpatial(stars[i].position).Normalize(); - Vec3 jSpatial = camera.CameraToSpatial(stars[j].position).Normalize(); - Vec3 kSpatial = camera.CameraToSpatial(stars[k].position).Normalize(); + // Add the best pyramid starting from this centroid to the result + result.emplace_back(centroidIndices[i], cosines[0].second, cosines[1].second, cosines[2].second, distancesSum); + } + + return result; +} - float ijDist = AngleUnit(iSpatial, jSpatial); +// see star-id-private.hpp for docs on PyramidIterator +PyramidIterator::PyramidIterator(const std::vector ¢roidSpatials, float minDistance, float maxDistance) + : minDistance(minDistance), maxDistance(maxDistance), allCentroidSpatials(centroidSpatials) { + for (int i = 0; i < (int)centroidSpatials.size(); i++) { + untriedCentroidIndices.push_back(i); + } +} - float iSinInner = sin(Angle(jSpatial - iSpatial, kSpatial - iSpatial)); - float jSinInner = sin(Angle(iSpatial - jSpatial, kSpatial - jSpatial)); - float kSinInner = sin(Angle(iSpatial - kSpatial, jSpatial - kSpatial)); +BestPyramidAtStar PyramidIterator::Next() { + if (untriedCentroidIndices.size() < 4) { + return BestPyramidAtStar(-1); + } - // if we made it this far, all 6 angles are confirmed! Now check - // that this match would not often occur due to chance. - // See Analytic_Star_Pattern_Probability on the HSL wiki for details - float expectedMismatches = expectedMismatchesConstant - * sin(ijDist) - / kSinInner - / std::max(std::max(iSinInner, jSinInner), kSinInner); + // Find the best pyramid + std::vector bestPyramids = ComputeBestPyramids(allCentroidSpatials, untriedCentroidIndices, + minDistance, maxDistance); + assert(!bestPyramids.empty()); - if (expectedMismatches > maxMismatchProbability) { - std::cout << "skip: mismatch prob." << std::endl; - continue; - } + // Find the best pyramid + auto minIt = std::min_element(bestPyramids.begin(), bestPyramids.end()); + assert(minIt != bestPyramids.end()); + BestPyramidAtStar bestPyramid = *minIt; - Vec3 rSpatial = camera.CameraToSpatial(stars[r].position).Normalize(); - - // sign of determinant, to detect flipped patterns - bool spectralTorch = iSpatial.CrossProduct(jSpatial)*kSpatial > 0; - - float ikDist = AngleUnit(iSpatial, kSpatial); - float irDist = AngleUnit(iSpatial, rSpatial); - float jkDist = AngleUnit(jSpatial, kSpatial); - float jrDist = AngleUnit(jSpatial, rSpatial); - float krDist = AngleUnit(kSpatial, rSpatial); // TODO: we don't really need to - // check krDist, if k has been - // verified by i and j it's fine. - - // we check the distances with the extra tolerance requirement to ensure that - // there isn't some pyramid that's just outside the database's bounds, but - // within measurement tolerance of the observed pyramid, since that would - // possibly cause a non-unique pyramid to be identified as unique. -#define _CHECK_DISTANCE(_dist) if (_dist < vectorDatabase.MinDistance() + tolerance || _dist > vectorDatabase.MaxDistance() - tolerance) { continue; } - _CHECK_DISTANCE(ikDist); - _CHECK_DISTANCE(irDist); - _CHECK_DISTANCE(jkDist); - _CHECK_DISTANCE(jrDist); - _CHECK_DISTANCE(krDist); + if (bestPyramid.distancesSum < 0) { + // No suitable pyramid exists + return BestPyramidAtStar(-1); + } + + // Remove all the stars in the best pyramid from the list of untried stars + for (int i = 0; i < 4; i++) { + // Possible to optimize this using remove_if, or otherwise doing all the removal at once. + untriedCentroidIndices.erase(std::remove(untriedCentroidIndices.begin(), untriedCentroidIndices.end(), + bestPyramid.centroidIndices[i]), + untriedCentroidIndices.end()); + } + + return bestPyramid; +} + +IdentifyPyramidResult IdentifyPyramid(const PairDistanceKVectorDatabase &db, + const Catalog &catalog, + float tolerance, + const Vec3 &iSpatial, const Vec3 &jSpatial, const Vec3 &kSpatial, const Vec3 &rSpatial, + int *a, int *b, int *c, int *d) { + + // sign of determinant, to detect flipped patterns + bool spectralTorch = iSpatial.CrossProduct(jSpatial)*kSpatial > 0; + + float ijDist = AngleUnit(iSpatial, jSpatial); + float ikDist = AngleUnit(iSpatial, kSpatial); + float irDist = AngleUnit(iSpatial, rSpatial); + float jkDist = AngleUnit(jSpatial, kSpatial); + float jrDist = AngleUnit(jSpatial, rSpatial); + float krDist = AngleUnit(kSpatial, rSpatial); // TODO: we don't really need to + // check krDist, if k has been + // verified by i and j it's fine. + + // we check the distances with the extra tolerance requirement to ensure that + // there isn't some pyramid that's just outside the database's bounds, but + // within measurement tolerance of the observed pyramid, since that would + // possibly cause a non-unique pyramid to be identified as unique. +#define _CHECK_DISTANCE(_dist) if (_dist < db.MinDistance() + tolerance || _dist > db.MaxDistance() - tolerance) { std::cerr << _dist << std::endl; return IdentifyPyramidResult::CannotPossiblyIdentify; } + _CHECK_DISTANCE(ijDist); + _CHECK_DISTANCE(ikDist); + _CHECK_DISTANCE(irDist); #undef _CHECK_DISTANCE - const int16_t *ijEnd, *ikEnd, *irEnd; - const int16_t *const ijQuery = vectorDatabase.FindPairsLiberal(ijDist - tolerance, ijDist + tolerance, &ijEnd); - const int16_t *const ikQuery = vectorDatabase.FindPairsLiberal(ikDist - tolerance, ikDist + tolerance, &ikEnd); - const int16_t *const irQuery = vectorDatabase.FindPairsLiberal(irDist - tolerance, irDist + tolerance, &irEnd); - - std::unordered_multimap ikMap = PairDistanceQueryToMap(ikQuery, ikEnd); - std::unordered_multimap irMap = PairDistanceQueryToMap(irQuery, irEnd); - - int iMatch = -1, jMatch = -1, kMatch = -1, rMatch = -1; - for (const int16_t *iCandidateQuery = ijQuery; iCandidateQuery != ijEnd; iCandidateQuery++) { - int iCandidate = *iCandidateQuery; - // depending on parity, the first or second star in the pair is the "other" one - int jCandidate = (iCandidateQuery - ijQuery) % 2 == 0 - ? iCandidateQuery[1] - : iCandidateQuery[-1]; - - const Vec3 &iCandidateSpatial = catalog[iCandidate].spatial; - const Vec3 &jCandidateSpatial = catalog[jCandidate].spatial; - - Vec3 ijCandidateCross = iCandidateSpatial.CrossProduct(jCandidateSpatial); - - for (auto kCandidateIt = ikMap.equal_range(iCandidate); kCandidateIt.first != kCandidateIt.second; kCandidateIt.first++) { - // kCandidate.first is iterator, then ->second is the value (other star) - int kCandidate = kCandidateIt.first->second; - Vec3 kCandidateSpatial = catalog[kCandidate].spatial; - bool candidateSpectralTorch = ijCandidateCross*kCandidateSpatial > 0; - // checking the spectral-ity early to fail fast - if (candidateSpectralTorch != spectralTorch) { - continue; - } - - // small optimization: We can calculate jk before iterating through r, so we will! - float jkCandidateDist = AngleUnit(jCandidateSpatial, kCandidateSpatial); - if (jkCandidateDist < jkDist - tolerance || jkCandidateDist > jkDist + tolerance) { - continue; - } - - // TODO: if there are no jr matches, there's no reason to - // continue iterating through all the other k-s. Possibly - // enumarete all r matches, according to ir, before this loop - for (auto rCandidateIt = irMap.equal_range(iCandidate); rCandidateIt.first != rCandidateIt.second; rCandidateIt.first++) { - int rCandidate = rCandidateIt.first->second; - const Vec3 &rCandidateSpatial = catalog[rCandidate].spatial; - float jrCandidateDist = AngleUnit(jCandidateSpatial, rCandidateSpatial); - float krCandidateDist; - if (jrCandidateDist < jrDist - tolerance || jrCandidateDist > jrDist + tolerance) { - continue; - } - krCandidateDist = AngleUnit(kCandidateSpatial, rCandidateSpatial); - if (krCandidateDist < krDist - tolerance || krCandidateDist > krDist + tolerance) { - continue; - } - - // we have a match! - - if (iMatch == -1) { - iMatch = iCandidate; - jMatch = jCandidate; - kMatch = kCandidate; - rMatch = rCandidate; - } else { - // uh-oh, stinky! - // TODO: test duplicate detection, it's hard to cause it in the real catalog... - std::cerr << "Pyramid not unique, skipping..." << std::endl; - goto sensorContinue; - } - } - } +#if LOST_DEBUG > 2 + std::cerr << "Passed distance checks for pyramid" << std::endl; +#endif + const int16_t *ijEnd, *ikEnd, *irEnd; + const int16_t *const ijQuery = db.FindPairsExact(catalog, ijDist - tolerance, ijDist + tolerance, &ijEnd); + const int16_t *const ikQuery = db.FindPairsExact(catalog, ikDist - tolerance, ikDist + tolerance, &ikEnd); + const int16_t *const irQuery = db.FindPairsExact(catalog, irDist - tolerance, irDist + tolerance, &irEnd); - } + // TODO: theoretically, it's fastest to relabel j,k,r to put the shortest one first (probably). + // But since we're not even sure of that, we'll just leave them alone for now. - if (iMatch != -1) { - printf("Matched unique pyramid!\nExpected mismatches: %e\n", expectedMismatches); - identified.push_back(StarIdentifier(i, iMatch)); - identified.push_back(StarIdentifier(j, jMatch)); - identified.push_back(StarIdentifier(k, kMatch)); - identified.push_back(StarIdentifier(r, rMatch)); +#if LOST_DEBUG > 3 + std::cerr << "Number of ij pairs: " << (ijEnd-ijQuery)/2 << std::endl; +#endif + std::multimap ikMap = PairDistanceQueryToMap(ikQuery, ikEnd); + std::multimap irMap = PairDistanceQueryToMap(irQuery, irEnd); + + bool foundMatchYet = false; + for (const int16_t *iCandidateQuery = ijQuery; iCandidateQuery != ijEnd; iCandidateQuery++) { + int iCandidate = *iCandidateQuery; + // depending on parity, the first or second star in the pair is the "other" one + int jCandidate = (iCandidateQuery - ijQuery) % 2 == 0 + ? iCandidateQuery[1] + : iCandidateQuery[-1]; + + // if debug>4, print the candidates +#if LOST_DEBUG > 3 + std::cerr << "iCandidate: " << *iCandidateQuery << std::endl; + std::cerr << "jCandidate: " << jCandidate << std::endl; +#endif - int numAdditionallyIdentified = IdentifyRemainingStarsPairDistance(&identified, stars, vectorDatabase, catalog, camera, tolerance); - printf("Identified an additional %d stars.\n", numAdditionallyIdentified); - assert(numAdditionallyIdentified == (int)identified.size()-4); + const Vec3 &iCandidateSpatial = catalog[iCandidate].spatial; + const Vec3 &jCandidateSpatial = catalog[jCandidate].spatial; - return identified; - } + Vec3 ijCandidateCross = iCandidateSpatial.CrossProduct(jCandidateSpatial); + + for (auto kCandidateIt = ikMap.equal_range(iCandidate); kCandidateIt.first != kCandidateIt.second; kCandidateIt.first++) { + // kCandidate.first is iterator, then ->second is the value (other star) + int kCandidate = kCandidateIt.first->second; + Vec3 kCandidateSpatial = catalog[kCandidate].spatial; + bool candidateSpectralTorch = ijCandidateCross*kCandidateSpatial > 0; + // checking the spectral-ity early to fail fast + if (candidateSpectralTorch != spectralTorch) { +#if LOST_DEBUG > 3 + std::cerr << "skipping candidate " << iCandidate << " " << jCandidate << " " << kCandidate << " because spectral-ity mismatch" << std::endl; +#endif + continue; + } + + // small optimization: We can calculate jk before iterating through r, so we will! + float jkCandidateDist = AngleUnit(jCandidateSpatial, kCandidateSpatial); + if (jkCandidateDist < jkDist - tolerance || jkCandidateDist > jkDist + tolerance) { + continue; + } - sensorContinue:; + // TODO: if there are no jr matches, there's no reason to + // continue iterating through all the other k-s. Possibly + // enumarete all r matches, according to ir, before this loop + for (auto rCandidateIt = irMap.equal_range(iCandidate); rCandidateIt.first != rCandidateIt.second; rCandidateIt.first++) { + int rCandidate = rCandidateIt.first->second; + const Vec3 &rCandidateSpatial = catalog[rCandidate].spatial; + float jrCandidateDist = AngleUnit(jCandidateSpatial, rCandidateSpatial); + float krCandidateDist; + if (jrCandidateDist < jrDist - tolerance || jrCandidateDist > jrDist + tolerance) { + continue; + } + krCandidateDist = AngleUnit(kCandidateSpatial, rCandidateSpatial); + if (krCandidateDist < krDist - tolerance || krCandidateDist > krDist + tolerance) { + continue; + } + + // we have a match! + if (!foundMatchYet) { + *a = iCandidate; + *b = jCandidate; + *c = kCandidate; + *d = rCandidate; + foundMatchYet = true; + } else { + // not unique! + return IdentifyPyramidResult::MatchedAmbiguously; } } } } + return foundMatchYet + ? IdentifyPyramidResult::MatchedUniquely + : IdentifyPyramidResult::NoMatch; + +} + +StarIdentifiers PyramidStarIdAlgorithm::Go( + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + + StarIdentifiers identified; + MultiDatabase multiDatabase(database); + const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(PairDistanceKVectorDatabase::kMagicValue); + if (databaseBuffer == NULL || stars.size() < 4) { + std::cerr << "Not enough stars, or database missing." << std::endl; + return identified; + } + PairDistanceKVectorDatabase vectorDatabase(databaseBuffer); + std::vector starSpatials; + for (const Star &star : stars) { + starSpatials.push_back(camera.CameraToSpatial(star.position).Normalize()); + } + + // smallest normal single-precision float is around 10^-38 so we should be all good. See + // Analytic_Star_Pattern_Probability on the HSL wiki for details. + float expectedMismatchesConstant = pow(numFalseStars, 4) * pow(tolerance, 5) / 2 / pow(M_PI, 2); + + // keep iterating through pyramids + PyramidIterator pyramidIterator(starSpatials, + vectorDatabase.MinDistance() + tolerance, + vectorDatabase.MaxDistance() - tolerance); + BestPyramidAtStar bestPyramid(-1); + while (bestPyramid = pyramidIterator.Next(), !bestPyramid.isNull()) { + +#if LOST_DEBUG > 2 + // print distance sum + std::cerr << "Current pyramid distance sum: " << bestPyramid.distancesSum << std::endl; + // print out each centroid index + std::cerr << bestPyramid.centroidIndices[0] << " " << bestPyramid.centroidIndices[1] << " " << bestPyramid.centroidIndices[2] << " " << bestPyramid.centroidIndices[3] << std::endl; +#endif + int i = bestPyramid.centroidIndices[0], + j = bestPyramid.centroidIndices[1], + k = bestPyramid.centroidIndices[2], + r = bestPyramid.centroidIndices[3]; + + assert(i != j && j != k && k != r && i != k && i != r && j != r); + + // TODO: move this out of the loop? + Vec3 iSpatial = starSpatials[i]; + Vec3 jSpatial = starSpatials[j]; + Vec3 kSpatial = starSpatials[k]; + + float ijDist = AngleUnit(iSpatial, jSpatial); + + float iSinInner = sin(Angle(jSpatial - iSpatial, kSpatial - iSpatial)); + float jSinInner = sin(Angle(iSpatial - jSpatial, kSpatial - jSpatial)); + float kSinInner = sin(Angle(iSpatial - kSpatial, jSpatial - kSpatial)); + + // if we made it this far, all 6 angles are confirmed! Now check + // that this match would not often occur due to chance. + // See Analytic_Star_Pattern_Probability on the HSL wiki for details + float expectedMismatches = expectedMismatchesConstant + * sin(ijDist) + / kSinInner + / std::max(std::max(iSinInner, jSinInner), kSinInner); + + if (expectedMismatches > maxMismatchProbability) { + std::cout << "skip: mismatch prob." << std::endl; + continue; + } + + Vec3 rSpatial = starSpatials[r]; + + int iMatch, jMatch, kMatch, rMatch; + IdentifyPyramidResult identifyResult + = IdentifyPyramid(vectorDatabase, catalog, tolerance, + iSpatial, jSpatial, kSpatial, rSpatial, + &iMatch, &jMatch, &kMatch, &rMatch); + + switch (identifyResult) { + case IdentifyPyramidResult::CannotPossiblyIdentify: + case IdentifyPyramidResult::NoMatch: + case IdentifyPyramidResult::MatchedAmbiguously: + continue; + case IdentifyPyramidResult::MatchedUniquely: + identified.push_back(StarIdentifier(i, iMatch)); + identified.push_back(StarIdentifier(j, jMatch)); + identified.push_back(StarIdentifier(k, kMatch)); + identified.push_back(StarIdentifier(r, rMatch)); + + int numAdditionallyIdentified = IdentifyRemainingStarsPairDistance(&identified, stars, vectorDatabase, catalog, camera, tolerance); + printf("Identified an additional %d stars.\n", numAdditionallyIdentified); + assert(numAdditionallyIdentified == (int)identified.size()-4); + + return identified; + } + } std::cerr << "Tried all pyramids; none matched." << std::endl; return identified; diff --git a/test/fixtures.cpp b/test/fixtures.cpp index 8020c886..fca2b16d 100644 --- a/test/fixtures.cpp +++ b/test/fixtures.cpp @@ -94,4 +94,25 @@ StarIdentifiers elevenStarIds = { StarIdentifier(11, 11, 1), }; +// "clusters" is for normies +Stars starCloisters = { + // four stars all fairly near each other + // vary the distances a bit so that we get a deterministic ordering + Star(1,1,1), // 0 + Star(0,1,1), // 1 + Star(1.1,0,1), // 2 + Star(2.2,1,0), // 3 + + //four stars a bit further away from each other + Star(10,10,1), // 4 + Star(8,10,1), // 5 + Star(10.1,8,1), // 6 + Star(12.2,10,0), // 7 + + // and now a remaining cluster of three stars, which shouldn't be attempted after the first two, at least not without switching to an alternate strategy + Star(20,20,1), // 8 + Star(20,17,1), // 9 + Star(17,20,1), // 10 +}; + } diff --git a/test/fixtures.hpp b/test/fixtures.hpp index 77db386f..5657a796 100644 --- a/test/fixtures.hpp +++ b/test/fixtures.hpp @@ -19,6 +19,7 @@ extern Quaternion straightAhead; extern Stars elevenStars; extern StarIdentifiers elevenStarIds; +extern Stars starCloisters; } diff --git a/test/pyramid.cpp b/test/pyramid.cpp new file mode 100644 index 00000000..cef97a38 --- /dev/null +++ b/test/pyramid.cpp @@ -0,0 +1,128 @@ +#include +#include + +#include "fixtures.hpp" +#include "attitude-utils.hpp" +#include "io.hpp" +#include "star-id.hpp" +#include "star-id-private.hpp" + +#include "utils.hpp" + +using namespace lost; // NOLINT + +TEST_CASE("Never don't identify a pyramid", "[pyramid]") { + float minDistance = DegToRad(0.5); + float maxDistance = DegToRad(10.0); + float tolerance = DegToRad(0.05); + // What fraction of the pyramids must be /uniquely/ identified. The test always requires that at + // least one identification be made for each pyramid, but sometimes there are multiple. + float minFractionUniquelyIdentified = 0.75; + + long length; + const Catalog catalog = NarrowCatalog(CatalogRead(), 9999, 9999, DegToRad(0.5)); + unsigned char *dbBytes = BuildPairDistanceKVectorDatabase( + catalog, &length, minDistance, maxDistance, 10000); + PairDistanceKVectorDatabase db(dbBytes); + std::cerr << "done narrowing and building" << std::endl; + + // now the fun begins + int numPyramidsToTry = 20; + // how do we "randomly" pick first stars without repetition? Pretty easy: Pick a prime modulus + // >catalog.size(), then choose some other number, which you keep multiplying and modulo-ing, + // you know it will create every element mod p before it repeats. + int modulus = 9103; // is prime + int multiplier1 = 737; + int multiplier2 = 8822; + int numPyramidsTried = 0; + int numUniquelyIdentified = 0; + for (int i = 1; numPyramidsTried < numPyramidsToTry; i++) { + int startIndex = i * multiplier1 % modulus; + // just keep sampling until we're inside the catalog + if (startIndex > (int)catalog.size()) { + continue; + } + numPyramidsTried++; + + // find other stars, creating a set of stars that are all within range. + // This loop could be sped up substantially by sorting on x-value, but whatever, it's just a test! + std::vector catalogIndices{startIndex}; + for (int j = 1; catalogIndices.size() < 4; j++) { + // There should always be three other stars within 20 degrees! + REQUIRE(j <= (int)catalog.size()); + + int otherIndex = j * multiplier2 % modulus; + if (otherIndex > (int)catalog.size()) { + continue; + } + for (int alreadyChosenIndex : catalogIndices) { + float dist = AngleUnit(catalog[alreadyChosenIndex].spatial, catalog[otherIndex].spatial); + if (dist <= minDistance+tolerance || dist >= maxDistance-tolerance) { + goto nextOtherIndex; + } + } + // made it through the gauntlet! + catalogIndices.push_back(otherIndex); + + nextOtherIndex:; + } + + int a, b, c, d; + IdentifyPyramidResult matchResult + = IdentifyPyramid(db, + catalog, + tolerance, + catalog[catalogIndices[0]].spatial, + catalog[catalogIndices[1]].spatial, + catalog[catalogIndices[2]].spatial, + catalog[catalogIndices[3]].spatial, + &a, &b, &c, &d); + CHECK((matchResult == IdentifyPyramidResult::MatchedUniquely + || matchResult == IdentifyPyramidResult::MatchedAmbiguously)); + if (matchResult == IdentifyPyramidResult::MatchedUniquely) { + numUniquelyIdentified++; + CHECK(a == catalogIndices[0]); + CHECK(b == catalogIndices[1]); + CHECK(c == catalogIndices[2]); + CHECK(d == catalogIndices[3]); + } + } + + CHECK((float)numUniquelyIdentified / numPyramidsToTry >= minFractionUniquelyIdentified); + delete[] dbBytes; +} + +// TODO: one where spectrality is nearly zero, and thus needs to be ignored. Might be tested by above test already, but unsure. + +TEST_CASE("Pyramid selection: Basic strategy only on starCloisters", "[pyramid] [fast]") { + std::vector starCloistersSpatials; + for (Star star : starCloisters) { + starCloistersSpatials.push_back(smolCamera.CameraToSpatial(star.position).Normalize()); + } + + PyramidIterator pyIter(starCloistersSpatials, 0.0, 100.0); + BestPyramidAtStar py1 = pyIter.Next(); + BestPyramidAtStar py2 = pyIter.Next(); + BestPyramidAtStar py3 = pyIter.Next(); + // CHECK(py1.distancesSum == Approx(3).margin(0.3)); // they're not even close, need to way up the margin + // CHECK(py2.distancesSum == Approx(6).margin(0.3)); + CHECK(py1.distancesSum < py2.distancesSum); + // iteration stopped: + CHECK(py3.distancesSum < 0); + + // of course, you could use a loop here...but it's way easier to read unrolled, and I have the unlimited power of Github copilot! + CHECK(py1.centroidIndices[0] == 0); + CHECK(py1.centroidIndices[1] == 1); + CHECK(py1.centroidIndices[2] == 2); + CHECK(py1.centroidIndices[3] == 3); + + CHECK(py2.centroidIndices[0] == 4); + CHECK(py2.centroidIndices[1] == 5); + CHECK(py2.centroidIndices[2] == 6); + CHECK(py2.centroidIndices[3] == 7); +} + +// TODO: +// TEST_CASE("Pyramids that exist and are unique are always matched immediately", "[pyramid] [fast]") { + +// } diff --git a/test/scripts/readme-examples-test.sh b/test/scripts/readme-examples-test.sh old mode 100644 new mode 100755