From 89d088380f6effbf0e2d1e8e532a967278ac40f6 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 27 Jun 2022 10:23:35 -0700 Subject: [PATCH 01/54] backbone of the tracking mode database wrote sort function can be made into database not entirely sure the build database stage is correct though (in io.cpp) --- src/databases.cpp | 20 ++++++++++++++++++++ src/databases.hpp | 14 ++++++++++++++ src/io.cpp | 10 ++++++++++ 3 files changed, 44 insertions(+) diff --git a/src/databases.cpp b/src/databases.cpp index 4cab6eae..ad31219c 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -314,6 +314,26 @@ MultiDatabaseBuilder::~MultiDatabaseBuilder() { free(buffer); } + +/*** for tracking mode ***/ + +long SerializeLengthTrackingCatalog(const Catalog &catalog) { + return catalog.size() * sizeof(CatalogStar); +} + + +bool CompareCatalogStars(const CatalogStar &s1, const CatalogStar &s2) { + return s1.spatial.x < s2.spatial.y; +} + +// sort by x coordinate of stars +std::vector SortTrackingCatalog(const Catalog &catalog) { + std::sort(catalog.begin(), catalog.end(), CompareCatalogStars); + return catalog; +} + + + } // TODO: after creating the database, print more statistics, such as average number of pairs per diff --git a/src/databases.hpp b/src/databases.hpp index 75a7ac19..9da37e9d 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -82,6 +82,20 @@ class TripleInnerKVectorDatabase { int16_t *triples; }; + + +// tracking mode database (basically stars are sorted by vector spatial x coord) +class TrackingSortedDatabase { +public: + TrackingSortedDatabase(const unsigned char *databaseBytes); + + const static int32_t magicValue = 0x2536f0A9; +private: +}; + +long SerializeLengthTrackingCatalog(const Catalog &catalog); +std::vector SortTrackingCatalog(const Catalog &catalog); + // maximum number of databases in a MultiDatabase const int kMultiDatabaseMaxDatabases = 64; const long kMultiDatabaseTocLength = 8*kMultiDatabaseMaxDatabases; diff --git a/src/io.cpp b/src/io.cpp index 7ff4f06b..74df4544 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -249,6 +249,16 @@ void BuildKVectorDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog, } +void BuildTrackingDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog) { + long length = SerializeLengthTrackingCatalog(catalog); + unsigned char *buffer = builder.AddSubDatabase(TrackingSortedDatabase::magicValue, length); + if (buffer == NULL) { + std::cerr << "No room for another database." << std::endl; + } + // TODO fix this line, because it returns something and I don't think it's quite analogous to the SerializePairDistanceKVector above + SortTrackingCatalog(catalog); +} + void GenerateDatabases(MultiDatabaseBuilder &builder, const Catalog &catalog, const DatabaseOptions &values) { From e1977a1e6156bc79fee9a51f4450b90256a54659 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 27 Jun 2022 10:43:19 -0700 Subject: [PATCH 02/54] add tracking mode to CLI --- documentation/database.man | 4 ++++ src/database-options.hpp | 1 + src/io.cpp | 2 ++ 3 files changed, 7 insertions(+) diff --git a/documentation/database.man b/documentation/database.man index f9183990..ce560a2b 100644 --- a/documentation/database.man +++ b/documentation/database.man @@ -41,6 +41,10 @@ Sets the maximum distance bin in the kvector building method to \fImax\fP (degre \fB--kvector-distance-bins\fP \fInum-bins\fP Sets the number of distance bins in the kvector building method to \fInum-bins\fP. Defaults to 10000 if option is not selected. +.TP +\fB--tracking\fP +Generate a Tracking mode database + .TP \fB--output\fP \fIoutput-path\fP The file to output the database to. Defaults to stdout. diff --git a/src/database-options.hpp b/src/database-options.hpp index f71f2030..922b72ca 100644 --- a/src/database-options.hpp +++ b/src/database-options.hpp @@ -6,4 +6,5 @@ LOST_CLI_OPTION("kvector" , bool , kvector , fa LOST_CLI_OPTION("kvector-min-distance" , float , kvectorMinDistance , 0.5 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("kvector-max-distance" , float , kvectorMaxDistance , 15 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("kvector-distance-bins", long , kvectorNumDistanceBins, 10000 , atol(optarg) , kNoDefaultArgument) +LOST_CLI_OPTION("tracking" , bool , tracking , false , atobool(optarg), true) LOST_CLI_OPTION("output" , std::string, outputPath , "-" , optarg , kNoDefaultArgument) diff --git a/src/io.cpp b/src/io.cpp index 74df4544..f08c2a03 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -267,6 +267,8 @@ void GenerateDatabases(MultiDatabaseBuilder &builder, const Catalog &catalog, co float maxDistance = DegToRad(values.kvectorMaxDistance); long numBins = values.kvectorNumDistanceBins; BuildKVectorDatabase(builder, catalog, minDistance, maxDistance, numBins); + } else if (values.tracking) { + BuildTrackingDatabase(builder, catalog); } else { std::cerr << "No database builder selected -- no database generated." << std::endl; exit(1); From 74560ae3c81255cebf8085a6137fb8c1d44ee6b3 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 27 Jun 2022 10:49:23 -0700 Subject: [PATCH 03/54] fix bug in sort --- src/databases.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index ad31219c..bd7d9775 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -327,13 +327,12 @@ bool CompareCatalogStars(const CatalogStar &s1, const CatalogStar &s2) { } // sort by x coordinate of stars -std::vector SortTrackingCatalog(const Catalog &catalog) { - std::sort(catalog.begin(), catalog.end(), CompareCatalogStars); - return catalog; +Catalog SortTrackingCatalog(const Catalog &catalog) { + std::vector stars = catalog; + std::sort(stars.begin(), stars.end(), CompareCatalogStars); + return stars; } - - } // TODO: after creating the database, print more statistics, such as average number of pairs per From 52e04c0a93e95c663aa02c6fa11dd2a368a53853 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 27 Jun 2022 10:59:22 -0700 Subject: [PATCH 04/54] fixed segfault with sorting stars --- src/databases.cpp | 2 +- src/star-utils.hpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/databases.cpp b/src/databases.cpp index bd7d9775..383ee672 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -329,7 +329,7 @@ bool CompareCatalogStars(const CatalogStar &s1, const CatalogStar &s2) { // sort by x coordinate of stars Catalog SortTrackingCatalog(const Catalog &catalog) { std::vector stars = catalog; - std::sort(stars.begin(), stars.end(), CompareCatalogStars); + std::sort(stars.begin(), stars.end()); return stars; } diff --git a/src/star-utils.hpp b/src/star-utils.hpp index 4b5d5214..e4faa5e7 100644 --- a/src/star-utils.hpp +++ b/src/star-utils.hpp @@ -18,6 +18,11 @@ class CatalogStar { Vec3 spatial; int magnitude; // *10^-2 int name; + + // for sorting + bool operator < (const CatalogStar& s1) { + return spatial.x < s1.spatial.x; + } }; class Star { From 549f32cb05372f735eefec19aadbe6543e1e298c Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 29 Jun 2022 17:12:14 -0700 Subject: [PATCH 05/54] serializes sorted indices of stars --- src/databases.cpp | 39 +++++++++++++++++++++++++++++++-------- src/databases.hpp | 4 +--- src/io.cpp | 3 +-- src/star-utils.hpp | 5 ----- 4 files changed, 33 insertions(+), 18 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 383ee672..8baa237d 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -317,20 +317,43 @@ MultiDatabaseBuilder::~MultiDatabaseBuilder() { /*** for tracking mode ***/ +struct TrackingStar { + int16_t index; + CatalogStar star; +}; + long SerializeLengthTrackingCatalog(const Catalog &catalog) { - return catalog.size() * sizeof(CatalogStar); + return catalog.size() * sizeof(int16_t); } - -bool CompareCatalogStars(const CatalogStar &s1, const CatalogStar &s2) { - return s1.spatial.x < s2.spatial.y; +bool CompareTrackingStars(const TrackingStar &s1, const TrackingStar &s2) { + return s1.star.spatial.x < s2.star.spatial.x; } // sort by x coordinate of stars -Catalog SortTrackingCatalog(const Catalog &catalog) { - std::vector stars = catalog; - std::sort(stars.begin(), stars.end()); - return stars; +void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { + std::vector stars; + + for (long unsigned int i = 0; i < catalog.size(); i++) { + TrackingStar s; + s.index = i; + s.star = catalog.at(i); + stars.push_back(s); + } + + std::sort(stars.begin(), stars.end(), CompareTrackingStars); + + // store the sorted list of indices into the buffer + unsigned char *bufferStart = buffer; + + // bulk pairs field + for (const TrackingStar &star : stars) { + *(int16_t *)buffer = star.index; + buffer += sizeof(int16_t); + } + + // verify length + assert(buffer - bufferStart == SerializeLengthTrackingCatalog(catalog)); } } diff --git a/src/databases.hpp b/src/databases.hpp index 9da37e9d..2010117a 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -88,13 +88,11 @@ class TripleInnerKVectorDatabase { class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); - const static int32_t magicValue = 0x2536f0A9; -private: }; long SerializeLengthTrackingCatalog(const Catalog &catalog); -std::vector SortTrackingCatalog(const Catalog &catalog); +void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer); // maximum number of databases in a MultiDatabase const int kMultiDatabaseMaxDatabases = 64; diff --git a/src/io.cpp b/src/io.cpp index f08c2a03..56be1dfa 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -255,8 +255,7 @@ void BuildTrackingDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog if (buffer == NULL) { std::cerr << "No room for another database." << std::endl; } - // TODO fix this line, because it returns something and I don't think it's quite analogous to the SerializePairDistanceKVector above - SortTrackingCatalog(catalog); + SerializeTrackingCatalog(catalog, buffer); } diff --git a/src/star-utils.hpp b/src/star-utils.hpp index e4faa5e7..4b5d5214 100644 --- a/src/star-utils.hpp +++ b/src/star-utils.hpp @@ -18,11 +18,6 @@ class CatalogStar { Vec3 spatial; int magnitude; // *10^-2 int name; - - // for sorting - bool operator < (const CatalogStar& s1) { - return spatial.x < s1.spatial.x; - } }; class Star { From 04ebb59a8c0854d7f82e3d8ec99b6c0270f56f70 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 1 Jul 2022 18:40:44 -0700 Subject: [PATCH 06/54] added constructor for TrackingSortedDatabase --- src/databases.cpp | 24 +++++++++++++++++++++--- src/databases.hpp | 4 ++++ 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 8baa237d..57044c45 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -323,7 +323,7 @@ struct TrackingStar { }; long SerializeLengthTrackingCatalog(const Catalog &catalog) { - return catalog.size() * sizeof(int16_t); + return (catalog.size()+1) * sizeof(int16_t); } bool CompareTrackingStars(const TrackingStar &s1, const TrackingStar &s2) { @@ -343,10 +343,14 @@ void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { std::sort(stars.begin(), stars.end(), CompareTrackingStars); - // store the sorted list of indices into the buffer + // serialize into buffer unsigned char *bufferStart = buffer; - // bulk pairs field + // store length of catalog into the buffer + *(int16_t *)buffer = catalog.size(); + buffer += sizeof(int16_t); + + // store the sorted list of indices into the buffer for (const TrackingStar &star : stars) { *(int16_t *)buffer = star.index; buffer += sizeof(int16_t); @@ -356,6 +360,20 @@ void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { assert(buffer - bufferStart == SerializeLengthTrackingCatalog(catalog)); } + +TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { + + length = *(int16_t *)buffer; + buffer += sizeof(int16_t); + + for (int i = 0; i < length; i++) { + int16_t index = *(int16_t *)buffer; + buffer += sizeof(int16_t); + indices.push_back(index); + } +} + + } // TODO: after creating the database, print more statistics, such as average number of pairs per diff --git a/src/databases.hpp b/src/databases.hpp index 2010117a..512ad95d 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,6 +89,10 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t magicValue = 0x2536f0A9; + +private: + int16_t length; // length of catalog + std::vector indices; // sorted list of catalog indices }; long SerializeLengthTrackingCatalog(const Catalog &catalog); From 8700009f3de7505f0e151275167125cd8682f8c9 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 6 Jul 2022 12:35:00 -0700 Subject: [PATCH 07/54] added query function --- src/databases.cpp | 17 +++++++++++++++++ src/databases.hpp | 1 + 2 files changed, 18 insertions(+) diff --git a/src/databases.cpp b/src/databases.cpp index 57044c45..92f5c0a8 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -373,6 +373,23 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } } +int16_t square(int16_t base) { + return base * base; +} + +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius) { + std::vector query_ind; + + for (int i = 0; i < indices.size(); i++) { + CatalogStar s = c[i]; + if (square(s.spatial.x - point.x) + square(s.spatial.y - point.y) + square(s.spatial.z - point.z) <= square(radius)) { + query_ind.push_back(i); + } + } + + return query_ind; +} + } diff --git a/src/databases.hpp b/src/databases.hpp index 512ad95d..e9cf4689 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,6 +89,7 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t magicValue = 0x2536f0A9; + std::vector QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius); private: int16_t length; // length of catalog From e6aa00bc6294b4bc442e676c2d91283a7737a902 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 6 Jul 2022 14:38:59 -0700 Subject: [PATCH 08/54] do math with full vector instead of its components --- src/databases.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/databases.cpp b/src/databases.cpp index 92f5c0a8..ae8b9858 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -382,7 +382,8 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, for (int i = 0; i < indices.size(); i++) { CatalogStar s = c[i]; - if (square(s.spatial.x - point.x) + square(s.spatial.y - point.y) + square(s.spatial.z - point.z) <= square(radius)) { + Vec3 diff = s.spatial - point; + if (diff.MagnitudeSq() <= square(radius)) { query_ind.push_back(i); } } From 6f44b408cd748bf680dd2a23f5516f51c83c4792 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 6 Jul 2022 18:08:48 -0700 Subject: [PATCH 09/54] got rid of warning --- src/databases.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/databases.cpp b/src/databases.cpp index ae8b9858..fef701b2 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -380,7 +380,7 @@ int16_t square(int16_t base) { std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius) { std::vector query_ind; - for (int i = 0; i < indices.size(); i++) { + for (long unsigned int i = 0; i < indices.size(); i++) { CatalogStar s = c[i]; Vec3 diff = s.spatial - point; if (diff.MagnitudeSq() <= square(radius)) { From d0624592c01d9df42b76025189a85e5d601cd020 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 29 Jul 2022 13:31:37 -0700 Subject: [PATCH 10/54] template for tracking mode added --- src/star-id.cpp | 16 ++++++++++++++++ src/star-id.hpp | 10 +++++++++- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 9112cbd3..6d1e9771 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -503,4 +503,20 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( return identified; } +StarIdentifiers TrackingModeStarIdAlgorithm::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(TrackingSortedDatabase::magicValue); + if (databaseBuffer == NULL) { + return identified; + } + TrackingSortedDatabase vectorDatabase(databaseBuffer); + + +} + + + } diff --git a/src/star-id.hpp b/src/star-id.hpp index f080d00e..d91dcae6 100644 --- a/src/star-id.hpp +++ b/src/star-id.hpp @@ -29,7 +29,6 @@ class GeometricVotingStarIdAlgorithm : public StarIdAlgorithm { float tolerance; }; - class PyramidStarIdAlgorithm final : public StarIdAlgorithm { public: StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; @@ -51,6 +50,15 @@ class PyramidStarIdAlgorithm final : public StarIdAlgorithm { long cutoff; }; + +class TrackingModeStarIdAlgorithm final : public StarIdAlgorithm { +public: + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; +}; + + + + } #endif From 4ad6bdda4638d4686cd68c470e242692d72400b7 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 29 Jul 2022 13:39:25 -0700 Subject: [PATCH 11/54] removed unnecessary function --- src/databases.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index fef701b2..c7c1665f 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -373,17 +373,13 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } } -int16_t square(int16_t base) { - return base * base; -} - std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius) { std::vector query_ind; for (long unsigned int i = 0; i < indices.size(); i++) { CatalogStar s = c[i]; Vec3 diff = s.spatial - point; - if (diff.MagnitudeSq() <= square(radius)) { + if (diff.MagnitudeSq() <= pow(radius,2)) { query_ind.push_back(i); } } From 3f24f6136b2fe72e0b3735c20eb329085bb401c1 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 3 Aug 2022 19:29:17 -0700 Subject: [PATCH 12/54] starting to write tracking mode algo (initial attempt) --- src/star-id.cpp | 71 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/src/star-id.cpp b/src/star-id.cpp index 6d1e9771..e4b0246e 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -515,6 +515,77 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( TrackingSortedDatabase vectorDatabase(databaseBuffer); + // TODO figure out how users can get + float radius; + float ra; + float dec; + float roll; + + + for (int i = 0; i < (int)stars.size(); i++) { + + // TODO figure out math for how to get the current position in degrees based on prev attitude + Vec3 position; + + std::vector possiblePrevStarInd = vectorDatabase.QueryNearestStars(catalog, position, radius); + + + + std::vector votes(catalog.size(), 0); + Vec2 c; + c.x = 0; + c.y = 0; + Vec3 center = camera.CameraToSpatial(c).Normalize(); + Vec3 iSpatial = camera.CameraToSpatial(stars[i].position).Normalize(); + float distance = Distance(center, iSpatial); + + + + + // for (int j = 0; j < (int)stars.size(); j++) { + // if (i != j) { + // // TODO: find a faster way to do this: + // std::vector votedInPair(catalog.size(), false); + // Vec3 jSpatial = camera.CameraToSpatial(stars[j].position).Normalize(); + // float greatCircleDistance = AngleUnit(iSpatial, jSpatial); + // //give a greater range for min-max Query for bigger radius (GreatCircleDistance) + // float lowerBoundRange = greatCircleDistance - tolerance; + // float upperBoundRange = greatCircleDistance + tolerance; + // const int16_t *upperBoundSearch; + // const int16_t *lowerBoundSearch = vectorDatabase.FindPairsLiberal( + // lowerBoundRange, upperBoundRange, &upperBoundSearch); + // //loop from lowerBoundSearch till numReturnedPairs, add one vote to each star in the pairs in the datastructure + // for (const int16_t *k = lowerBoundSearch; k != upperBoundSearch; k++) { + // if ((k - lowerBoundSearch) % 2 == 0) { + // float actualAngle = AngleUnit(catalog[*k].spatial, catalog[*(k+1)].spatial); + // assert(actualAngle <= greatCircleDistance + tolerance * 2); + // assert(actualAngle >= greatCircleDistance - tolerance * 2); + // } + // if (!votedInPair[*k] || true) { + // votes[*k]++; + // votedInPair[*k] = true; + // } + // } + // // US voting system + // } + // } + // // Find star w most votes + // int16_t maxVotes = votes[0]; + // int indexOfMax = 0; + // for (int v = 1; v < (int)votes.size(); v++) { + // if (votes[v] > maxVotes) { + // maxVotes = votes[v]; + // indexOfMax = v; + // } + // } + // StarIdentifier newStar(i, indexOfMax); + // // Set identified[i] to value of catalog index of star w most votesr + // identified.push_back(newStar); + } + + + + } From 052300aee81310afa76d9807f0b21a0c8d821267 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 3 Aug 2022 19:29:48 -0700 Subject: [PATCH 13/54] fix int16_t radius to float --- src/databases.cpp | 2 +- src/databases.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index c7c1665f..2fd75d79 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -373,7 +373,7 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } } -std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius) { +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, float radius) { std::vector query_ind; for (long unsigned int i = 0; i < indices.size(); i++) { diff --git a/src/databases.hpp b/src/databases.hpp index e9cf4689..0d827bca 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,7 +89,7 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t magicValue = 0x2536f0A9; - std::vector QueryNearestStars(const Catalog c, const Vec3 point, int16_t radius); + std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius); private: int16_t length; // length of catalog From 0cf4f541234c04dfa6ec42152aa3b921edba21b9 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 3 Aug 2022 19:30:39 -0700 Subject: [PATCH 14/54] rewriting query to make use of sorted property binary search algorithm --- src/databases.cpp | 49 ++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 44 insertions(+), 5 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 2fd75d79..52ca2003 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -376,18 +376,57 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, float radius) { std::vector query_ind; - for (long unsigned int i = 0; i < indices.size(); i++) { - CatalogStar s = c[i]; + float radiusSq = pow(radius,2); + + // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) + int left = 0; + int right = indices.size()-1; + int index = -1; + + while (left <= right) { + int mid = left + (right - left) / 2; + CatalogStar s = c[mid]; Vec3 diff = s.spatial - point; - if (diff.MagnitudeSq() <= pow(radius,2)) { - query_ind.push_back(i); + + if (diff.MagnitudeSq() <= radiusSq) { + index = mid; + } else if (s.spatial.x < point.x) { + left += mid; + } else { + right = mid - 1; } } + left = index; + right = index; + + // see how far left you can go + CatalogStar sLeft = c[left]; + Vec3 diffLeft = sLeft.spatial - point; + while (left > 0 && sLeft.spatial.x <= point.x - radius) { + if (diffLeft.MagnitudeSq() <= radiusSq) { + query_ind.push_back(left); + } + left--; + sLeft = c[left]; + diffLeft = sLeft.spatial - point; + } + + + // see how far right you can go + CatalogStar sRight = c[right]; + Vec3 diffRight = sRight.spatial - point; + while (right > 0 && sRight.spatial.x <= point.x - radius) { + if (diffRight.MagnitudeSq() <= radiusSq) { + query_ind.push_back(right); + } + right--; + sRight = c[right]; + diffLeft = sRight.spatial - point; + } return query_ind; } - } // TODO: after creating the database, print more statistics, such as average number of pairs per From e0abceef521863b22e0041d85b74146c3bfcc12f Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 8 Aug 2022 17:03:03 -0700 Subject: [PATCH 15/54] adding files to gitignore for convenience --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index f1b57a54..a7e084c6 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,8 @@ *.o *.d *.png +*.dat +*.txt /documentation/man-*.h From 4d1177698794d84164604d532ada1adaf2c62789 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 11 Aug 2022 21:50:48 -0700 Subject: [PATCH 16/54] add previous attitude class --- src/star-utils.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/star-utils.hpp b/src/star-utils.hpp index 4b5d5214..587dfe71 100644 --- a/src/star-utils.hpp +++ b/src/star-utils.hpp @@ -60,6 +60,16 @@ const CatalogStar *findNamedStar(const Catalog &, int name); // (so 523 = 5.23) Catalog NarrowCatalog(const Catalog &, int maxMagnitude, int maxStars); +// for tracking mode +class PrevAttitude { +public: + PrevAttitude(Attitude prev, float uncertainty) + : prev(prev), uncertainty(uncertainty) { }; + + Attitude prev; + float uncertainty; +}; + } #endif From 99e9c1d9c493c4cc8d714a5913c89860e2679dce Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 11 Aug 2022 22:57:13 -0700 Subject: [PATCH 17/54] add prevAttitude arg to all starid algos, set its value with command line --- documentation/pipeline.man | 10 +++++++++- src/io.cpp | 21 +++++++++++++++++++-- src/io.hpp | 1 + src/pipeline-options.hpp | 2 ++ src/star-id.cpp | 8 ++++---- src/star-id.hpp | 10 +++++----- src/star-utils.hpp | 2 ++ 7 files changed, 42 insertions(+), 12 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index 63697aa3..ce9091b1 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -11,7 +11,7 @@ pipeline \- builds a pipeline of the star identification process .br \fBpipeline\fP --generate[=\fInum-images\fP] [--horizontal-res \fIres\fP] [--vertical-res \fIres\fP] [--horizontal-fov \fIdegrees\fP] [--ref-brightness \fImag\fP] [--spread-stddev \fIstddev\fP] [--noise-stddev \fIstddev\fP] [--boresight-right-asc \fIascension\fP] [--boresight-dec \fIdeclination\fP] [--boresight-roll \fIroll\fP] [--centroid-algo \fIalgorithm\fP [--centroid-dummy-stars \fInum-stars\fP]] [--centroid-mag-filter \fImin-mag\fP] -[--database \fIfilename\fP] [--star-id-algo \fIalgo\fP (--gv-tolerance \fIdegrees\fP | --py-tolerance \fIdegrees\fP --false-stars \fInum\fP --max-mismatch-prob \fIprobability\fP)] [--attitude-algo \fIalgo\fP] [--plot \fIoutput-path\fB] +[--database \fIfilename\fP] [--star-id-algo \fIalgo\fP (--gv-tolerance \fIdegrees\fP | --py-tolerance \fIdegrees\fP --false-stars \fInum\fP --max-mismatch-prob \fIprobability\fP | --prev-attitude \fIattitude\fP --uncertainty \fIradius\fP)] [--attitude-algo \fIalgo\fP] [--plot \fIoutput-path\fB] .SH DESCRIPTION @@ -69,6 +69,14 @@ used in some star id algorithms, to \fItolerance\fP degrees. Defaults to 0.04 de \fB--max-mismatch-prob\fP \fIprobability\fP \fIprobability\fP is the maximum allowable probability of an incorrect star identification, for star id algorithms which support it. Defaults to 0.001. +.TP +\fB--prev-attitude\fP \fIattitude\fP +\fIattitude\fP is the known previous attitude for tracking mode. Should give ra, dec, and roll as a comma-separated list: "ra,dec,roll." + +.TP +\fB--uncertainty\fP \fIradius\fP +For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. + .TP \fB--attitude-algo\fP \fIalgo\fP Runs the \fIalgo\fP algorithm for the attitude stage of the pipeline. Current options are "dqm" (Davenport Q) and "triad". Defaults to dqm. diff --git a/src/io.cpp b/src/io.cpp index 56be1dfa..d778264e 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -695,6 +695,24 @@ Pipeline SetPipeline(const PipelineOptions &values) { result.starIdAlgorithm = std::unique_ptr(new GeometricVotingStarIdAlgorithm(DegToRad(values.angularTolerance))); } else if (values.idAlgo == "py") { result.starIdAlgorithm = std::unique_ptr(new PyramidStarIdAlgorithm(DegToRad(values.angularTolerance), values.estimatedNumFalseStars, values.maxMismatchProb, 1000)); + } else if (values.idAlgo == "tracking") { + result.starIdAlgorithm = std::unique_ptr(new TrackingModeStarIdAlgorithm()); + + // convert user inputted string ra,dec,roll to individual floats + std::vector attInputs; + std::stringstream ss (values.prevAttitudeString); + std::string item; + while (std::getline (ss, item, ',')) { + attInputs.push_back(stof(item)); + } + + // convert ra, dec, and roll to quaternion attitude + Quaternion q = SphericalToQuaternion(attInputs[0], attInputs[1], attInputs[2]); + Attitude a = Attitude(q); + + // set prev attitude part of pipeline + PrevAttitude prev(a, values.uncertainty); + result.prevAttitude = prev; } else if (values.idAlgo != "") { std::cout << "Illegal id algorithm." << std::endl; exit(1); @@ -757,7 +775,7 @@ PipelineOutput Pipeline::Go(const PipelineInput &input) { if (starIdAlgorithm && database && inputStars && input.InputCamera()) { // TODO: don't copy the vector! result.starIds = std::unique_ptr(new std::vector( - starIdAlgorithm->Go(database.get(), *inputStars, result.catalog, *input.InputCamera()))); + starIdAlgorithm->Go(database.get(), *inputStars, result.catalog, *input.InputCamera(), prevAttitude))); inputStarIds = result.starIds.get(); } @@ -773,7 +791,6 @@ PipelineOutput Pipeline::Go(const PipelineInput &input) { std::vector Pipeline::Go(const PipelineInputList &inputs) { std::vector result; - for (const std::unique_ptr &input : inputs) { result.push_back(Go(*input)); } diff --git a/src/io.hpp b/src/io.hpp index 7e468ff5..3d480e2e 100644 --- a/src/io.hpp +++ b/src/io.hpp @@ -191,6 +191,7 @@ class Pipeline { std::unique_ptr starIdAlgorithm; std::unique_ptr attitudeEstimationAlgorithm; std::unique_ptr database; + PrevAttitude prevAttitude; }; Pipeline SetPipeline(const PipelineOptions &values); diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index a8e74a00..eb65afed 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -24,6 +24,8 @@ LOST_CLI_OPTION("centroid-dummy-stars" , int , centroidDummyNumStars LOST_CLI_OPTION("centroid-mag-filter" , float , centroidMagFilter , -1 , atof(optarg) , 5) LOST_CLI_OPTION("database" , std::string, databasePath , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") +LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) +LOST_CLI_OPTION("uncertainty " , float , uncertainty , -1 , atof(optarg) , 5) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("max-mismatch-probability" , float , maxMismatchProb , .001, atof(optarg) , kNoDefaultArgument) diff --git a/src/star-id.cpp b/src/star-id.cpp index e4b0246e..53a628ac 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -10,7 +10,7 @@ namespace lost { StarIdentifiers DummyStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { StarIdentifiers result; @@ -22,7 +22,7 @@ StarIdentifiers DummyStarIdAlgorithm::Go( } StarIdentifiers GeometricVotingStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { StarIdentifiers identified; MultiDatabase multiDatabase(database); @@ -285,7 +285,7 @@ void PyramidIdentifyRemainingStars(StarIdentifiers *identifiers, } StarIdentifiers PyramidStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { StarIdentifiers identified; MultiDatabase multiDatabase(database); @@ -504,7 +504,7 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( } StarIdentifiers TrackingModeStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { StarIdentifiers identified; MultiDatabase multiDatabase(database); diff --git a/src/star-id.hpp b/src/star-id.hpp index d91dcae6..f62cc400 100644 --- a/src/star-id.hpp +++ b/src/star-id.hpp @@ -12,18 +12,18 @@ namespace lost { class StarIdAlgorithm { public: virtual StarIdentifiers Go( - const unsigned char *database, const Stars &, const Catalog &, const Camera &) const = 0; + const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const = 0; virtual ~StarIdAlgorithm() { }; }; class DummyStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; }; class GeometricVotingStarIdAlgorithm : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; GeometricVotingStarIdAlgorithm(float tolerance): tolerance(tolerance) { }; private: float tolerance; @@ -31,7 +31,7 @@ class GeometricVotingStarIdAlgorithm : public StarIdAlgorithm { class PyramidStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; /** * @param tolerance Angular tolerance in distances (measurement error) * @param numFalseStars an estimate of the number of false stars in the whole celestial sphere @@ -53,7 +53,7 @@ class PyramidStarIdAlgorithm final : public StarIdAlgorithm { class TrackingModeStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; }; diff --git a/src/star-utils.hpp b/src/star-utils.hpp index 587dfe71..98203452 100644 --- a/src/star-utils.hpp +++ b/src/star-utils.hpp @@ -65,6 +65,8 @@ class PrevAttitude { public: PrevAttitude(Attitude prev, float uncertainty) : prev(prev), uncertainty(uncertainty) { }; + PrevAttitude() + : PrevAttitude(Attitude(), -1.0f) { }; Attitude prev; float uncertainty; From 80ea959f3672e35b916b8ab35474a59b9694ddb3 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 12 Aug 2022 17:34:20 -0700 Subject: [PATCH 18/54] first try at implementing starid algorithm --- src/star-id.cpp | 128 +++++++++++++++++++++++------------------------- 1 file changed, 60 insertions(+), 68 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 53a628ac..3f91db33 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "star-id.hpp" #include "databases.hpp" @@ -503,6 +504,13 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( return identified; } +// for ordering vec3 in votes map in tracking mode +bool operator<(const Vec3& l, const Vec3& r) { + if (l.x < r.x) return true; + if (l.y < r.y) return true; + return (l.z < r.z); +} + StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { @@ -514,80 +522,64 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); + std::map votes; - // TODO figure out how users can get - float radius; - float ra; - float dec; - float roll; - - + // for every centroid... for (int i = 0; i < (int)stars.size(); i++) { + + // find previous position of the centroid based on the old attitude + Vec3 prevPosition = camera.CameraToSpatial(stars[i].position); + prevPosition = prevAttitude.prev.Rotate(prevPosition); + + // find out which stars are within the bounds of that position + std::vector possiblePrevStarInd = vectorDatabase.QueryNearestStars(catalog, prevPosition, prevAttitude.uncertainty); + + // for every possible prev position... + for (int j = 0; j < (int)possiblePrevStarInd.size(); j++) { + + // vote for the amount of rotation + CatalogStar possibleStar = catalog[possiblePrevStarInd[j]]; + Vec3 pos = possibleStar.spatial; + Vec3 diff = prevPosition - pos; + + if (votes.count(diff)) { + votes[diff]++; + } else { + votes.insert(std::make_pair(diff,1)); + } + } + + } - // TODO figure out math for how to get the current position in degrees based on prev attitude - Vec3 position; - - std::vector possiblePrevStarInd = vectorDatabase.QueryNearestStars(catalog, position, radius); - - - - std::vector votes(catalog.size(), 0); - Vec2 c; - c.x = 0; - c.y = 0; - Vec3 center = camera.CameraToSpatial(c).Normalize(); - Vec3 iSpatial = camera.CameraToSpatial(stars[i].position).Normalize(); - float distance = Distance(center, iSpatial); - - - - - // for (int j = 0; j < (int)stars.size(); j++) { - // if (i != j) { - // // TODO: find a faster way to do this: - // std::vector votedInPair(catalog.size(), false); - // Vec3 jSpatial = camera.CameraToSpatial(stars[j].position).Normalize(); - // float greatCircleDistance = AngleUnit(iSpatial, jSpatial); - // //give a greater range for min-max Query for bigger radius (GreatCircleDistance) - // float lowerBoundRange = greatCircleDistance - tolerance; - // float upperBoundRange = greatCircleDistance + tolerance; - // const int16_t *upperBoundSearch; - // const int16_t *lowerBoundSearch = vectorDatabase.FindPairsLiberal( - // lowerBoundRange, upperBoundRange, &upperBoundSearch); - // //loop from lowerBoundSearch till numReturnedPairs, add one vote to each star in the pairs in the datastructure - // for (const int16_t *k = lowerBoundSearch; k != upperBoundSearch; k++) { - // if ((k - lowerBoundSearch) % 2 == 0) { - // float actualAngle = AngleUnit(catalog[*k].spatial, catalog[*(k+1)].spatial); - // assert(actualAngle <= greatCircleDistance + tolerance * 2); - // assert(actualAngle >= greatCircleDistance - tolerance * 2); - // } - // if (!votedInPair[*k] || true) { - // votes[*k]++; - // votedInPair[*k] = true; - // } - // } - // // US voting system - // } - // } - // // Find star w most votes - // int16_t maxVotes = votes[0]; - // int indexOfMax = 0; - // for (int v = 1; v < (int)votes.size(); v++) { - // if (votes[v] > maxVotes) { - // maxVotes = votes[v]; - // indexOfMax = v; - // } - // } - // StarIdentifier newStar(i, indexOfMax); - // // Set identified[i] to value of catalog index of star w most votesr - // identified.push_back(newStar); - } - + // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) + Vec3 votedDiff = {0,0,0}; + std::pair entryWithMaxValue = std::make_pair(votedDiff,0); + std::map::iterator currentEntry; + for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { + + if (currentEntry->second > entryWithMaxValue.second) { + entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); + votedDiff = currentEntry->first; + } + } + + for (int i = 0; i < (int)stars.size(); i++) { + // find star's past position + Vec3 prevPosition = camera.CameraToSpatial(stars[i].position); + prevPosition = prevAttitude.prev.Rotate(prevPosition); + prevPosition = prevPosition - votedDiff; + // identify which star in the catalog has the same past position + for (int j = 0; j < (int)catalog.size(); j++) { + if (catalog[j].spatial.x == prevPosition.x && catalog[j].spatial.y == prevPosition.y && catalog[j].spatial.z == prevPosition.z) { + identified.push_back(StarIdentifier(i, j)); + break; + } + } + } + return identified; } - - } From 43bffc8bb5ce740fba3f55f28f73adeede865141 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 12 Aug 2022 18:12:25 -0700 Subject: [PATCH 19/54] fix cli desciption, style constant name according to Google guidelines --- documentation/pipeline.man | 4 ++-- src/databases.hpp | 2 +- src/io.cpp | 2 +- src/star-id.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index ce9091b1..591e5e8a 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -55,7 +55,7 @@ Will not consider candidate as centroid if its magnitude is below \fImin-mag\fP. .TP \fB--star-id-algo\fP \fIalgo\fP -Runs the \fIalgo\fP star identification algorithm. Current options are "dummy", "gv", and "pyramid". Defaults to "dummy" if option is not selected. +Runs the \fIalgo\fP star identification algorithm. Current options are "dummy", "gv", "pyramid", and "tracking". Defaults to "pyramid" if option is not selected. .TP \fB--angular-tolerance\fP [\fItolerance\fP] Sets the estimated angular centroiding error tolerance, @@ -75,7 +75,7 @@ used in some star id algorithms, to \fItolerance\fP degrees. Defaults to 0.04 de .TP \fB--uncertainty\fP \fIradius\fP -For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. +For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 5 if not selected. .TP \fB--attitude-algo\fP \fIalgo\fP diff --git a/src/databases.hpp b/src/databases.hpp index 0d827bca..ef2ab5d9 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -88,7 +88,7 @@ class TripleInnerKVectorDatabase { class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); - const static int32_t magicValue = 0x2536f0A9; + const static int32_t kMagicValue = 0x2536f0A9; std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius); private: diff --git a/src/io.cpp b/src/io.cpp index d778264e..e49ff138 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -251,7 +251,7 @@ void BuildKVectorDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog, void BuildTrackingDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog) { long length = SerializeLengthTrackingCatalog(catalog); - unsigned char *buffer = builder.AddSubDatabase(TrackingSortedDatabase::magicValue, length); + unsigned char *buffer = builder.AddSubDatabase(TrackingSortedDatabase::kMagicValue, length); if (buffer == NULL) { std::cerr << "No room for another database." << std::endl; } diff --git a/src/star-id.cpp b/src/star-id.cpp index 3f91db33..2e227d69 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -516,7 +516,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( StarIdentifiers identified; MultiDatabase multiDatabase(database); - const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::magicValue); + const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue); if (databaseBuffer == NULL) { return identified; } From 16f7108766068b71c48638a5073ef64a0fb66402 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Fri, 12 Aug 2022 18:31:49 -0700 Subject: [PATCH 20/54] compare vec3 equality based on threshold passed through cli instead of == --- documentation/pipeline.man | 6 +++++- src/io.cpp | 2 +- src/pipeline-options.hpp | 1 + src/star-id.cpp | 19 ++++++++++++++++--- src/star-utils.hpp | 7 ++++--- 5 files changed, 27 insertions(+), 8 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index 591e5e8a..bc09afd1 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -11,7 +11,7 @@ pipeline \- builds a pipeline of the star identification process .br \fBpipeline\fP --generate[=\fInum-images\fP] [--horizontal-res \fIres\fP] [--vertical-res \fIres\fP] [--horizontal-fov \fIdegrees\fP] [--ref-brightness \fImag\fP] [--spread-stddev \fIstddev\fP] [--noise-stddev \fIstddev\fP] [--boresight-right-asc \fIascension\fP] [--boresight-dec \fIdeclination\fP] [--boresight-roll \fIroll\fP] [--centroid-algo \fIalgorithm\fP [--centroid-dummy-stars \fInum-stars\fP]] [--centroid-mag-filter \fImin-mag\fP] -[--database \fIfilename\fP] [--star-id-algo \fIalgo\fP (--gv-tolerance \fIdegrees\fP | --py-tolerance \fIdegrees\fP --false-stars \fInum\fP --max-mismatch-prob \fIprobability\fP | --prev-attitude \fIattitude\fP --uncertainty \fIradius\fP)] [--attitude-algo \fIalgo\fP] [--plot \fIoutput-path\fB] +[--database \fIfilename\fP] [--star-id-algo \fIalgo\fP (--gv-tolerance \fIdegrees\fP | --py-tolerance \fIdegrees\fP --false-stars \fInum\fP --max-mismatch-prob \fIprobability\fP | --prev-attitude \fIattitude\fP --uncertainty \fIradius\fP (--tracking-compare-threshold \fIthreshold\fP))] [--attitude-algo \fIalgo\fP] [--plot \fIoutput-path\fB] .SH DESCRIPTION @@ -77,6 +77,10 @@ used in some star id algorithms, to \fItolerance\fP degrees. Defaults to 0.04 de \fB--uncertainty\fP \fIradius\fP For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 5 if not selected. +.TP +\fB--tracking-compare-threshold\fP \fIthreshold\fP +\fIthreshold\fP is the threshold for comparing floats for tracking mode. Defaults to 0.0001 if no value is given. + .TP \fB--attitude-algo\fP \fIalgo\fP Runs the \fIalgo\fP algorithm for the attitude stage of the pipeline. Current options are "dqm" (Davenport Q) and "triad". Defaults to dqm. diff --git a/src/io.cpp b/src/io.cpp index e49ff138..5a0d1159 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -711,7 +711,7 @@ Pipeline SetPipeline(const PipelineOptions &values) { Attitude a = Attitude(q); // set prev attitude part of pipeline - PrevAttitude prev(a, values.uncertainty); + PrevAttitude prev(a, values.uncertainty, values.trackingCompareThreshold); result.prevAttitude = prev; } else if (values.idAlgo != "") { std::cout << "Illegal id algorithm." << std::endl; diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index eb65afed..23495134 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -26,6 +26,7 @@ LOST_CLI_OPTION("database" , std::string, databasePath LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("uncertainty " , float , uncertainty , -1 , atof(optarg) , 5) +LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, -1 , atof(optarg) , 0.0001) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("max-mismatch-probability" , float , maxMismatchProb , .001, atof(optarg) , kNoDefaultArgument) diff --git a/src/star-id.cpp b/src/star-id.cpp index 2e227d69..16c9f1ab 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -511,6 +511,13 @@ bool operator<(const Vec3& l, const Vec3& r) { return (l.z < r.z); } +bool vecEquals(const Vec3& l, const Vec3& r, const float threshold) { + if (abs(l.x - r.x) > threshold) return false; + if (abs(l.y - r.y) > threshold) return false; + if (abs(l.z - r.z) > threshold) return false; + return true; +} + StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { @@ -542,9 +549,15 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( Vec3 pos = possibleStar.spatial; Vec3 diff = prevPosition - pos; - if (votes.count(diff)) { - votes[diff]++; - } else { + bool found = false; + for (auto& pair : votes) { + if (vecEquals(pair.first, diff, prevAttitude.compareThreshold)) { + pair.second++; + found = true; + break; + } + } + if (!found) { votes.insert(std::make_pair(diff,1)); } } diff --git a/src/star-utils.hpp b/src/star-utils.hpp index 98203452..b91cbae1 100644 --- a/src/star-utils.hpp +++ b/src/star-utils.hpp @@ -63,13 +63,14 @@ Catalog NarrowCatalog(const Catalog &, int maxMagnitude, int maxStars); // for tracking mode class PrevAttitude { public: - PrevAttitude(Attitude prev, float uncertainty) - : prev(prev), uncertainty(uncertainty) { }; + PrevAttitude(Attitude prev, float uncertainty, float compareThreshold) + : prev(prev), uncertainty(uncertainty), compareThreshold(compareThreshold) { }; PrevAttitude() - : PrevAttitude(Attitude(), -1.0f) { }; + : PrevAttitude(Attitude(), -1.0f, 0.001) { }; Attitude prev; float uncertainty; + float compareThreshold; }; } From 072236648b8a9dbe404a077a4b13908b495cb39b Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 18:41:45 -0700 Subject: [PATCH 21/54] second attempt at tracking starid vote by rotation of star pairs instead --- src/star-id.cpp | 120 +++++++++++++++++++++++++++++------------------- 1 file changed, 74 insertions(+), 46 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 16c9f1ab..8f730516 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -504,18 +504,38 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( return identified; } -// for ordering vec3 in votes map in tracking mode -bool operator<(const Vec3& l, const Vec3& r) { - if (l.x < r.x) return true; - if (l.y < r.y) return true; - return (l.z < r.z); +// for ordering Quaternions in votes map in tracking mode +bool operator<(const Quaternion& l, const Quaternion& r) { + if (l.real < r.real) return true; + if (l.i < r.i) return true; + if (l.j < r.j) return true; + return (l.k < r.k); } -bool vecEquals(const Vec3& l, const Vec3& r, const float threshold) { +// for tracking mode vector equality +bool quatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { + if (abs(l.real - r.real) > threshold) return false; + if (abs(l.i - r.i) > threshold) return false; + if (abs(l.j - r.j) > threshold) return false; + return abs(l.k - r.k) > threshold; +} + +bool vec3Equals(const Vec3& l, const Vec3& r, const float threshold) { if (abs(l.x - r.x) > threshold) return false; if (abs(l.y - r.y) > threshold) return false; - if (abs(l.z - r.z) > threshold) return false; - return true; + return abs(l.z - r.z) > threshold; +} + +// return a matrix whose columns are the axes of the frame +static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { + Vec3 d1 = v1.Normalize(); + Vec3 d2 = v1.crossProduct(v2).Normalize(); + Vec3 d3 = d1.crossProduct(d2).Normalize(); + return { + d1.x, d2.x, d3.x, + d1.y, d2.y, d3.y, + d1.z, d2.z, d3.z, + }; } StarIdentifiers TrackingModeStarIdAlgorithm::Go( @@ -529,63 +549,71 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::map votes; + std::map votes; - // for every centroid... + // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { - + // find previous position of the centroid based on the old attitude - Vec3 prevPosition = camera.CameraToSpatial(stars[i].position); - prevPosition = prevAttitude.prev.Rotate(prevPosition); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); + starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); - // find out which stars are within the bounds of that position - std::vector possiblePrevStarInd = vectorDatabase.QueryNearestStars(catalog, prevPosition, prevAttitude.uncertainty); - - // for every possible prev position... - for (int j = 0; j < (int)possiblePrevStarInd.size(); j++) { - - // vote for the amount of rotation - CatalogStar possibleStar = catalog[possiblePrevStarInd[j]]; - Vec3 pos = possibleStar.spatial; - Vec3 diff = prevPosition - pos; - - bool found = false; - for (auto& pair : votes) { - if (vecEquals(pair.first, diff, prevAttitude.compareThreshold)) { - pair.second++; - found = true; - break; + // find all the possible previous stars + std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); + + for (int j = i+1; j < (int)stars.size()-1; j++) { + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); + starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); + + std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); + + // vote for the rotation that every pair makes + for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { + for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { + + // calculate the rotation (using triad attitude estimation method) + Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); + Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); + Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); // TODO normalize to unit quaternion? + + // vote for the quaternion + bool found = false; + for (auto& pair : votes) { + if (quatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + pair.second++; + found = true; + break; + } + } + if (!found) { + votes.insert(std::make_pair(rot,1)); + } } } - if (!found) { - votes.insert(std::make_pair(diff,1)); - } - } - + } } // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) - Vec3 votedDiff = {0,0,0}; - std::pair entryWithMaxValue = std::make_pair(votedDiff,0); - std::map::iterator currentEntry; + Quaternion votedQuat = {0,0,0,0}; + std::pair entryWithMaxValue = std::make_pair(votedQuat,0); + std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { - if (currentEntry->second > entryWithMaxValue.second) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); - votedDiff = currentEntry->first; + votedQuat = currentEntry->first; } } for (int i = 0; i < (int)stars.size(); i++) { - // find star's past position - Vec3 prevPosition = camera.CameraToSpatial(stars[i].position); - prevPosition = prevAttitude.prev.Rotate(prevPosition); - prevPosition = prevPosition - votedDiff; + // find star's current position + Vec3 currPosition = camera.CameraToSpatial(stars[i].position); + currPosition = prevAttitude.prev.Rotate(currPosition); + currPosition = Attitude(votedQuat).Rotate(currPosition); - // identify which star in the catalog has the same past position + // identify which star in the catalog has the same curr position for (int j = 0; j < (int)catalog.size(); j++) { - if (catalog[j].spatial.x == prevPosition.x && catalog[j].spatial.y == prevPosition.y && catalog[j].spatial.z == prevPosition.z) { + if (vec3Equals(catalog[j].spatial,currPosition, prevAttitude.compareThreshold)) { identified.push_back(StarIdentifier(i, j)); break; } From 8912d801955544ee8d6110a92d009cfc195d0a56 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 19:02:48 -0700 Subject: [PATCH 22/54] pipeline fixes added warning when forgetting to pass in prev attitude fixed the way prevAttitude is being passed in (as member of the star id algo class instead of part of the pipeline) --- src/io.cpp | 16 ++++++++++------ src/io.hpp | 1 - src/star-id.cpp | 13 +++++++++---- src/star-id.hpp | 16 ++++++++++------ 4 files changed, 29 insertions(+), 17 deletions(-) diff --git a/src/io.cpp b/src/io.cpp index 5a0d1159..9507cbfe 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -696,7 +696,10 @@ Pipeline SetPipeline(const PipelineOptions &values) { } else if (values.idAlgo == "py") { result.starIdAlgorithm = std::unique_ptr(new PyramidStarIdAlgorithm(DegToRad(values.angularTolerance), values.estimatedNumFalseStars, values.maxMismatchProb, 1000)); } else if (values.idAlgo == "tracking") { - result.starIdAlgorithm = std::unique_ptr(new TrackingModeStarIdAlgorithm()); + + if (values.prevAttitudeString == "") { + std::cout << "WARNING: information about the previous attitude has not been passed in while tracking mode is set." << std::endl; + } // convert user inputted string ra,dec,roll to individual floats std::vector attInputs; @@ -707,12 +710,13 @@ Pipeline SetPipeline(const PipelineOptions &values) { } // convert ra, dec, and roll to quaternion attitude - Quaternion q = SphericalToQuaternion(attInputs[0], attInputs[1], attInputs[2]); + Quaternion q = SphericalToQuaternion(DegToRad(attInputs[0]), DegToRad(attInputs[1]), DegToRad(attInputs[2])); Attitude a = Attitude(q); - - // set prev attitude part of pipeline + + // set prev attitude and id algo PrevAttitude prev(a, values.uncertainty, values.trackingCompareThreshold); - result.prevAttitude = prev; + result.starIdAlgorithm = std::unique_ptr(new TrackingModeStarIdAlgorithm(prev)); + } else if (values.idAlgo != "") { std::cout << "Illegal id algorithm." << std::endl; exit(1); @@ -775,7 +779,7 @@ PipelineOutput Pipeline::Go(const PipelineInput &input) { if (starIdAlgorithm && database && inputStars && input.InputCamera()) { // TODO: don't copy the vector! result.starIds = std::unique_ptr(new std::vector( - starIdAlgorithm->Go(database.get(), *inputStars, result.catalog, *input.InputCamera(), prevAttitude))); + starIdAlgorithm->Go(database.get(), *inputStars, result.catalog, *input.InputCamera()))); inputStarIds = result.starIds.get(); } diff --git a/src/io.hpp b/src/io.hpp index 3d480e2e..7e468ff5 100644 --- a/src/io.hpp +++ b/src/io.hpp @@ -191,7 +191,6 @@ class Pipeline { std::unique_ptr starIdAlgorithm; std::unique_ptr attitudeEstimationAlgorithm; std::unique_ptr database; - PrevAttitude prevAttitude; }; Pipeline SetPipeline(const PipelineOptions &values); diff --git a/src/star-id.cpp b/src/star-id.cpp index 8f730516..35b21828 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -11,7 +11,7 @@ namespace lost { StarIdentifiers DummyStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { StarIdentifiers result; @@ -23,7 +23,7 @@ StarIdentifiers DummyStarIdAlgorithm::Go( } StarIdentifiers GeometricVotingStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { StarIdentifiers identified; MultiDatabase multiDatabase(database); @@ -286,7 +286,7 @@ void PyramidIdentifyRemainingStars(StarIdentifiers *identifiers, } StarIdentifiers PyramidStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { StarIdentifiers identified; MultiDatabase multiDatabase(database); @@ -539,7 +539,10 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { } StarIdentifiers TrackingModeStarIdAlgorithm::Go( - const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera, const PrevAttitude &prevAttitude) const { + const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { + + std::cout << "HERE" << std::endl; + StarIdentifiers identified; MultiDatabase multiDatabase(database); @@ -549,6 +552,8 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); + std::cout << "HERE2" << std::endl; + std::map votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position diff --git a/src/star-id.hpp b/src/star-id.hpp index f62cc400..77ab31a5 100644 --- a/src/star-id.hpp +++ b/src/star-id.hpp @@ -11,19 +11,18 @@ namespace lost { class StarIdAlgorithm { public: - virtual StarIdentifiers Go( - const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const = 0; + virtual StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const = 0; virtual ~StarIdAlgorithm() { }; }; class DummyStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; }; class GeometricVotingStarIdAlgorithm : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; GeometricVotingStarIdAlgorithm(float tolerance): tolerance(tolerance) { }; private: float tolerance; @@ -31,7 +30,7 @@ class GeometricVotingStarIdAlgorithm : public StarIdAlgorithm { class PyramidStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; /** * @param tolerance Angular tolerance in distances (measurement error) * @param numFalseStars an estimate of the number of false stars in the whole celestial sphere @@ -53,7 +52,12 @@ class PyramidStarIdAlgorithm final : public StarIdAlgorithm { class TrackingModeStarIdAlgorithm final : public StarIdAlgorithm { public: - StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &, const PrevAttitude &) const; + StarIdentifiers Go(const unsigned char *database, const Stars &, const Catalog &, const Camera &) const; + TrackingModeStarIdAlgorithm(PrevAttitude prevAttitude) + : prevAttitude(prevAttitude) { }; + +private: + PrevAttitude prevAttitude; }; From 209c14b4f2b84bd5f060e78a00132dc57a90eef2 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 20:28:04 -0700 Subject: [PATCH 23/54] fix png error is already fixed in master but didn't realize it's not fixed on this branch! --- src/pipeline-options.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index 23495134..1a11d0c4 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -13,7 +13,7 @@ // Inside of the current Paragraph to comma. // CAMERA -LOST_CLI_OPTION("png" , std::string, png , "" , atof(optarg) , kNoDefaultArgument) +LOST_CLI_OPTION("png" , std::string, png , "" , optarg, kNoDefaultArgument) LOST_CLI_OPTION("focal-length" , float , focalLength , 0 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("pixel-size" , float , pixelSize , -1 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("fov" , float , fov , 20 , atof(optarg) , kNoDefaultArgument) From 55ee8883cc482def4d21430b6b253a4e7c23bdb3 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 20:48:12 -0700 Subject: [PATCH 24/54] fix infinite loop in query --- src/databases.cpp | 1 + src/star-id.cpp | 5 ----- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 52ca2003..0533dae4 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -390,6 +390,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, if (diff.MagnitudeSq() <= radiusSq) { index = mid; + break; } else if (s.spatial.x < point.x) { left += mid; } else { diff --git a/src/star-id.cpp b/src/star-id.cpp index 35b21828..c7c0272b 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -541,9 +541,6 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { - std::cout << "HERE" << std::endl; - - StarIdentifiers identified; MultiDatabase multiDatabase(database); const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue); @@ -552,8 +549,6 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::cout << "HERE2" << std::endl; - std::map votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position From e320cc8d31378bbc471fbbb77565120f15834e90 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 21:32:09 -0700 Subject: [PATCH 25/54] change rotation type from quaternion to dcm, fix bug in query --- src/databases.cpp | 3 ++- src/star-id.cpp | 39 +++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 0533dae4..7d637575 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -391,7 +391,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, if (diff.MagnitudeSq() <= radiusSq) { index = mid; break; - } else if (s.spatial.x < point.x) { + } else if (s.spatial.x > point.x) { left += mid; } else { right = mid - 1; @@ -425,6 +425,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, sRight = c[right]; diffLeft = sRight.spatial - point; } + return query_ind; } diff --git a/src/star-id.cpp b/src/star-id.cpp index c7c0272b..d8e637c5 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -505,19 +505,19 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( } // for ordering Quaternions in votes map in tracking mode -bool operator<(const Quaternion& l, const Quaternion& r) { - if (l.real < r.real) return true; - if (l.i < r.i) return true; - if (l.j < r.j) return true; - return (l.k < r.k); +bool operator<(const Mat3& l, const Mat3& r) { + for (int i = 0; i < 9; i++) { + if (l.x[i] < r.x[i]) return true; + } + return false; } -// for tracking mode vector equality -bool quatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { - if (abs(l.real - r.real) > threshold) return false; - if (abs(l.i - r.i) > threshold) return false; - if (abs(l.j - r.j) > threshold) return false; - return abs(l.k - r.k) > threshold; +// for tracking mode dcm matrix equality +bool mat3Equals(const Mat3& l, const Mat3& r, const float threshold) { + for (int i = 0; i < 9; i++) { + if (abs(l.x[i] - r.x[i]) > threshold) return false; + } + return true; } bool vec3Equals(const Vec3& l, const Vec3& r, const float threshold) { @@ -549,7 +549,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::map votes; + std::map votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { @@ -557,7 +557,6 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // find previous position of the centroid based on the old attitude Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); - // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); @@ -574,12 +573,12 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); - Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); // TODO normalize to unit quaternion? + Mat3 rot = prevFrame*possibleFrame.Transpose(); // vote for the quaternion bool found = false; for (auto& pair : votes) { - if (quatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + if (mat3Equals(pair.first, rot, prevAttitude.compareThreshold)) { pair.second++; found = true; break; @@ -594,13 +593,13 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) - Quaternion votedQuat = {0,0,0,0}; - std::pair entryWithMaxValue = std::make_pair(votedQuat,0); - std::map::iterator currentEntry; + Mat3 votedDCM = {0,0,0,0,0,0,0,0,0}; + std::pair entryWithMaxValue = std::make_pair(votedDCM,0); + std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { if (currentEntry->second > entryWithMaxValue.second) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); - votedQuat = currentEntry->first; + votedDCM = currentEntry->first; } } @@ -609,7 +608,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // find star's current position Vec3 currPosition = camera.CameraToSpatial(stars[i].position); currPosition = prevAttitude.prev.Rotate(currPosition); - currPosition = Attitude(votedQuat).Rotate(currPosition); + currPosition = Attitude(votedDCM).Rotate(currPosition); // identify which star in the catalog has the same curr position for (int j = 0; j < (int)catalog.size(); j++) { From b14869e11cd01c07821a9ddf1d37a61b48751d1d Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 21:35:43 -0700 Subject: [PATCH 26/54] fix bug in uncertainty default value --- src/pipeline-options.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index 1a11d0c4..44d3fa79 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -25,7 +25,7 @@ LOST_CLI_OPTION("centroid-mag-filter" , float , centroidMagFilter LOST_CLI_OPTION("database" , std::string, databasePath , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) -LOST_CLI_OPTION("uncertainty " , float , uncertainty , -1 , atof(optarg) , 5) +LOST_CLI_OPTION("uncertainty " , float , uncertainty , 5 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, -1 , atof(optarg) , 0.0001) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) From 6706376bd4cfc8dee3fb15d3b23b7c23d3259a17 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 21:57:59 -0700 Subject: [PATCH 27/54] fix errors in querying I copy/pasted stuff from left loop to right and missed a few things to change Also fixed conditionals for loop Also made sure to assert radius is nonnegative, added to man page --- documentation/pipeline.man | 2 +- src/databases.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index bc09afd1..b5c05188 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -75,7 +75,7 @@ used in some star id algorithms, to \fItolerance\fP degrees. Defaults to 0.04 de .TP \fB--uncertainty\fP \fIradius\fP -For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 5 if not selected. +For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 5 if not selected. Must be nonnegative. .TP \fB--tracking-compare-threshold\fP \fIthreshold\fP diff --git a/src/databases.cpp b/src/databases.cpp index 7d637575..2a940f4d 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -374,6 +374,7 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, float radius) { + assert(radius >= 0); std::vector query_ind; float radiusSq = pow(radius,2); @@ -399,12 +400,12 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, } left = index; - right = index; + right = index+1; // see how far left you can go CatalogStar sLeft = c[left]; Vec3 diffLeft = sLeft.spatial - point; - while (left > 0 && sLeft.spatial.x <= point.x - radius) { + while (left > 0 && (abs(diffLeft.x) <= radius)) { if (diffLeft.MagnitudeSq() <= radiusSq) { query_ind.push_back(left); } @@ -413,19 +414,23 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, diffLeft = sLeft.spatial - point; } - // see how far right you can go CatalogStar sRight = c[right]; Vec3 diffRight = sRight.spatial - point; - while (right > 0 && sRight.spatial.x <= point.x - radius) { + while (right < (int)c.size() && (abs(diffRight.x) <= radius)) { if (diffRight.MagnitudeSq() <= radiusSq) { query_ind.push_back(right); } - right--; + right++; sRight = c[right]; - diffLeft = sRight.spatial - point; + diffRight = sRight.spatial - point; } + // std::cout << query_ind.size() << std::endl; + // for (int i = 0; i < (int)query_ind.size(); i++) { + // std::cout << "i " << query_ind[i] << std::endl; + // } + return query_ind; } From fe4833e1df5082d7ab067f1976caaf3d3751ea83 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sat, 13 Aug 2022 22:24:19 -0700 Subject: [PATCH 28/54] identifies stars! (incorrectly, but at least not 0 identified) --- src/star-id.cpp | 63 ++++++++++++++++++++++++++++++++++--------------- 1 file changed, 44 insertions(+), 19 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index d8e637c5..6b8de4d8 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -538,6 +538,11 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { }; } +struct IndexChanges{ + int starsIndex; + int catalogIndex; +}; + StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -549,7 +554,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::map votes; + std::map> votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { @@ -570,6 +575,16 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { + // get the changes + IndexChanges starAChanges; + starAChanges.starsIndex = i; + starAChanges.catalogIndex = starAPossiblePrevStars[k]; + + // get the changes + IndexChanges starBChanges; + starBChanges.starsIndex = j; + starBChanges.catalogIndex = starBPossiblePrevStars[l]; + // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); @@ -579,13 +594,14 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( bool found = false; for (auto& pair : votes) { if (mat3Equals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second++; + pair.second.push_back(starAChanges); + pair.second.push_back(starBChanges); found = true; break; } } if (!found) { - votes.insert(std::make_pair(rot,1)); + votes.insert(std::make_pair(rot,std::vector{starAChanges, starBChanges})); } } } @@ -594,31 +610,40 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) Mat3 votedDCM = {0,0,0,0,0,0,0,0,0}; - std::pair entryWithMaxValue = std::make_pair(votedDCM,0); - std::map::iterator currentEntry; + std::pair> entryWithMaxValue = std::make_pair(votedDCM,std::vector()); + std::map>::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { - if (currentEntry->second > entryWithMaxValue.second) { + if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); votedDCM = currentEntry->first; + std::cout << currentEntry->second.size() << std::endl; } } - for (int i = 0; i < (int)stars.size(); i++) { - - // find star's current position - Vec3 currPosition = camera.CameraToSpatial(stars[i].position); - currPosition = prevAttitude.prev.Rotate(currPosition); - currPosition = Attitude(votedDCM).Rotate(currPosition); + std::cout << entryWithMaxValue.second.size() << std::endl; - // identify which star in the catalog has the same curr position - for (int j = 0; j < (int)catalog.size(); j++) { - if (vec3Equals(catalog[j].spatial,currPosition, prevAttitude.compareThreshold)) { - identified.push_back(StarIdentifier(i, j)); - break; - } - } + for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { + identified.push_back(StarIdentifier(entryWithMaxValue.second[i].starsIndex, entryWithMaxValue.second[i].catalogIndex)); } + // for (int i = 0; i < (int)stars.size(); i++) { + + // // find star's current position + // Vec3 currPosition = camera.CameraToSpatial(stars[i].position); + // currPosition = prevAttitude.prev.Rotate(currPosition); + // currPosition = Attitude(votedDCM).Rotate(currPosition); + // std::cout << "HERE " << i << std::endl; + + // // identify which star in the catalog has the same curr position + // for (int j = 0; j < (int)catalog.size(); j++) { + // if (vec3Equals(catalog[j].spatial,currPosition, prevAttitude.compareThreshold)) { + // std::cout << "HERE " << std::endl; + // identified.push_back(StarIdentifier(i, j)); + // break; + // } + // } + // } + return identified; } From c4c38923c0ab578d4549232b86128a3335bb6c2e Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sun, 14 Aug 2022 17:36:14 -0700 Subject: [PATCH 29/54] directly use StarIdentifer --- src/star-id.cpp | 45 ++++++++------------------------------------- 1 file changed, 8 insertions(+), 37 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 6b8de4d8..d61a600e 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -538,11 +538,6 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { }; } -struct IndexChanges{ - int starsIndex; - int catalogIndex; -}; - StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -554,7 +549,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::map> votes; + std::map votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { @@ -576,14 +571,8 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { // get the changes - IndexChanges starAChanges; - starAChanges.starsIndex = i; - starAChanges.catalogIndex = starAPossiblePrevStars[k]; - - // get the changes - IndexChanges starBChanges; - starBChanges.starsIndex = j; - starBChanges.catalogIndex = starBPossiblePrevStars[l]; + StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); + StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); @@ -601,7 +590,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } } if (!found) { - votes.insert(std::make_pair(rot,std::vector{starAChanges, starBChanges})); + votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); } } } @@ -610,8 +599,8 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) Mat3 votedDCM = {0,0,0,0,0,0,0,0,0}; - std::pair> entryWithMaxValue = std::make_pair(votedDCM,std::vector()); - std::map>::iterator currentEntry; + std::pair entryWithMaxValue = std::make_pair(votedDCM,StarIdentifiers{}); + std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); @@ -620,30 +609,12 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } } - std::cout << entryWithMaxValue.second.size() << std::endl; + // std::cout << entryWithMaxValue.second.size() << std::endl; for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { - identified.push_back(StarIdentifier(entryWithMaxValue.second[i].starsIndex, entryWithMaxValue.second[i].catalogIndex)); + identified.push_back(entryWithMaxValue.second[i]); } - // for (int i = 0; i < (int)stars.size(); i++) { - - // // find star's current position - // Vec3 currPosition = camera.CameraToSpatial(stars[i].position); - // currPosition = prevAttitude.prev.Rotate(currPosition); - // currPosition = Attitude(votedDCM).Rotate(currPosition); - // std::cout << "HERE " << i << std::endl; - - // // identify which star in the catalog has the same curr position - // for (int j = 0; j < (int)catalog.size(); j++) { - // if (vec3Equals(catalog[j].spatial,currPosition, prevAttitude.compareThreshold)) { - // std::cout << "HERE " << std::endl; - // identified.push_back(StarIdentifier(i, j)); - // break; - // } - // } - // } - return identified; } From 0a537edc0b18e8e3c15abeba74d2ba448fef3e09 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sun, 14 Aug 2022 17:51:08 -0700 Subject: [PATCH 30/54] switch back to describing rotation with quaternions use conjugates and angles to determine equality --- src/star-id.cpp | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index d61a600e..04897cc6 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -505,19 +505,16 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( } // for ordering Quaternions in votes map in tracking mode -bool operator<(const Mat3& l, const Mat3& r) { - for (int i = 0; i < 9; i++) { - if (l.x[i] < r.x[i]) return true; - } - return false; +bool operator<(const Quaternion& l, const Quaternion& r) { + if (l.real < r.real) return true; + if (l.i < r.i) return true; + if (l.j < r.j) return true; + return (l.k < r.k); } -// for tracking mode dcm matrix equality -bool mat3Equals(const Mat3& l, const Mat3& r, const float threshold) { - for (int i = 0; i < 9; i++) { - if (abs(l.x[i] - r.x[i]) > threshold) return false; - } - return true; +// for tracking mode rotation equality +bool QuatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { + return (l * r.Conjugate()).Angle() < threshold; } bool vec3Equals(const Vec3& l, const Vec3& r, const float threshold) { @@ -549,7 +546,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); - std::map votes; + std::map votes; // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { @@ -577,12 +574,12 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); - Mat3 rot = prevFrame*possibleFrame.Transpose(); + Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); // vote for the quaternion bool found = false; for (auto& pair : votes) { - if (mat3Equals(pair.first, rot, prevAttitude.compareThreshold)) { + if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { pair.second.push_back(starAChanges); pair.second.push_back(starBChanges); found = true; @@ -598,13 +595,13 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) - Mat3 votedDCM = {0,0,0,0,0,0,0,0,0}; - std::pair entryWithMaxValue = std::make_pair(votedDCM,StarIdentifiers{}); - std::map::iterator currentEntry; + Quaternion votedRot = {0,0,0,0}; + std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); + std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); - votedDCM = currentEntry->first; + votedRot = currentEntry->first; std::cout << currentEntry->second.size() << std::endl; } } From c088f80c18f172c29bc527c68eb1d3edddcfb160 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sun, 14 Aug 2022 18:11:56 -0700 Subject: [PATCH 31/54] fix database bugs no reason to use radius^2 had previously thought the < was wrong in the else if so I changed it to > but no, < is correct --- src/databases.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 2a940f4d..2cbe662f 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -377,7 +377,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, assert(radius >= 0); std::vector query_ind; - float radiusSq = pow(radius,2); + // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) int left = 0; @@ -388,11 +388,13 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, int mid = left + (right - left) / 2; CatalogStar s = c[mid]; Vec3 diff = s.spatial - point; + // std::cout << "INDEX " << mid << " CATALOG STAR " << s.spatial.x << ", " << s.spatial.y << ", " << s.spatial.z << std::endl; - if (diff.MagnitudeSq() <= radiusSq) { + if (diff.Magnitude() <= radius) { index = mid; + // std::cout << "HERE " << std::endl; break; - } else if (s.spatial.x > point.x) { + } else if (s.spatial.x < point.x) { left += mid; } else { right = mid - 1; @@ -406,7 +408,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, CatalogStar sLeft = c[left]; Vec3 diffLeft = sLeft.spatial - point; while (left > 0 && (abs(diffLeft.x) <= radius)) { - if (diffLeft.MagnitudeSq() <= radiusSq) { + if (diffLeft.Magnitude() <= radius) { query_ind.push_back(left); } left--; @@ -418,7 +420,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, CatalogStar sRight = c[right]; Vec3 diffRight = sRight.spatial - point; while (right < (int)c.size() && (abs(diffRight.x) <= radius)) { - if (diffRight.MagnitudeSq() <= radiusSq) { + if (diffRight.Magnitude() <= radius) { query_ind.push_back(right); } right++; @@ -431,6 +433,8 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, // std::cout << "i " << query_ind[i] << std::endl; // } + // std::cout << query_ind.size() << std::endl; + return query_ind; } From 5b87217acc12e212e60d88bad4c613dd200c966e Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sun, 14 Aug 2022 18:27:44 -0700 Subject: [PATCH 32/54] account for impossible rotations? also normalize CameraToSpatial outputs and a debug thing for query --- src/star-id.cpp | 43 ++++++++++++++++++++++++++++--------------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 04897cc6..5422d719 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -535,6 +535,8 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { }; } +#define debugQuery 0 + StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -552,13 +554,15 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( for (int i = 0; i < (int)stars.size(); i++) { // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); + if (debugQuery) break; + for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); @@ -574,20 +578,29 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); - Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); - - // vote for the quaternion - bool found = false; - for (auto& pair : votes) { - if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second.push_back(starAChanges); - pair.second.push_back(starBChanges); - found = true; - break; + + + Mat3 dcmRot = prevFrame*possibleFrame.Transpose(); + if (!(abs(dcmRot.Column(0).Magnitude()-1) < 0.001)) { + std::cout << "impossible" << std::endl; + } else { + // std::cout << "okay" << std::endl; + + Quaternion rot = DCMToQuaternion(dcmRot); + + // vote for the quaternion + bool found = false; + for (auto& pair : votes) { + if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + pair.second.push_back(starAChanges); + pair.second.push_back(starBChanges); + found = true; + break; + } + } + if (!found) { + votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); } - } - if (!found) { - votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); } } } From 0824c8e18f41c5d9e76ef3fdb953de52301cd6fe Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Sun, 14 Aug 2022 20:49:03 -0700 Subject: [PATCH 33/54] fix binary search left bug 1's and l's look too similar :sob: --- src/databases.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 2cbe662f..18621503 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -388,14 +388,12 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, int mid = left + (right - left) / 2; CatalogStar s = c[mid]; Vec3 diff = s.spatial - point; - // std::cout << "INDEX " << mid << " CATALOG STAR " << s.spatial.x << ", " << s.spatial.y << ", " << s.spatial.z << std::endl; if (diff.Magnitude() <= radius) { index = mid; - // std::cout << "HERE " << std::endl; break; } else if (s.spatial.x < point.x) { - left += mid; + left = mid + 1; } else { right = mid - 1; } From bee762cc3af16ea4cb5f9f7595fb196664671b18 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 15 Aug 2022 10:15:45 -0700 Subject: [PATCH 34/54] add check for not comparing the same catalog star --- src/star-id.cpp | 47 +++++++++++++++++++++++------------------------ 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 5422d719..964a6ce5 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -553,8 +553,10 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { + std::cout << "i: " << i << std::endl; + // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); @@ -562,7 +564,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( if (debugQuery) break; for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); @@ -575,38 +577,35 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); + if (starAChanges.catalogIndex == starBChanges.catalogIndex) continue; + // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); - Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAPossiblePrevStars[k]].spatial, catalog[starBPossiblePrevStars[l]].spatial); - - + Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); Mat3 dcmRot = prevFrame*possibleFrame.Transpose(); - if (!(abs(dcmRot.Column(0).Magnitude()-1) < 0.001)) { - std::cout << "impossible" << std::endl; - } else { - // std::cout << "okay" << std::endl; - - Quaternion rot = DCMToQuaternion(dcmRot); - - // vote for the quaternion - bool found = false; - for (auto& pair : votes) { - if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second.push_back(starAChanges); - pair.second.push_back(starBChanges); - found = true; - break; - } - } - if (!found) { - votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + Quaternion rot = DCMToQuaternion(dcmRot); + + // vote for the quaternion + bool found = false; + for (auto& pair : votes) { + if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + pair.second.push_back(starAChanges); + pair.second.push_back(starBChanges); + found = true; + break; } } + if (!found) { + votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + } } } } } +std::cout << "Done with voting" << std::endl; + + // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) Quaternion votedRot = {0,0,0,0}; std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); From b71ab211964e568e4eb7355255475fef9c9c1df4 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 15 Aug 2022 21:03:40 -0700 Subject: [PATCH 35/54] small code cleanup --- src/io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/io.cpp b/src/io.cpp index 9507cbfe..86c543a2 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -709,9 +709,9 @@ Pipeline SetPipeline(const PipelineOptions &values) { attInputs.push_back(stof(item)); } - // convert ra, dec, and roll to quaternion attitude + // convert ra, dec, and roll to attitude Quaternion q = SphericalToQuaternion(DegToRad(attInputs[0]), DegToRad(attInputs[1]), DegToRad(attInputs[2])); - Attitude a = Attitude(q); + Attitude a(q); // set prev attitude and id algo PrevAttitude prev(a, values.uncertainty, values.trackingCompareThreshold); From 43b8e9fc03a97cdbb5524d693aafea869e448a53 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 15 Aug 2022 23:03:08 -0700 Subject: [PATCH 36/54] fix default value for compare threshold --- src/pipeline-options.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index 44d3fa79..54ea2476 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -26,7 +26,7 @@ LOST_CLI_OPTION("database" , std::string, databasePath LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("uncertainty " , float , uncertainty , 5 , atof(optarg) , kNoDefaultArgument) -LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, -1 , atof(optarg) , 0.0001) +LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, 0.05 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("max-mismatch-probability" , float , maxMismatchProb , .001, atof(optarg) , kNoDefaultArgument) From d7898570c3c8dcde1e0baaf3d7894f48afd65313 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 15 Aug 2022 23:41:02 -0700 Subject: [PATCH 37/54] fixing bugs in database I was doing a binary search through the catalog instead of the sorted indices list also has a temp fix cuz sometimes the binary search comes up blank when it should have something --- src/databases.cpp | 96 +++++++++++++++++++++++++++++++++-------------- 1 file changed, 68 insertions(+), 28 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 18621503..06272157 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -360,7 +360,7 @@ void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { assert(buffer - bufferStart == SerializeLengthTrackingCatalog(catalog)); } - +// deserialize database TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { length = *(int16_t *)buffer; @@ -373,21 +373,23 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } } -std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, const Vec3 point, float radius) { +// query database +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius) { assert(radius >= 0); std::vector query_ind; // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) - int left = 0; - int right = indices.size()-1; - int index = -1; + int16_t left = 0; + int16_t right = length-1; + int16_t index = -1; while (left <= right) { - int mid = left + (right - left) / 2; - CatalogStar s = c[mid]; + int16_t mid = left + (right - left) / 2; + CatalogStar s = catalog[indices[mid]]; Vec3 diff = s.spatial - point; + // std::cout << mid << std::endl; if (diff.Magnitude() <= radius) { index = mid; @@ -399,31 +401,43 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, } } - left = index; - right = index+1; + if (index != -1) { + left = index; + right = index+1; - // see how far left you can go - CatalogStar sLeft = c[left]; - Vec3 diffLeft = sLeft.spatial - point; - while (left > 0 && (abs(diffLeft.x) <= radius)) { - if (diffLeft.Magnitude() <= radius) { - query_ind.push_back(left); + // see how far left you can go + CatalogStar sLeft = catalog[indices[left]]; + Vec3 diffLeft = sLeft.spatial - point; + while (left >= 0 && (abs(diffLeft.x) <= radius)) { + if (diffLeft.Magnitude() <= radius) { + query_ind.push_back(indices[left]); + } + left--; + sLeft = catalog[indices[left]]; + diffLeft = sLeft.spatial - point; } - left--; - sLeft = c[left]; - diffLeft = sLeft.spatial - point; - } - // see how far right you can go - CatalogStar sRight = c[right]; - Vec3 diffRight = sRight.spatial - point; - while (right < (int)c.size() && (abs(diffRight.x) <= radius)) { - if (diffRight.Magnitude() <= radius) { - query_ind.push_back(right); + // see how far right you can go + CatalogStar sRight = catalog[indices[right]]; + Vec3 diffRight = sRight.spatial - point; + while (right < length && (abs(diffRight.x) <= radius)) { + if (diffRight.Magnitude() <= radius) { + query_ind.push_back(indices[right]); + } + right++; + sRight = catalog[indices[right]]; + diffRight = sRight.spatial - point; + } + } else { + for (int i = 0; i < length; i++) { + CatalogStar cstar = catalog[i]; + Vec3 diff = cstar.spatial - point; + if (diff.Magnitude() <= radius) { + query_ind.push_back(i); + // std::cout << "CORRECT : " << i << std::endl; + } } - right++; - sRight = c[right]; - diffRight = sRight.spatial - point; + } // std::cout << query_ind.size() << std::endl; @@ -433,6 +447,32 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog c, // std::cout << query_ind.size() << std::endl; + + std::vector correct_query_ind; + for (int i = 0; i < (int)catalog.size(); i++) { + CatalogStar cstar = catalog[i]; + Vec3 diff = cstar.spatial - point; + if (diff.Magnitude() <= radius) { + correct_query_ind.push_back(i); + // std::cout << "CORRECT : " << i << std::endl; + } + } + + // for (int i = 0; i < query_ind.size(); i++) + + // assert(query_ind == correct_query_ind); + + // std::cout << query_ind.size() << std::endl; + // std::cout << "=============" << std::endl; + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + assert(query_ind.size() == correct_query_ind.size()); + for (int i = 0; i < query_ind.size(); i++) { + assert(query_ind[i] == correct_query_ind[i]); + } + + return query_ind; } From 632647c381dd70cb96e5bec3f33a71fb65ab2ca6 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 15 Aug 2022 23:41:50 -0700 Subject: [PATCH 38/54] skip pairs that are close by, impossible rotations assuming rotations should preserve distance between stars --- src/star-id.cpp | 80 +++++++++++++++++++++++++++++++++++-------------- 1 file changed, 58 insertions(+), 22 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 964a6ce5..51687f58 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -550,23 +550,35 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( std::map votes; + + // int max = log(stars.size() - 1); + + // EulerAngles spherical = prevAttitude.prev.ToSpherical(); + // std::cout << "attitude_ra " << RadToDeg(spherical.ra) << std::endl; + // std::cout << "attitude_de " << RadToDeg(spherical.de) << std::endl; + // std::cout << "attitude_roll " << RadToDeg(spherical.roll) << std::endl; + // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { - std::cout << "i: " << i << std::endl; + // std::cout << "i: " << i << std::endl; // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); - if (debugQuery) break; + if (debugQuery && i > 0) break; for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); + // skip stars that are close to each other + float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); + if (prevDist <= prevAttitude.uncertainty) continue; + std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); // vote for the rotation that every pair makes @@ -576,34 +588,60 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // get the changes StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); + float possibleDist = (catalog[starAChanges.catalogIndex].spatial - catalog[starBChanges.catalogIndex].spatial).Magnitude(); - if (starAChanges.catalogIndex == starBChanges.catalogIndex) continue; + // don't vote for impossible rotations + if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= 0.001)) continue; + // if (starAChanges.catalogIndex == starBChanges.catalogIndex) continue; // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); + + Mat3 dcmRot = prevFrame*possibleFrame.Transpose(); - Quaternion rot = DCMToQuaternion(dcmRot); - - // vote for the quaternion - bool found = false; - for (auto& pair : votes) { - if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second.push_back(starAChanges); - pair.second.push_back(starBChanges); - found = true; - break; + + if (!(abs(dcmRot.Column(0).Magnitude()-1) < 0.001)) { + std::cout << "STAR A: Catalog is " << starAChanges.catalogIndex << " and stars index is " << starAChanges.starIndex << std::endl; + std::cout << "STAR B: Catalog is " << starBChanges.catalogIndex << " and stars index is " << starBChanges.starIndex << std::endl; + std::cout << "MATRIX prevFrame: " << std::endl; + std::cout << prevFrame.At(0,0) << ", " << prevFrame.At(0,1) << ", " << prevFrame.At(0,2) << ", " << std::endl; + std::cout << prevFrame.At(1,0) << ", " << prevFrame.At(1,1) << ", " << prevFrame.At(1,2) << ", " << std::endl; + std::cout << prevFrame.At(2,0) << ", " << prevFrame.At(2,1) << ", " << prevFrame.At(2,2) << ", " << std::endl; + std::cout << "MATRIX possibleFrame: " << std::endl; + std::cout << possibleFrame.At(0,0) << ", " << possibleFrame.At(0,1) << ", " << possibleFrame.At(0,2) << ", " << std::endl; + std::cout << possibleFrame.At(1,0) << ", " << possibleFrame.At(1,1) << ", " << possibleFrame.At(1,2) << ", " << std::endl; + std::cout << possibleFrame.At(2,0) << ", " << possibleFrame.At(2,1) << ", " << possibleFrame.At(2,2) << ", " << std::endl; + std::cout << "MATRIX: " << std::endl; + std::cout << dcmRot.At(0,0) << ", " << dcmRot.At(0,1) << ", " << dcmRot.At(0,2) << ", " << std::endl; + std::cout << dcmRot.At(1,0) << ", " << dcmRot.At(1,1) << ", " << dcmRot.At(1,2) << ", " << std::endl; + std::cout << dcmRot.At(2,0) << ", " << dcmRot.At(2,1) << ", " << dcmRot.At(2,2) << ", " << std::endl; + std::cout << "======================" << std::endl; + + } else { + Quaternion rot = DCMToQuaternion(dcmRot); + + // vote for the quaternion + bool found = false; + for (auto& pair : votes) { + if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + pair.second.push_back(starAChanges); + pair.second.push_back(starBChanges); + found = true; + break; + } } - } - if (!found) { - votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + if (!found) { + votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + } + } } } } } -std::cout << "Done with voting" << std::endl; + std::cout << "Done with voting" << std::endl; // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) @@ -614,11 +652,9 @@ std::cout << "Done with voting" << std::endl; if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); votedRot = currentEntry->first; - std::cout << currentEntry->second.size() << std::endl; + std::cout << entryWithMaxValue.second.size() << std::endl; } } - - // std::cout << entryWithMaxValue.second.size() << std::endl; for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { identified.push_back(entryWithMaxValue.second[i]); From e252f038976b074df773c9ddfce2cda69e136854 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 17 Aug 2022 14:17:02 -0700 Subject: [PATCH 39/54] angle between two vectors function in Vec3 class --- src/attitude-utils.cpp | 6 ++++++ src/attitude-utils.hpp | 1 + 2 files changed, 7 insertions(+) diff --git a/src/attitude-utils.cpp b/src/attitude-utils.cpp index d61bd0cc..0c01263d 100644 --- a/src/attitude-utils.cpp +++ b/src/attitude-utils.cpp @@ -196,6 +196,12 @@ Vec3 Vec3::crossProduct(const Vec3 &other) const { }; } +// angle between two vectors (rad) +float Vec3::Angle(const Vec3 &other) const { + return acos((*this) * other) / (this->Magnitude() * other.Magnitude()); +} + + float Mat3::At(int i, int j) const { return x[3*i+j]; } diff --git a/src/attitude-utils.hpp b/src/attitude-utils.hpp index 083c51a5..af5390a1 100644 --- a/src/attitude-utils.hpp +++ b/src/attitude-utils.hpp @@ -38,6 +38,7 @@ class Vec3 { Vec3 operator*(const float &) const; Vec3 operator-(const Vec3 &) const; Vec3 crossProduct(const Vec3 &) const; + float Angle(const Vec3 &) const; }; class Mat3 { From 7c93c23a5b9c86874ea7619a5fdebce1f79fe33c Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 17 Aug 2022 14:17:59 -0700 Subject: [PATCH 40/54] create list of stars definitely sure about (where the query returns 1 star) also more debugging code to take out later --- src/star-id.cpp | 53 +++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 51687f58..e75f5bc6 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -541,6 +541,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { StarIdentifiers identified; + StarIdentifiers definite; MultiDatabase multiDatabase(database); const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue); if (databaseBuffer == NULL) { @@ -553,10 +554,10 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // int max = log(stars.size() - 1); - // EulerAngles spherical = prevAttitude.prev.ToSpherical(); - // std::cout << "attitude_ra " << RadToDeg(spherical.ra) << std::endl; - // std::cout << "attitude_de " << RadToDeg(spherical.de) << std::endl; - // std::cout << "attitude_roll " << RadToDeg(spherical.roll) << std::endl; + EulerAngles spherical = prevAttitude.prev.ToSpherical(); + std::cout << "attitude_ra " << RadToDeg(spherical.ra) << std::endl; + std::cout << "attitude_de " << RadToDeg(spherical.de) << std::endl; + std::cout << "attitude_roll " << RadToDeg(spherical.roll) << std::endl; // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { @@ -564,15 +565,18 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // std::cout << "i: " << i << std::endl; // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); + if (starAPossiblePrevStars.size() == 1) { + definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); + } if (debugQuery && i > 0) break; for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); // skip stars that are close to each other @@ -580,6 +584,9 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( if (prevDist <= prevAttitude.uncertainty) continue; std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); + if (starBPossiblePrevStars.size() == 1) { + definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); + } // vote for the rotation that every pair makes for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { @@ -591,14 +598,12 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( float possibleDist = (catalog[starAChanges.catalogIndex].spatial - catalog[starBChanges.catalogIndex].spatial).Magnitude(); // don't vote for impossible rotations - if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= 0.001)) continue; + if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= prevAttitude.uncertainty)) continue; // if (starAChanges.catalogIndex == starBChanges.catalogIndex) continue; // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); - Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); - - + Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); Mat3 dcmRot = prevFrame*possibleFrame.Transpose(); if (!(abs(dcmRot.Column(0).Magnitude()-1) < 0.001)) { @@ -649,6 +654,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { + // if (currentEntry->second.size() == 8) { if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); votedRot = currentEntry->first; @@ -656,8 +662,33 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } } + std::cout << entryWithMaxValue.second.size() << std::endl; + + + Quaternion quat = entryWithMaxValue.first; + Attitude att(quat); + EulerAngles identifiedAtt = att.ToSpherical(); + std::cout << "attitude_ra " << RadToDeg(identifiedAtt.ra) << std::endl; + std::cout << "attitude_de " << RadToDeg(identifiedAtt.de) << std::endl; + std::cout << "attitude_roll " << RadToDeg(identifiedAtt.roll) << std::endl; + + // Quaternion currentAtt = prevAttitude.prev.Rotate(quat); + + for (int i = 0; i < (int)definite.size(); i++) { + identified.push_back(definite[i]); + } + for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { - identified.push_back(entryWithMaxValue.second[i]); + bool found = false; + for (int j = 0; j < (int)definite.size(); j++) { + if (entryWithMaxValue.second[i].catalogIndex == definite[j].catalogIndex) { + found = true; + break; + } + } + if (!found) { + identified.push_back(entryWithMaxValue.second[i]); + } } return identified; From e564591b49b4954a1445e2d9add544c67843e173 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Wed, 17 Aug 2022 15:05:34 -0700 Subject: [PATCH 41/54] no more angles --- src/attitude-utils.cpp | 6 ------ src/attitude-utils.hpp | 1 - 2 files changed, 7 deletions(-) diff --git a/src/attitude-utils.cpp b/src/attitude-utils.cpp index 0c01263d..d61bd0cc 100644 --- a/src/attitude-utils.cpp +++ b/src/attitude-utils.cpp @@ -196,12 +196,6 @@ Vec3 Vec3::crossProduct(const Vec3 &other) const { }; } -// angle between two vectors (rad) -float Vec3::Angle(const Vec3 &other) const { - return acos((*this) * other) / (this->Magnitude() * other.Magnitude()); -} - - float Mat3::At(int i, int j) const { return x[3*i+j]; } diff --git a/src/attitude-utils.hpp b/src/attitude-utils.hpp index af5390a1..083c51a5 100644 --- a/src/attitude-utils.hpp +++ b/src/attitude-utils.hpp @@ -38,7 +38,6 @@ class Vec3 { Vec3 operator*(const float &) const; Vec3 operator-(const Vec3 &) const; Vec3 crossProduct(const Vec3 &) const; - float Angle(const Vec3 &) const; }; class Mat3 { From 119bc2efee71efd9a12442f770dece3b2fabf80b Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 09:52:16 -0700 Subject: [PATCH 42/54] bug fix in generated pipeline input expected stars already fixed in master but that was before I made this branch --- src/io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/io.cpp b/src/io.cpp index 86c543a2..78c2f2ca 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -484,8 +484,8 @@ GeneratedPipelineInput::GeneratedPipelineInput(const Catalog &catalog, // don't add false stars to centroids or star ids if (isTrueStar) { - starIds.push_back(StarIdentifier(stars.size() - 1, i)); stars.push_back(star); + starIds.push_back(StarIdentifier(stars.size() - 1, i)); } } } From f956e3fe6ae6f794130270f9413587133fe2deaa Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 10:46:56 -0700 Subject: [PATCH 43/54] working version!!!! --- src/databases.cpp | 36 +++++++++++++-------------- src/databases.hpp | 2 +- src/star-id.cpp | 62 ++++++++++++++++++++--------------------------- 3 files changed, 44 insertions(+), 56 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 06272157..fc0b4bde 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -374,9 +374,11 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } // query database -std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius) { +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius, float threshold) { assert(radius >= 0); - std::vector query_ind; + radius += threshold; + + std::vector query_ind; // the list of catalog indeces to be returned // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; @@ -432,48 +434,44 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat for (int i = 0; i < length; i++) { CatalogStar cstar = catalog[i]; Vec3 diff = cstar.spatial - point; + if (diff.Magnitude() <= radius) { query_ind.push_back(i); // std::cout << "CORRECT : " << i << std::endl; } + } } - // std::cout << query_ind.size() << std::endl; - // for (int i = 0; i < (int)query_ind.size(); i++) { - // std::cout << "i " << query_ind[i] << std::endl; - // } - - // std::cout << query_ind.size() << std::endl; + // CatalogStar correct = catalog[1679]; + // CatalogStar identified = catalog[6536]; + // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; + // std::cout << "correct " << correct.spatial.x << ", " << correct.spatial.y << ", " << correct.spatial.z << std::endl; + // std::cout << "identified " << identified.spatial.x << ", " << identified.spatial.y << ", " << identified.spatial.z << std::endl; std::vector correct_query_ind; for (int i = 0; i < (int)catalog.size(); i++) { CatalogStar cstar = catalog[i]; Vec3 diff = cstar.spatial - point; - if (diff.Magnitude() <= radius) { + if (diff.Magnitude() <= radius + threshold) { correct_query_ind.push_back(i); // std::cout << "CORRECT : " << i << std::endl; } } - // for (int i = 0; i < query_ind.size(); i++) + // std::cout << "QUERY" << std::endl; + // std::cout << correct_query_ind.size() << std::endl; + // std::cout << correct_query_ind[0] << std::endl; - // assert(query_ind == correct_query_ind); - - // std::cout << query_ind.size() << std::endl; // std::cout << "=============" << std::endl; std::sort(query_ind.begin(), query_ind.end()); std::sort(correct_query_ind.begin(), correct_query_ind.end()); - assert(query_ind.size() == correct_query_ind.size()); - for (int i = 0; i < query_ind.size(); i++) { - assert(query_ind[i] == correct_query_ind[i]); - } - + // assert(query_ind == correct_query_ind); - return query_ind; + return correct_query_ind; } } diff --git a/src/databases.hpp b/src/databases.hpp index ef2ab5d9..d4d152ca 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,7 +89,7 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t kMagicValue = 0x2536f0A9; - std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius); + std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius, float threshold); private: int16_t length; // length of catalog diff --git a/src/star-id.cpp b/src/star-id.cpp index e75f5bc6..e8a3c8f0 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -491,6 +491,9 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( PyramidIdentifyRemainingStars(&identified, stars, catalog, vectorDatabase, camera, tolerance); printf("Identified an additional %d stars\n", (int)identified.size() - 4); + for (int i = 0; i < (int)identified.size(); i++) { + std::cout << "STAR: " << identified[i].starIndex << ", " << identified[i].catalogIndex << std::endl; + } return identified; } @@ -536,6 +539,7 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { } #define debugQuery 0 +#define query_threshold 0 StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -551,39 +555,36 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( std::map votes; - // int max = log(stars.size() - 1); - EulerAngles spherical = prevAttitude.prev.ToSpherical(); - std::cout << "attitude_ra " << RadToDeg(spherical.ra) << std::endl; - std::cout << "attitude_de " << RadToDeg(spherical.de) << std::endl; - std::cout << "attitude_roll " << RadToDeg(spherical.roll) << std::endl; - // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { // std::cout << "i: " << i << std::endl; // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position); - starAPrevPos = prevAttitude.prev.Rotate(starAPrevPos); + Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); + starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); + // find all the possible previous stars - std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty); + std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty, prevAttitude.compareThreshold); if (starAPossiblePrevStars.size() == 1) { + std::cout << "HERE " << i << " " << starAPossiblePrevStars[0] << std::endl; definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); + // break; } - if (debugQuery && i > 0) break; + if (debugQuery && i >= query_threshold) continue; for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position); - starBPrevPos = prevAttitude.prev.Rotate(starBPrevPos); + Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); + starBPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starBPrevPos); // skip stars that are close to each other float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); if (prevDist <= prevAttitude.uncertainty) continue; - std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty); + std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty, prevAttitude.compareThreshold); if (starBPossiblePrevStars.size() == 1) { definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); } @@ -654,7 +655,6 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); std::map::iterator currentEntry; for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { - // if (currentEntry->second.size() == 8) { if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); votedRot = currentEntry->first; @@ -664,32 +664,22 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( std::cout << entryWithMaxValue.second.size() << std::endl; - - Quaternion quat = entryWithMaxValue.first; - Attitude att(quat); - EulerAngles identifiedAtt = att.ToSpherical(); - std::cout << "attitude_ra " << RadToDeg(identifiedAtt.ra) << std::endl; - std::cout << "attitude_de " << RadToDeg(identifiedAtt.de) << std::endl; - std::cout << "attitude_roll " << RadToDeg(identifiedAtt.roll) << std::endl; - - // Quaternion currentAtt = prevAttitude.prev.Rotate(quat); - for (int i = 0; i < (int)definite.size(); i++) { identified.push_back(definite[i]); } - for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { - bool found = false; - for (int j = 0; j < (int)definite.size(); j++) { - if (entryWithMaxValue.second[i].catalogIndex == definite[j].catalogIndex) { - found = true; - break; - } - } - if (!found) { - identified.push_back(entryWithMaxValue.second[i]); - } - } + // for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { + // bool found = false; + // for (int j = 0; j < (int)definite.size(); j++) { + // if (entryWithMaxValue.second[i].catalogIndex == definite[j].catalogIndex) { + // found = true; + // break; + // } + // } + // if (!found) { + // identified.push_back(entryWithMaxValue.second[i]); + // } + // } return identified; } From ba7623330a377242eec72e1ea1e53bc89a53c3bc Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 11:30:15 -0700 Subject: [PATCH 44/54] some code cleanup --- src/star-id.cpp | 94 ++++++++++++++----------------------------------- 1 file changed, 27 insertions(+), 67 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index e8a3c8f0..b4fa0a23 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include "star-id.hpp" @@ -507,7 +508,7 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( return identified; } -// for ordering Quaternions in votes map in tracking mode +// for ordering Quaternions in tracking mode votes map bool operator<(const Quaternion& l, const Quaternion& r) { if (l.real < r.real) return true; if (l.i < r.i) return true; @@ -516,11 +517,12 @@ bool operator<(const Quaternion& l, const Quaternion& r) { } // for tracking mode rotation equality -bool QuatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { +bool TrackingQuatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { return (l * r.Conjugate()).Angle() < threshold; } -bool vec3Equals(const Vec3& l, const Vec3& r, const float threshold) { +// for tracking mode vector equality +bool TrackingVec3Equals(const Vec3& l, const Vec3& r, const float threshold) { if (abs(l.x - r.x) > threshold) return false; if (abs(l.y - r.y) > threshold) return false; return abs(l.z - r.z) > threshold; @@ -545,7 +547,6 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { StarIdentifiers identified; - StarIdentifiers definite; MultiDatabase multiDatabase(database); const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue); if (databaseBuffer == NULL) { @@ -553,15 +554,12 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( } TrackingSortedDatabase vectorDatabase(databaseBuffer); + StarIdentifiers definite; std::map votes; - // int max = log(stars.size() - 1); - // vote for each rotation that would make each pair of stars go from the old attitude to the current position for (int i = 0; i < (int)stars.size(); i++) { - // std::cout << "i: " << i << std::endl; - // find previous position of the centroid based on the old attitude Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); @@ -569,9 +567,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // find all the possible previous stars std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty, prevAttitude.compareThreshold); if (starAPossiblePrevStars.size() == 1) { - std::cout << "HERE " << i << " " << starAPossiblePrevStars[0] << std::endl; definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); - // break; } if (debugQuery && i >= query_threshold) continue; @@ -600,56 +596,30 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( // don't vote for impossible rotations if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= prevAttitude.uncertainty)) continue; - // if (starAChanges.catalogIndex == starBChanges.catalogIndex) continue; // calculate the rotation (using triad attitude estimation method) Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); - Mat3 dcmRot = prevFrame*possibleFrame.Transpose(); - - if (!(abs(dcmRot.Column(0).Magnitude()-1) < 0.001)) { - std::cout << "STAR A: Catalog is " << starAChanges.catalogIndex << " and stars index is " << starAChanges.starIndex << std::endl; - std::cout << "STAR B: Catalog is " << starBChanges.catalogIndex << " and stars index is " << starBChanges.starIndex << std::endl; - std::cout << "MATRIX prevFrame: " << std::endl; - std::cout << prevFrame.At(0,0) << ", " << prevFrame.At(0,1) << ", " << prevFrame.At(0,2) << ", " << std::endl; - std::cout << prevFrame.At(1,0) << ", " << prevFrame.At(1,1) << ", " << prevFrame.At(1,2) << ", " << std::endl; - std::cout << prevFrame.At(2,0) << ", " << prevFrame.At(2,1) << ", " << prevFrame.At(2,2) << ", " << std::endl; - std::cout << "MATRIX possibleFrame: " << std::endl; - std::cout << possibleFrame.At(0,0) << ", " << possibleFrame.At(0,1) << ", " << possibleFrame.At(0,2) << ", " << std::endl; - std::cout << possibleFrame.At(1,0) << ", " << possibleFrame.At(1,1) << ", " << possibleFrame.At(1,2) << ", " << std::endl; - std::cout << possibleFrame.At(2,0) << ", " << possibleFrame.At(2,1) << ", " << possibleFrame.At(2,2) << ", " << std::endl; - std::cout << "MATRIX: " << std::endl; - std::cout << dcmRot.At(0,0) << ", " << dcmRot.At(0,1) << ", " << dcmRot.At(0,2) << ", " << std::endl; - std::cout << dcmRot.At(1,0) << ", " << dcmRot.At(1,1) << ", " << dcmRot.At(1,2) << ", " << std::endl; - std::cout << dcmRot.At(2,0) << ", " << dcmRot.At(2,1) << ", " << dcmRot.At(2,2) << ", " << std::endl; - std::cout << "======================" << std::endl; - - } else { - Quaternion rot = DCMToQuaternion(dcmRot); - - // vote for the quaternion - bool found = false; - for (auto& pair : votes) { - if (QuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second.push_back(starAChanges); - pair.second.push_back(starBChanges); - found = true; - break; - } - } - if (!found) { - votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); + + // vote for the quaternion + bool found = false; + for (auto& pair : votes) { + if (TrackingQuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + pair.second.push_back(starAChanges); + pair.second.push_back(starBChanges); + found = true; + break; } - + } + if (!found) { + votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); } } } } } - std::cout << "Done with voting" << std::endl; - - // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) Quaternion votedRot = {0,0,0,0}; std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); @@ -658,29 +628,19 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( if (currentEntry->second.size() > entryWithMaxValue.second.size()) { entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); votedRot = currentEntry->first; - std::cout << entryWithMaxValue.second.size() << std::endl; } } - std::cout << entryWithMaxValue.second.size() << std::endl; - - for (int i = 0; i < (int)definite.size(); i++) { - identified.push_back(definite[i]); + // don't overwrite stars + identified = definite; + for (StarIdentifier& s : entryWithMaxValue.second) { + if (std::find_if(identified.begin(), identified.end(), [s](StarIdentifier const& id){ + return id.starIndex == s.starIndex; + }) == identified.end()) { + identified.push_back(s); + }; } - // for (int i = 0; i < (int)entryWithMaxValue.second.size(); i++) { - // bool found = false; - // for (int j = 0; j < (int)definite.size(); j++) { - // if (entryWithMaxValue.second[i].catalogIndex == definite[j].catalogIndex) { - // found = true; - // break; - // } - // } - // if (!found) { - // identified.push_back(entryWithMaxValue.second[i]); - // } - // } - return identified; } From 77b4198776a774fbbeb0806449988f7b75fa542b Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 11:53:13 -0700 Subject: [PATCH 45/54] tracking mode unit tests, clean up query --- src/databases.cpp | 90 +++++++++++++---------------------------------- test/tracking.cpp | 82 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 106 insertions(+), 66 deletions(-) create mode 100644 test/tracking.cpp diff --git a/src/databases.cpp b/src/databases.cpp index fc0b4bde..3f024c80 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -378,9 +378,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat assert(radius >= 0); radius += threshold; - std::vector query_ind; // the list of catalog indeces to be returned - - // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; + std::vector query_ind; // the list of catalog indices to be returned // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) int16_t left = 0; @@ -403,75 +401,35 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat } } - if (index != -1) { - left = index; - right = index+1; - - // see how far left you can go - CatalogStar sLeft = catalog[indices[left]]; - Vec3 diffLeft = sLeft.spatial - point; - while (left >= 0 && (abs(diffLeft.x) <= radius)) { - if (diffLeft.Magnitude() <= radius) { - query_ind.push_back(indices[left]); - } - left--; - sLeft = catalog[indices[left]]; - diffLeft = sLeft.spatial - point; - } - - // see how far right you can go - CatalogStar sRight = catalog[indices[right]]; - Vec3 diffRight = sRight.spatial - point; - while (right < length && (abs(diffRight.x) <= radius)) { - if (diffRight.Magnitude() <= radius) { - query_ind.push_back(indices[right]); - } - right++; - sRight = catalog[indices[right]]; - diffRight = sRight.spatial - point; - } - } else { - for (int i = 0; i < length; i++) { - CatalogStar cstar = catalog[i]; - Vec3 diff = cstar.spatial - point; - - if (diff.Magnitude() <= radius) { - query_ind.push_back(i); - // std::cout << "CORRECT : " << i << std::endl; - } + // now see which other stars are within radius + left = index; + right = index+1; + // see how far left you can go + CatalogStar sLeft = catalog[indices[left]]; + Vec3 diffLeft = sLeft.spatial - point; + while (left >= 0 && (abs(diffLeft.x) <= radius)) { + if (diffLeft.Magnitude() <= radius) { + query_ind.push_back(indices[left]); } - + left--; + sLeft = catalog[indices[left]]; + diffLeft = sLeft.spatial - point; } - - // CatalogStar correct = catalog[1679]; - // CatalogStar identified = catalog[6536]; - // std::cout << "POINT " << point.x << ", " << point.y << ", " << point.z << std::endl; - // std::cout << "correct " << correct.spatial.x << ", " << correct.spatial.y << ", " << correct.spatial.z << std::endl; - // std::cout << "identified " << identified.spatial.x << ", " << identified.spatial.y << ", " << identified.spatial.z << std::endl; - - std::vector correct_query_ind; - for (int i = 0; i < (int)catalog.size(); i++) { - CatalogStar cstar = catalog[i]; - Vec3 diff = cstar.spatial - point; - if (diff.Magnitude() <= radius + threshold) { - correct_query_ind.push_back(i); - // std::cout << "CORRECT : " << i << std::endl; + // see how far right you can go + CatalogStar sRight = catalog[indices[right]]; + Vec3 diffRight = sRight.spatial - point; + while (right < length && (abs(diffRight.x) <= radius)) { + if (diffRight.Magnitude() <= radius) { + query_ind.push_back(indices[right]); } + right++; + sRight = catalog[indices[right]]; + diffRight = sRight.spatial - point; } - - // std::cout << "QUERY" << std::endl; - // std::cout << correct_query_ind.size() << std::endl; - // std::cout << correct_query_ind[0] << std::endl; - - // std::cout << "=============" << std::endl; - - std::sort(query_ind.begin(), query_ind.end()); - std::sort(correct_query_ind.begin(), correct_query_ind.end()); - // assert(query_ind == correct_query_ind); - - return correct_query_ind; + + return query_ind; } } diff --git a/test/tracking.cpp b/test/tracking.cpp new file mode 100644 index 00000000..3d1cf2d0 --- /dev/null +++ b/test/tracking.cpp @@ -0,0 +1,82 @@ +#include + +#include "databases.hpp" +#include "io.hpp" +#include "attitude-utils.hpp" + +using namespace lost; + +static unsigned char *BuildTrackingDatabase(const Catalog &catalog, long *length) { + + long dummyLength; + if (length == NULL) length = &dummyLength; + + *length = SerializeLengthTrackingCatalog(catalog); + unsigned char *result = new unsigned char[*length]; + SerializeTrackingCatalog(catalog, result); + return result; +} + +std::vector NaiveQuery(const Catalog catalog, const Vec3 point, float radius, float threshold) { + std::vector correct_query_ind; + for (int i = 0; i < (int)catalog.size(); i++) { + CatalogStar cstar = catalog[i]; + Vec3 diff = cstar.spatial - point; + if (diff.Magnitude() <= radius + threshold) { + correct_query_ind.push_back(i); + } + } + return correct_query_ind; +} + +TEST_CASE("Tracking mode database", "[tracking]") { + long length; + Catalog &catalog = CatalogRead(); + unsigned char *dbBytes = BuildTrackingDatabase(catalog, &length); + REQUIRE(length < 999999); + TrackingSortedDatabase db(dbBytes); + + SECTION("small uncertainty") { + + Vec3 point = catalog[rand() % catalog.size()].spatial; + float radius = 0.001; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind == correct_query_ind); + } + + SECTION("medium uncertainty") { + Vec3 point = catalog[rand() % catalog.size()].spatial; + float radius = 0.05; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind == correct_query_ind); + } + + SECTION("large uncertainty") { + Vec3 point = catalog[rand() % catalog.size()].spatial; + float radius = 1; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind == correct_query_ind); + } +} + From 8b919c2915a388234971e0c7675eb26e11699ed3 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 12:15:45 -0700 Subject: [PATCH 46/54] update to cover catalog --- test/tracking.cpp | 71 ++++++++++++++++++++++++++--------------------- 1 file changed, 40 insertions(+), 31 deletions(-) diff --git a/test/tracking.cpp b/test/tracking.cpp index 3d1cf2d0..ae3983b3 100644 --- a/test/tracking.cpp +++ b/test/tracking.cpp @@ -38,45 +38,54 @@ TEST_CASE("Tracking mode database", "[tracking]") { SECTION("small uncertainty") { - Vec3 point = catalog[rand() % catalog.size()].spatial; - float radius = 0.001; - float threshold = 0.001; - - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); - - std::sort(query_ind.begin(), query_ind.end()); - std::sort(correct_query_ind.begin(), correct_query_ind.end()); - CHECK(query_ind.size() == correct_query_ind.size()); - CHECK(query_ind == correct_query_ind); + for (int i = 0; i < (int)catalog.size(); i++) { + Vec3 point = catalog[i].spatial; + float radius = 0.001; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind == correct_query_ind); + } + } SECTION("medium uncertainty") { - Vec3 point = catalog[rand() % catalog.size()].spatial; - float radius = 0.05; - float threshold = 0.001; - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + for (int i = 0; i < (int)catalog.size(); i++) { + Vec3 point = catalog[i].spatial; + float radius = 0.05; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); - std::sort(query_ind.begin(), query_ind.end()); - std::sort(correct_query_ind.begin(), correct_query_ind.end()); - CHECK(query_ind.size() == correct_query_ind.size()); - CHECK(query_ind == correct_query_ind); + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind == correct_query_ind); + } } SECTION("large uncertainty") { - Vec3 point = catalog[rand() % catalog.size()].spatial; - float radius = 1; - float threshold = 0.001; - - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); - - std::sort(query_ind.begin(), query_ind.end()); - std::sort(correct_query_ind.begin(), correct_query_ind.end()); - CHECK(query_ind.size() == correct_query_ind.size()); - CHECK(query_ind == correct_query_ind); + for (int i = 0; i < (int)catalog.size(); i++) { + Vec3 point = catalog[i].spatial; + float radius = 1; + float threshold = 0.001; + + std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + + std::sort(query_ind.begin(), query_ind.end()); + std::sort(correct_query_ind.begin(), correct_query_ind.end()); + CHECK(query_ind.size() == correct_query_ind.size()); + CHECK(query_ind.size() != 0); + CHECK(query_ind == correct_query_ind); + } } } From 01ba8b9cdc6b679f2f6be0c04c0b0079550a700d Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 17:55:35 -0700 Subject: [PATCH 47/54] fix bug in query I should've just been considering if x is within radius for the starting index since that's what it's sorted by --- src/databases.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 3f024c80..6e41d48c 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -389,9 +389,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat int16_t mid = left + (right - left) / 2; CatalogStar s = catalog[indices[mid]]; Vec3 diff = s.spatial - point; - // std::cout << mid << std::endl; - - if (diff.Magnitude() <= radius) { + if (abs(diff.x) <= radius) { index = mid; break; } else if (s.spatial.x < point.x) { From bbe64d9fe786dabb1ea9242c452e47556d1f7ae8 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 17:58:45 -0700 Subject: [PATCH 48/54] don't pass threshold as parameter, just add it directly when passing radius in --- src/databases.cpp | 5 ++--- src/databases.hpp | 2 +- src/star-id.cpp | 9 +++++---- test/tracking.cpp | 16 ++++++++-------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 6e41d48c..8e0ceba6 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -374,10 +374,9 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } // query database -std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius, float threshold) { +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius) { assert(radius >= 0); - radius += threshold; - + std::vector query_ind; // the list of catalog indices to be returned // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) diff --git a/src/databases.hpp b/src/databases.hpp index d4d152ca..ef2ab5d9 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,7 +89,7 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t kMagicValue = 0x2536f0A9; - std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius, float threshold); + std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius); private: int16_t length; // length of catalog diff --git a/src/star-id.cpp b/src/star-id.cpp index b4fa0a23..6cdafee1 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -541,7 +541,7 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { } #define debugQuery 0 -#define query_threshold 0 +#define query_threshold 5 StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -565,12 +565,13 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); // find all the possible previous stars - std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty, prevAttitude.compareThreshold); + std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); if (starAPossiblePrevStars.size() == 1) { definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); } - if (debugQuery && i >= query_threshold) continue; + // std::cout << "i: " << i << std::endl; + if (debugQuery && i <= query_threshold) continue; for (int j = i+1; j < (int)stars.size()-1; j++) { Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); @@ -580,7 +581,7 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); if (prevDist <= prevAttitude.uncertainty) continue; - std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty, prevAttitude.compareThreshold); + std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); if (starBPossiblePrevStars.size() == 1) { definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); } diff --git a/test/tracking.cpp b/test/tracking.cpp index ae3983b3..d7bf203f 100644 --- a/test/tracking.cpp +++ b/test/tracking.cpp @@ -17,12 +17,12 @@ static unsigned char *BuildTrackingDatabase(const Catalog &catalog, long *length return result; } -std::vector NaiveQuery(const Catalog catalog, const Vec3 point, float radius, float threshold) { +std::vector NaiveQuery(const Catalog catalog, const Vec3 point, float radius) { std::vector correct_query_ind; for (int i = 0; i < (int)catalog.size(); i++) { CatalogStar cstar = catalog[i]; Vec3 diff = cstar.spatial - point; - if (diff.Magnitude() <= radius + threshold) { + if (diff.Magnitude() <= radius) { correct_query_ind.push_back(i); } } @@ -43,8 +43,8 @@ TEST_CASE("Tracking mode database", "[tracking]") { float radius = 0.001; float threshold = 0.001; - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + std::vector query_ind = db.QueryNearestStars(catalog, point, radius+threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius+threshold); std::sort(query_ind.begin(), query_ind.end()); std::sort(correct_query_ind.begin(), correct_query_ind.end()); @@ -61,8 +61,8 @@ TEST_CASE("Tracking mode database", "[tracking]") { float radius = 0.05; float threshold = 0.001; - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + std::vector query_ind = db.QueryNearestStars(catalog, point, radius+threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius+threshold); std::sort(query_ind.begin(), query_ind.end()); std::sort(correct_query_ind.begin(), correct_query_ind.end()); @@ -77,8 +77,8 @@ TEST_CASE("Tracking mode database", "[tracking]") { float radius = 1; float threshold = 0.001; - std::vector query_ind = db.QueryNearestStars(catalog, point, radius, threshold); - std::vector correct_query_ind = NaiveQuery(catalog, point, radius, threshold); + std::vector query_ind = db.QueryNearestStars(catalog, point, radius+threshold); + std::vector correct_query_ind = NaiveQuery(catalog, point, radius+threshold); std::sort(query_ind.begin(), query_ind.end()); std::sort(correct_query_ind.begin(), correct_query_ind.end()); From ce74e1d994d44ae05835809f1b63d8fd38d0ba35 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 18:01:51 -0700 Subject: [PATCH 49/54] change default values for uncertainty and compare threshold --- documentation/pipeline.man | 2 +- src/pipeline-options.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index b5c05188..5d8d6482 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -75,7 +75,7 @@ used in some star id algorithms, to \fItolerance\fP degrees. Defaults to 0.04 de .TP \fB--uncertainty\fP \fIradius\fP -For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 5 if not selected. Must be nonnegative. +For tracking mode. \fIradius\fP gives the boundary for how much the field of view could have rotated. Defaults to 0.001 if not selected. Must be nonnegative. .TP \fB--tracking-compare-threshold\fP \fIthreshold\fP diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index 54ea2476..3f7ee015 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -25,8 +25,8 @@ LOST_CLI_OPTION("centroid-mag-filter" , float , centroidMagFilter LOST_CLI_OPTION("database" , std::string, databasePath , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) -LOST_CLI_OPTION("uncertainty " , float , uncertainty , 5 , atof(optarg) , kNoDefaultArgument) -LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, 0.05 , atof(optarg) , kNoDefaultArgument) +LOST_CLI_OPTION("uncertainty " , float , uncertainty , 0.001 , atof(optarg) , kNoDefaultArgument) +LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, 0.0001 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("max-mismatch-probability" , float , maxMismatchProb , .001, atof(optarg) , kNoDefaultArgument) From 40b8415fe1a68f668284418f35802b82cc40f866 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Thu, 18 Aug 2022 18:16:20 -0700 Subject: [PATCH 50/54] code cleanup --- src/databases.cpp | 11 +++++++---- src/star-id.cpp | 12 +----------- 2 files changed, 8 insertions(+), 15 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 8e0ceba6..68dcf62c 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -317,20 +317,23 @@ MultiDatabaseBuilder::~MultiDatabaseBuilder() { /*** for tracking mode ***/ +// to associate stars with a definite index in the catalog struct TrackingStar { int16_t index; CatalogStar star; }; +// length of the serialized tracking catalog long SerializeLengthTrackingCatalog(const Catalog &catalog) { return (catalog.size()+1) * sizeof(int16_t); } +// comparator for ordering stars in tracking database bool CompareTrackingStars(const TrackingStar &s1, const TrackingStar &s2) { return s1.star.spatial.x < s2.star.spatial.x; } -// sort by x coordinate of stars +// serialize tracking catalog (length of catalog, then list of indices into catalog sorted by x-coordinate of stars) void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { std::vector stars; @@ -373,13 +376,13 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } } -// query database +// query database (returns list of indices into the catalog that have stars within radius of point) std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius) { assert(radius >= 0); - std::vector query_ind; // the list of catalog indices to be returned + std::vector query_ind; - // use binary search to find an initial element within the right range (see https://www.geeksforgeeks.org/binary-search/) + // binary search to find an initial element with x-element in right range int16_t left = 0; int16_t right = length-1; int16_t index = -1; diff --git a/src/star-id.cpp b/src/star-id.cpp index 6cdafee1..4f958ddb 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -491,10 +491,6 @@ StarIdentifiers PyramidStarIdAlgorithm::Go( PyramidIdentifyRemainingStars(&identified, stars, catalog, vectorDatabase, camera, tolerance); printf("Identified an additional %d stars\n", (int)identified.size() - 4); - - for (int i = 0; i < (int)identified.size(); i++) { - std::cout << "STAR: " << identified[i].starIndex << ", " << identified[i].catalogIndex << std::endl; - } return identified; } @@ -540,9 +536,6 @@ static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { }; } -#define debugQuery 0 -#define query_threshold 5 - StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { @@ -570,14 +563,11 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); } - // std::cout << "i: " << i << std::endl; - if (debugQuery && i <= query_threshold) continue; - for (int j = i+1; j < (int)stars.size()-1; j++) { Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); starBPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starBPrevPos); - // skip stars that are close to each other + // skip stars that are close to each other, since that gets confusing float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); if (prevDist <= prevAttitude.uncertainty) continue; From 9e16953fe4e38bdc986448123b6f56c9204b87b8 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 20 Mar 2023 14:33:53 -0700 Subject: [PATCH 51/54] giant shortcut to tracking mode --- src/star-id.cpp | 174 ++++++++++++++++++++++++++++-------------------- 1 file changed, 103 insertions(+), 71 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 4f958ddb..108ff1ff 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -550,87 +550,119 @@ StarIdentifiers TrackingModeStarIdAlgorithm::Go( StarIdentifiers definite; std::map votes; - // vote for each rotation that would make each pair of stars go from the old attitude to the current position + // find all stars that only have 1 possible previous star; so we've definitely id'd them for (int i = 0; i < (int)stars.size(); i++) { + Vec3 prevPos = camera.CameraToSpatial(stars[i].position).Normalize(); + prevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(prevPos); - // find previous position of the centroid based on the old attitude - Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); - starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); + std::vector possiblePrevStars = vectorDatabase.QueryNearestStars(catalog, prevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); + if (possiblePrevStars.size() == 1) { + // definite.push_back(possiblePrevStars[0]); + definite.push_back(StarIdentifier(i,possiblePrevStars[0])); - // find all the possible previous stars - std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); - if (starAPossiblePrevStars.size() == 1) { - definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); } + } - for (int j = i+1; j < (int)stars.size()-1; j++) { - Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); - starBPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starBPrevPos); + std::cout << "tot " << stars.size() << std::endl; + std::cout << "def " << definite.size() << std::endl; - // skip stars that are close to each other, since that gets confusing - float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); - if (prevDist <= prevAttitude.uncertainty) continue; + return definite; - std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); - if (starBPossiblePrevStars.size() == 1) { - definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); - } + // TODO if at least 2 definite, can find the position of the other stars as wel - // vote for the rotation that every pair makes - for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { - for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { - - // get the changes - StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); - StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); - float possibleDist = (catalog[starAChanges.catalogIndex].spatial - catalog[starBChanges.catalogIndex].spatial).Magnitude(); - - // don't vote for impossible rotations - if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= prevAttitude.uncertainty)) continue; - - // calculate the rotation (using triad attitude estimation method) - Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); - Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); - Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); - - // vote for the quaternion - bool found = false; - for (auto& pair : votes) { - if (TrackingQuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - pair.second.push_back(starAChanges); - pair.second.push_back(starBChanges); - found = true; - break; - } - } - if (!found) { - votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); - } - } - } - } - } + // Vec3 starAPrevPos = catalog[definite[0]].spatial; + // Vec3 starBPrevPos = catalog[definite[1]].spatial; - // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) - Quaternion votedRot = {0,0,0,0}; - std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); - std::map::iterator currentEntry; - for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { - if (currentEntry->second.size() > entryWithMaxValue.second.size()) { - entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); - votedRot = currentEntry->first; - } - } + // StarIdentifier starAChanges = StarIdentifier(definite[0], starAPrevPos); + // StarIdentifier starBChanges = StarIdentifier(definite[1], starBPrevPos); - // don't overwrite stars - identified = definite; - for (StarIdentifier& s : entryWithMaxValue.second) { - if (std::find_if(identified.begin(), identified.end(), [s](StarIdentifier const& id){ - return id.starIndex == s.starIndex; - }) == identified.end()) { - identified.push_back(s); - }; - } + + // Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); + // Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); + // Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); + + + // // vote for each rotation that would make each pair of stars go from the old attitude to the current position + // for (int i = 0; i < (int)stars.size(); i++) { + + // // find previous position of the centroid based on the old attitude + // Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); + // starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); + + // // find all the possible previous stars + // std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); + // if (starAPossiblePrevStars.size() == 1) { + // definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); + // } + + // for (int j = i+1; j < (int)stars.size()-1; j++) { + // Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); + // starBPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starBPrevPos); + + // // skip stars that are close to each other, since that gets confusing + // float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); + // if (prevDist <= prevAttitude.uncertainty) continue; + + // std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); + // if (starBPossiblePrevStars.size() == 1) { + // definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); + // } + + // // vote for the rotation that every pair makes + // for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { + // for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { + + // // get the changes + // StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); + // StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); + // float possibleDist = (catalog[starAChanges.catalogIndex].spatial - catalog[starBChanges.catalogIndex].spatial).Magnitude(); + + // // don't vote for impossible rotations + // if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= prevAttitude.uncertainty)) continue; + + // // calculate the rotation (using triad attitude estimation method) + // Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); + // Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); + // Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); + + // // vote for the quaternion + // bool found = false; + // for (auto& pair : votes) { + // if (TrackingQuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { + // pair.second.push_back(starAChanges); + // pair.second.push_back(starBChanges); + // found = true; + // break; + // } + // } + // if (!found) { + // votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); + // } + // } + // } + // } + // } + + // // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) + // Quaternion votedRot = {0,0,0,0}; + // std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); + // std::map::iterator currentEntry; + // for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { + // if (currentEntry->second.size() > entryWithMaxValue.second.size()) { + // entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); + // votedRot = currentEntry->first; + // } + // } + + // // don't overwrite stars + // identified = definite; + // for (StarIdentifier& s : entryWithMaxValue.second) { + // if (std::find_if(identified.begin(), identified.end(), [s](StarIdentifier const& id){ + // return id.starIndex == s.starIndex; + // }) == identified.end()) { + // identified.push_back(s); + // }; + // } return identified; } From 905103bf34c39baefed21768f34724f5fd74de92 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Mon, 20 Mar 2023 14:34:06 -0700 Subject: [PATCH 52/54] change default threshold values --- documentation/pipeline.man | 2 +- src/pipeline-options.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/documentation/pipeline.man b/documentation/pipeline.man index 5d8d6482..251f33f8 100644 --- a/documentation/pipeline.man +++ b/documentation/pipeline.man @@ -79,7 +79,7 @@ For tracking mode. \fIradius\fP gives the boundary for how much the field of vie .TP \fB--tracking-compare-threshold\fP \fIthreshold\fP -\fIthreshold\fP is the threshold for comparing floats for tracking mode. Defaults to 0.0001 if no value is given. +\fIthreshold\fP is the threshold for comparing floats for tracking mode. Defaults to 0.001 if no value is given. .TP \fB--attitude-algo\fP \fIalgo\fP diff --git a/src/pipeline-options.hpp b/src/pipeline-options.hpp index 3f7ee015..0762fa63 100644 --- a/src/pipeline-options.hpp +++ b/src/pipeline-options.hpp @@ -26,7 +26,7 @@ LOST_CLI_OPTION("database" , std::string, databasePath LOST_CLI_OPTION("star-id-algo" , std::string, idAlgo , "" , optarg , "pyramid") LOST_CLI_OPTION("prev-attitude" , std::string, prevAttitudeString , "" , optarg , kNoDefaultArgument) LOST_CLI_OPTION("uncertainty " , float , uncertainty , 0.001 , atof(optarg) , kNoDefaultArgument) -LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, 0.0001 , atof(optarg) , kNoDefaultArgument) +LOST_CLI_OPTION("tracking-compare-threshold", float , trackingCompareThreshold, 0.001 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("angular-tolerance" , float , angularTolerance , .04 , atof(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("false-stars-estimate" , int , estimatedNumFalseStars , 500 , atoi(optarg) , kNoDefaultArgument) LOST_CLI_OPTION("max-mismatch-probability" , float , maxMismatchProb , .001, atof(optarg) , kNoDefaultArgument) From 3e5cbbf03f55080c0bec8ab57ef7d5ada6702a9b Mon Sep 17 00:00:00 2001 From: Edward Zhang Date: Mon, 20 Mar 2023 22:22:34 -0700 Subject: [PATCH 53/54] smol fix, maybe saves quite a bit of copying to/from Catalog --- src/databases.cpp | 10 +++++----- src/databases.hpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/databases.cpp b/src/databases.cpp index 68dcf62c..5fef6244 100644 --- a/src/databases.cpp +++ b/src/databases.cpp @@ -219,7 +219,7 @@ void SerializePairDistanceKVector(const Catalog &catalog, float minDistance, flo PairDistanceKVectorDatabase::PairDistanceKVectorDatabase(const unsigned char *buffer) : index(KVectorIndex(buffer)) { - + // TODO: errors? (not even sure what i meant by this comment anymore) buffer += SerializeLengthKVectorIndex(index.NumBins()); pairs = (const int16_t *)buffer; @@ -345,7 +345,7 @@ void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer) { } std::sort(stars.begin(), stars.end(), CompareTrackingStars); - + // serialize into buffer unsigned char *bufferStart = buffer; @@ -377,7 +377,7 @@ TrackingSortedDatabase::TrackingSortedDatabase(const unsigned char *buffer) { } // query database (returns list of indices into the catalog that have stars within radius of point) -std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog catalog, const Vec3 point, float radius) { +std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog& catalog, const Vec3 point, float radius) { assert(radius >= 0); std::vector query_ind; @@ -388,7 +388,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat int16_t index = -1; while (left <= right) { - int16_t mid = left + (right - left) / 2; + int16_t mid = left + (right - left) / 2; CatalogStar s = catalog[indices[mid]]; Vec3 diff = s.spatial - point; if (abs(diff.x) <= radius) { @@ -428,7 +428,7 @@ std::vector TrackingSortedDatabase::QueryNearestStars(const Catalog cat sRight = catalog[indices[right]]; diffRight = sRight.spatial - point; } - + return query_ind; } diff --git a/src/databases.hpp b/src/databases.hpp index ef2ab5d9..fc29e133 100644 --- a/src/databases.hpp +++ b/src/databases.hpp @@ -89,7 +89,7 @@ class TrackingSortedDatabase { public: TrackingSortedDatabase(const unsigned char *databaseBytes); const static int32_t kMagicValue = 0x2536f0A9; - std::vector QueryNearestStars(const Catalog c, const Vec3 point, float radius); + std::vector QueryNearestStars(const Catalog &, const Vec3 point, float radius); private: int16_t length; // length of catalog From eb95e448939f19574f1b356f4930a15308de6a08 Mon Sep 17 00:00:00 2001 From: Karen Haining Date: Tue, 28 Mar 2023 12:49:05 -0700 Subject: [PATCH 54/54] working tracking mode --- src/star-id.cpp | 136 ++---------------------------------------------- 1 file changed, 3 insertions(+), 133 deletions(-) diff --git a/src/star-id.cpp b/src/star-id.cpp index 108ff1ff..b395486f 100644 --- a/src/star-id.cpp +++ b/src/star-id.cpp @@ -512,159 +512,29 @@ bool operator<(const Quaternion& l, const Quaternion& r) { return (l.k < r.k); } -// for tracking mode rotation equality -bool TrackingQuatEquals(const Quaternion& l, const Quaternion& r, const float threshold) { - return (l * r.Conjugate()).Angle() < threshold; -} - -// for tracking mode vector equality -bool TrackingVec3Equals(const Vec3& l, const Vec3& r, const float threshold) { - if (abs(l.x - r.x) > threshold) return false; - if (abs(l.y - r.y) > threshold) return false; - return abs(l.z - r.z) > threshold; -} - -// return a matrix whose columns are the axes of the frame -static Mat3 TrackingCoordinateFrame(Vec3 v1, Vec3 v2) { - Vec3 d1 = v1.Normalize(); - Vec3 d2 = v1.crossProduct(v2).Normalize(); - Vec3 d3 = d1.crossProduct(d2).Normalize(); - return { - d1.x, d2.x, d3.x, - d1.y, d2.y, d3.y, - d1.z, d2.z, d3.z, - }; -} - StarIdentifiers TrackingModeStarIdAlgorithm::Go( const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const { - StarIdentifiers identified; + StarIdentifiers definite; MultiDatabase multiDatabase(database); const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue); if (databaseBuffer == NULL) { - return identified; + return definite; } TrackingSortedDatabase vectorDatabase(databaseBuffer); - StarIdentifiers definite; - std::map votes; - - // find all stars that only have 1 possible previous star; so we've definitely id'd them + // find all stars that only have 1 possible previous star, so we've definitely id'd them for (int i = 0; i < (int)stars.size(); i++) { Vec3 prevPos = camera.CameraToSpatial(stars[i].position).Normalize(); prevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(prevPos); std::vector possiblePrevStars = vectorDatabase.QueryNearestStars(catalog, prevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); if (possiblePrevStars.size() == 1) { - // definite.push_back(possiblePrevStars[0]); definite.push_back(StarIdentifier(i,possiblePrevStars[0])); - } } - std::cout << "tot " << stars.size() << std::endl; - std::cout << "def " << definite.size() << std::endl; - return definite; - - // TODO if at least 2 definite, can find the position of the other stars as wel - - // Vec3 starAPrevPos = catalog[definite[0]].spatial; - // Vec3 starBPrevPos = catalog[definite[1]].spatial; - - // StarIdentifier starAChanges = StarIdentifier(definite[0], starAPrevPos); - // StarIdentifier starBChanges = StarIdentifier(definite[1], starBPrevPos); - - - // Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); - // Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); - // Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); - - - // // vote for each rotation that would make each pair of stars go from the old attitude to the current position - // for (int i = 0; i < (int)stars.size(); i++) { - - // // find previous position of the centroid based on the old attitude - // Vec3 starAPrevPos = camera.CameraToSpatial(stars[i].position).Normalize(); - // starAPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starAPrevPos); - - // // find all the possible previous stars - // std::vector starAPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starAPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); - // if (starAPossiblePrevStars.size() == 1) { - // definite.push_back(StarIdentifier(i,starAPossiblePrevStars[0])); - // } - - // for (int j = i+1; j < (int)stars.size()-1; j++) { - // Vec3 starBPrevPos = camera.CameraToSpatial(stars[j].position).Normalize(); - // starBPrevPos = prevAttitude.prev.GetQuaternion().Conjugate().Rotate(starBPrevPos); - - // // skip stars that are close to each other, since that gets confusing - // float prevDist = (starAPrevPos - starBPrevPos).Magnitude(); - // if (prevDist <= prevAttitude.uncertainty) continue; - - // std::vector starBPossiblePrevStars = vectorDatabase.QueryNearestStars(catalog, starBPrevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold); - // if (starBPossiblePrevStars.size() == 1) { - // definite.push_back(StarIdentifier(j,starBPossiblePrevStars[0])); - // } - - // // vote for the rotation that every pair makes - // for (int k = 0; k < (int)starAPossiblePrevStars.size(); k++) { - // for (int l = 0; l < (int)starBPossiblePrevStars.size(); l++) { - - // // get the changes - // StarIdentifier starAChanges = StarIdentifier(i, starAPossiblePrevStars[k]); - // StarIdentifier starBChanges = StarIdentifier(j, starBPossiblePrevStars[l]); - // float possibleDist = (catalog[starAChanges.catalogIndex].spatial - catalog[starBChanges.catalogIndex].spatial).Magnitude(); - - // // don't vote for impossible rotations - // if ((possibleDist <= prevAttitude.uncertainty) || (abs(possibleDist - prevDist) >= prevAttitude.uncertainty)) continue; - - // // calculate the rotation (using triad attitude estimation method) - // Mat3 prevFrame = TrackingCoordinateFrame(starAPrevPos, starBPrevPos); - // Mat3 possibleFrame = TrackingCoordinateFrame(catalog[starAChanges.catalogIndex].spatial, catalog[starBChanges.catalogIndex].spatial); - // Quaternion rot = DCMToQuaternion(prevFrame*possibleFrame.Transpose()); - - // // vote for the quaternion - // bool found = false; - // for (auto& pair : votes) { - // if (TrackingQuatEquals(pair.first, rot, prevAttitude.compareThreshold)) { - // pair.second.push_back(starAChanges); - // pair.second.push_back(starBChanges); - // found = true; - // break; - // } - // } - // if (!found) { - // votes.insert(std::make_pair(rot,StarIdentifiers{starAChanges, starBChanges})); - // } - // } - // } - // } - // } - - // // find most-voted difference (https://www.geeksforgeeks.org/how-to-find-the-entry-with-largest-value-in-a-c-map/) - // Quaternion votedRot = {0,0,0,0}; - // std::pair entryWithMaxValue = std::make_pair(votedRot,StarIdentifiers{}); - // std::map::iterator currentEntry; - // for (currentEntry = votes.begin(); currentEntry != votes.end(); ++currentEntry) { - // if (currentEntry->second.size() > entryWithMaxValue.second.size()) { - // entryWithMaxValue = std::make_pair(currentEntry->first, currentEntry->second); - // votedRot = currentEntry->first; - // } - // } - - // // don't overwrite stars - // identified = definite; - // for (StarIdentifier& s : entryWithMaxValue.second) { - // if (std::find_if(identified.begin(), identified.end(), [s](StarIdentifier const& id){ - // return id.starIndex == s.starIndex; - // }) == identified.end()) { - // identified.push_back(s); - // }; - // } - - return identified; } }