LOST 0.0.1
LOST: Open-source Star Tracker
Loading...
Searching...
No Matches
databases.cpp
Go to the documentation of this file.
1#include "databases.hpp"
2
3#include <assert.h>
4#include <math.h>
5#include <stdlib.h>
6#include <algorithm>
7#include <cstdint>
8#include <iostream>
9#include <vector>
10
11#include "attitude-utils.hpp"
12#include "serialize-helpers.hpp"
13#include "star-utils.hpp"
14
15namespace lost {
16
17const int32_t PairDistanceKVectorDatabase::kMagicValue = 0x2536f009;
18const int32_t TetraDatabase::kMagicValue = 0x26683787;
19
20inline bool isFlagSet(uint32_t dbFlags, uint32_t flag) {
21 return (dbFlags & flag) != 0;
22}
23
25 int16_t index1;
26 int16_t index2;
28};
29
30bool CompareKVectorPairs(const KVectorPair &p1, const KVectorPair &p2) {
31 return p1.distance < p2.distance;
32}
33
34// just the index part of the kvector, doesn't store the sorted list it refers to. This makes it
35// flexible whether used to store star distance angles, an angle of a triangle, etc.
48
49// apparently there's no easy way to accept an iterator argument. Hate java all you want, but at
50// least it can do that!
51// https://stackoverflow.com/questions/5054087/declare-a-function-accepting-generic-iterator or
52// rather, the correct way is to use a template and just use the ++ and * operators willy-nilly,
53// which will throw an error if the operators are not implemented.
54
65void SerializeKVectorIndex(SerializeContext *ser, const std::vector<decimal> &values, decimal min, decimal max, long numBins) {
66 std::vector<int32_t> kVector(numBins+1); // We store sums before and after each bin
67 decimal binWidth = (max - min) / numBins;
68
69 // generate the k-vector part
70 // Idea: When we find the first star that's across any bin boundary, we want to update all the newly sealed bins
71 long lastBin = 0; // first bin the last star belonged to
72 for (int32_t i = 0; i < (int32_t)values.size(); i++) {
73 if (i > 0) {
74 assert(values[i] >= values[i-1]);
75 }
76 assert(values[i] >= min);
77 assert(values[i] <= max);
78 long thisBin = (long)ceil((values[i] - min) / binWidth); // first bin we belong to
79 assert(thisBin >= 0);
80 assert(thisBin <= numBins); // thisBin == numBins is acceptable since kvector length == numBins + 1
81 // if thisBin > lastBin, then no more stars can be added to those bins between thisBin and lastBin, so set them.
82 for (long bin = lastBin; bin < thisBin; bin++) {
83 kVector[bin] = i; // our index is the number of pairs with shorter distance
84 }
85 lastBin = thisBin;
86 }
87 for (long bin = lastBin; bin <= numBins; bin++) {
88 kVector[bin] = values.size();
89 }
90
91 // verify kvector
92 int32_t lastBinVal = -1;
93 for (const int32_t &bin : kVector) {
94 assert(bin >= lastBinVal);
95 lastBinVal = bin;
96 }
97
98 // metadata fields
99 SerializePrimitive<int32_t>(ser, values.size());
102 SerializePrimitive<int32_t>(ser, numBins);
103
104 // kvector index field
105 for (const int32_t &bin : kVector) {
107 }
108}
109
112
113 numValues = DeserializePrimitive<int32_t>(des);
116 numBins = DeserializePrimitive<int32_t>(des);
117
118 assert(min >= DECIMAL(0.0));
119 assert(max > min);
120 binWidth = (max - min) / numBins;
121
122 bins = DeserializeArray<int32_t>(des, numBins+1);
123}
124
130long KVectorIndex::QueryLiberal(decimal minQueryDistance, decimal maxQueryDistance, long *upperIndex) const {
131 assert(maxQueryDistance > minQueryDistance);
132 if (maxQueryDistance >= max) {
133 maxQueryDistance = max - DECIMAL(0.00001); // TODO: better way to avoid hitting the bottom bin
134 }
135 if (minQueryDistance <= min) {
136 minQueryDistance = min + DECIMAL(0.00001);
137 }
138 if (minQueryDistance > max || maxQueryDistance < min) {
139 *upperIndex = 0;
140 return 0;
141 }
142 long lowerBin = BinFor(minQueryDistance);
143 long upperBin = BinFor(maxQueryDistance);
144 assert(upperBin >= lowerBin);
145 assert(upperBin <= numBins);
146 // bins[lowerBin-1]=number of pairs <= r < query distance, so it is the index of the
147 // first possible item that might be equal to the query distance
148 int lowerIndex = bins[lowerBin-1];
149 if (lowerIndex >= numValues) {
150 // all pairs have distance less than queried. Return value is irrelevant as long as
151 // numReturned=0
152 return 0;
153 }
154 // bins[upperBin]=number of pairs <= r >= query distance
155 *upperIndex = bins[upperBin];
156 return lowerIndex;
157}
158
160long KVectorIndex::BinFor(decimal query) const {
161 long result = (long)ceil((query - min) / binWidth);
162 assert(result >= 0);
163 assert(result <= numBins);
164 return result;
165}
166
176std::vector<KVectorPair> CatalogToPairDistances(const Catalog &catalog, decimal minDistance, decimal maxDistance) {
177 std::vector<KVectorPair> result;
178 for (int16_t i = 0; i < (int16_t)catalog.size(); i++) {
179 for (int16_t k = i+1; k < (int16_t)catalog.size(); k++) {
180
181 KVectorPair pair = { i, k, AngleUnit(catalog[i].spatial, catalog[k].spatial) };
182 assert(isfinite(pair.distance));
183 assert(pair.distance >= 0);
184 assert(pair.distance <= DECIMAL_M_PI);
185
186 if (pair.distance >= minDistance && pair.distance <= maxDistance) {
187 // we'll sort later
188 result.push_back(pair);
189 }
190 }
191 }
192 return result;
193}
194
199void SerializePairDistanceKVector(SerializeContext *ser, const Catalog &catalog, decimal minDistance, decimal maxDistance, long numBins) {
200 std::vector<int32_t> kVector(numBins+1); // numBins = length, all elements zero
201 std::vector<KVectorPair> pairs = CatalogToPairDistances(catalog, minDistance, maxDistance);
202
203 // sort pairs in increasing order.
204 std::sort(pairs.begin(), pairs.end(), CompareKVectorPairs);
205 std::vector<decimal> distances;
206
207 for (const KVectorPair &pair : pairs) {
208 distances.push_back(pair.distance);
209 }
210
211 // index field
212 SerializeKVectorIndex(ser, distances, minDistance, maxDistance, numBins);
213
214 // bulk pairs field
215 for (const KVectorPair &pair : pairs) {
216 SerializePrimitive<int16_t>(ser, pair.index1);
217 SerializePrimitive<int16_t>(ser, pair.index2);
218 }
219}
220
221std::vector<uint16_t> TetraPreprocessCatalog(const Catalog& catalog,
222 const decimal maxFovDeg,
223 int numPatternStarsPerFov) {
224 // To eliminate ambiguity (double stars), enforce that all
225 // pattern stars must be > 0.05 degrees apart
226 const decimal starMinSepRad = DegToRad(DECIMAL(0.05));
227 const decimal maxFovRad = DegToRad(maxFovDeg);
228
229 // Catalog is sorted brightest first so brighter stars get priority,
230 // banking on the assumption that brighter stars have lower centroiding error
231 std::vector<uint16_t> patternStarInds{0};
232
233 for (size_t i = 1; i < catalog.size(); i++) {
234 const Vec3& vec = catalog[i].spatial;
235
236 // Reject catalog star if it is a double star of an already-kept star, or if there are
237 // already numPatternStarsPerFov kept stars within maxFov/2 of it
238 bool keep = true;
239 int numPatternStarsInFov = 0;
240 for (uint16_t j : patternStarInds) {
241 decimal angle = Angle(vec, catalog[j].spatial);
242 if (angle < starMinSepRad) {
243 keep = false;
244 break;
245 }
246 if (angle < maxFovRad / 2 && ++numPatternStarsInFov >= numPatternStarsPerFov) {
247 keep = false;
248 break;
249 }
250 }
251
252 if (keep) {
253 patternStarInds.push_back(i);
254 }
255 }
256 return patternStarInds;
257}
258
260 maxFov_ = DeserializePrimitive<decimal>(des);
261 numEdgeRatioBins_ = DeserializePrimitive<int32_t>(des);
262 patternTableSize_ = DeserializePrimitive<uint64_t>(des);
263 tetraCatalogSize_ = DeserializePrimitive<uint16_t>(des);
264 patternTable_ =
266 catalogIndices_ = DeserializeArray<uint16_t>(des, tetraCatalogSize_);
267}
268
270 TetraPattern res;
271 for (int i = 0; i < TetraConstants::numPatternStars; i++) {
272 res[i] = patternTable_[TetraConstants::numPatternStars * index + i];
273 }
274 return res;
275}
276
277uint16_t TetraDatabase::CatalogIndex(int tetraCatalogInd) const {
278 return catalogIndices_[tetraCatalogInd];
279}
280
281std::vector<TetraPattern> TetraDatabase::GetPatternMatches(size_t index) const {
282 std::vector<TetraPattern> res;
283 for (size_t probeStep = 0; probeStep < PatternTableSize(); probeStep++) {
284 size_t i = (index + probeStep * probeStep) % PatternTableSize();
285
286 TetraPattern tableRow = GetPattern(i);
287
288 if (tableRow[0] == 0 && tableRow[1] == 0) {
289 break;
290 } else {
291 res.push_back(tableRow);
292 }
293 }
294 return res;
295}
296
299 : index(KVectorIndex(des)) {
300
301 pairs = DeserializeArray<int16_t>(des, 2*index.NumValues());
302}
303
306 return num < low ? low : num > high ? high : num;
307}
308
315 decimal minQueryDistance, decimal maxQueryDistance, const int16_t **end) const {
316
317 assert(maxQueryDistance <= DECIMAL_M_PI);
318
319 long upperIndex = -1;
320 long lowerIndex = index.QueryLiberal(minQueryDistance, maxQueryDistance, &upperIndex);
321 *end = &pairs[upperIndex * 2];
322 return &pairs[lowerIndex * 2];
323}
324
326 decimal minQueryDistance, decimal maxQueryDistance, const int16_t **end) const {
327
328 // Instead of computing the angle for every pair in the database, we pre-compute the /cosines/
329 // of the min and max query distances so that we can compare against dot products directly! As
330 // angle increases, cosine decreases, up to DECIMAL_M_PI (and queries larger than that don't really make
331 // sense anyway)
332 assert(maxQueryDistance <= DECIMAL_M_PI);
333
334 decimal maxQueryCos = DECIMAL_COS(minQueryDistance);
335 decimal minQueryCos = DECIMAL_COS(maxQueryDistance);
336
337 long liberalUpperIndex;
338 long liberalLowerIndex = index.QueryLiberal(minQueryDistance, maxQueryDistance, &liberalUpperIndex);
339 // now we need to find the first and last index that actually matches the query
340 // step the lower index forward
341 // There's no good reason to be using >= and <= for the comparison against max/min, but the tests fail otherwise (because they use angle, with its acos, instead of forward cos like us). It's an insignificant difference.
342 while (liberalLowerIndex < liberalUpperIndex
343 && catalog[pairs[liberalLowerIndex*2]].spatial * catalog[pairs[liberalLowerIndex*2+1]].spatial >= maxQueryCos
344 )
345 { liberalLowerIndex++; }
346
347 // step the upper index backward
348 while (liberalLowerIndex < liberalUpperIndex
349 // the liberalUpperIndex is past the end of the logically returned range, so we need to subtract 1
350 && catalog[pairs[(liberalUpperIndex-1)*2]].spatial * catalog[pairs[(liberalUpperIndex-1)*2+1]].spatial <= minQueryCos
351 )
352 { liberalUpperIndex--; }
353
354 *end = &pairs[liberalUpperIndex * 2];
355 return &pairs[liberalLowerIndex * 2];
356}
357
360 return index.NumValues();
361}
362
364std::vector<decimal> PairDistanceKVectorDatabase::StarDistances(int16_t star, const Catalog &catalog) const {
365 std::vector<decimal> result;
366 for (int i = 0; i < NumPairs(); i++) {
367 if (pairs[i*2] == star || pairs[i*2+1] == star) {
368 result.push_back(AngleUnit(catalog[pairs[i*2]].spatial, catalog[pairs[i*2+1]].spatial));
369 }
370 }
371 return result;
372}
373
375 const Catalog& catalog,
376 decimal maxFovDeg,
377 const std::vector<uint16_t>& catalogPatternStarInds,
378 int numEdgeRatioBins) {
379 const decimal maxFovRad = DegToRad(maxFovDeg);
380 const decimal cosMaxFov = DECIMAL_COS(maxFovRad);
381
382 const size_t numPatternStars = catalogPatternStarInds.size();
383
384 // Helper to look up a star's spatial vector given Tetra-internal star catalog index
385 auto spatial = [&](size_t tetraInd) -> const Vec3& {
386 return catalog[catalogPatternStarInds[tetraInd]].spatial;
387 };
388
389 // Every star is a pattern star. Each star is marked unavailable after being
390 // used as the first vertex, so each 4-star group is generated exactly once.
391 std::vector<bool> available(numPatternStars, true);
392
393 std::vector<TetraPattern> patterns;
394
395 for (size_t firstStarInd = 0; firstStarInd < numPatternStars; firstStarInd++) {
396 available[firstStarInd] = false;
397
398 const Vec3& v = spatial(firstStarInd);
399
400 // Find all remaining pattern stars within maxFov
401 std::vector<uint16_t> nearbyStars;
402 for (size_t starId = 0; starId < numPatternStars; starId++) {
403 if (!available[starId]) continue;
404 if (v * spatial(starId) > cosMaxFov) {
405 nearbyStars.push_back(starId);
406 }
407 }
408 CombinationIterator combos(3, nearbyStars.size());
409 while (combos.next()) {
410 const std::vector<int>& idx = combos.indices();
411 TetraPattern patt = {(uint16_t)firstStarInd,
412 nearbyStars[idx[0]],
413 nearbyStars[idx[1]],
414 nearbyStars[idx[2]]};
415
416 bool pattFits = AllPairsWithinFov(
418 [&](int i) -> const Vec3& { return spatial(patt[i]); },
419 cosMaxFov);
420
421 if (pattFits) {
422 patterns.push_back(patt);
423 }
424 }
425 }
426
427 std::cerr << "Tetra found " << patterns.size() << " patterns" << std::endl;
428 // Ensure load factor just < 0.5 to prevent against cycling
429 size_t patternTableSize = 2 * patterns.size() + 1;
430 std::vector<TetraPattern> patternTable(patternTableSize);
431 for (const TetraPattern& patt : patterns) {
432 std::vector<decimal> edgeRatios = TetraEdgeRatios(
433 TetraConstants::numPatternStars, [&](int i) -> const Vec3& { return spatial(patt[i]); });
434
435 std::vector<int> binnedEdgeRatios;
436 for (decimal edgeRatio : edgeRatios) {
437 binnedEdgeRatios.push_back(int(edgeRatio * numEdgeRatioBins));
438 }
439
440 size_t patternInd =
441 TetraFindPatternIndex(binnedEdgeRatios.data(), binnedEdgeRatios.size(), numEdgeRatioBins, patternTableSize);
442 for (size_t probeStep = 0; probeStep < patternTableSize; probeStep++) {
443 size_t index = (patternInd + probeStep * probeStep) % patternTableSize;
444 if (patternTable[index][0] == 0 && patternTable[index][1] == 0) {
445 patternTable[index] = patt;
446 break;
447 }
448 }
449 }
450 SerializePrimitive<decimal>(ser, maxFovDeg); // maxFov_
451 SerializePrimitive<int32_t>(ser, numEdgeRatioBins); // numEdgeRatioBins_
452 SerializePrimitive<uint64_t>(ser, patternTable.size()); // patternTableSize_
453 SerializePrimitive<uint16_t>(ser, catalogPatternStarInds.size()); // tetraCatalogSize_
454 for (const TetraPattern& patt : patternTable) {
455 for (int i = 0; i < TetraConstants::numPatternStars; i++) {
456 SerializePrimitive<uint16_t>(ser, patt[i]);
457 }
458 }
459 for (uint16_t ind : catalogPatternStarInds) {
461 }
462}
463
476
484const unsigned char *MultiDatabase::SubDatabasePointer(int32_t magicValue) const {
485 DeserializeContext desValue(buffer);
486 DeserializeContext *des = &desValue; // just for naming consistency with how we use `des` elsewhere
487
488 assert(magicValue != 0);
489 while (true) {
490 int32_t curMagicValue = DeserializePrimitive<int32_t>(des);
491 if (curMagicValue == 0) {
492 return nullptr;
493 }
494 uint32_t dbFlags = DeserializePrimitive<uint32_t>(des);
495
496 // Ensure that our database is using the same type as the runtime.
497 #ifdef LOST_FLOAT_MODE
498 if (!isFlagSet(dbFlags, MULTI_DB_FLOAT_FLAG)) {
499 std::cerr << "LOST was compiled in float mode. This database was serialized in double mode and is incompatible." << std::endl;
500 exit(1);
501 }
502 #else
503 if (isFlagSet(dbFlags, MULTI_DB_FLOAT_FLAG)) {
504 std::cerr << "LOST was compiled in double mode. This database was serialized in float mode and is incompatible." << std::endl;
505 exit(1);
506 }
507 #endif
508
509 uint32_t dbLength = DeserializePrimitive<uint32_t>(des);
510 assert(dbLength > 0);
511 DeserializePadding<uint64_t>(des); // align to an 8-byte boundary
512 const unsigned char *curSubDatabasePointer = DeserializeArray<unsigned char>(des, dbLength);
513 if (curMagicValue == magicValue) {
514 return curSubDatabasePointer;
515 }
516 }
517 // shouldn't ever make it here. Compiler should remove this assertion as unreachable.
518 assert(false);
519}
520
522 const MultiDatabaseDescriptor &dbs,
523 uint32_t flags) {
524 for (const MultiDatabaseEntry &multiDbEntry : dbs) {
525 SerializePrimitive<int32_t>(ser, multiDbEntry.magicValue);
527 SerializePrimitive<uint32_t>(ser, multiDbEntry.bytes.size());
529 std::copy(multiDbEntry.bytes.cbegin(), multiDbEntry.bytes.cend(), std::back_inserter(ser->buffer));
530 }
531 SerializePrimitive<int32_t>(ser, 0); // caboose
532}
533
534} // namespace lost
535
536// TODO: after creating the database, print more statistics, such as average number of pairs per
537// star, stars per bin, space distribution between array and index.
const std::vector< int > & indices() const
A data structure enabling constant-time range queries into fixed numerical data.
Definition databases.hpp:25
KVectorIndex(DeserializeContext *des)
Construct from serialized buffer.
long QueryLiberal(decimal minQueryDistance, decimal maxQueryDistance, long *upperIndex) const
Finds all the entries in the given range, and possibly a few just outside the range on the ends.
const unsigned char * SubDatabasePointer(int32_t magicValue) const
MultiDatabase memory layout:
long NumPairs() const
Exact number of stored pairs.
std::vector< decimal > StarDistances(int16_t star, const Catalog &) const
Return the distances from the given star to each star it's paired with in the database (for debugging...
PairDistanceKVectorDatabase(DeserializeContext *des)
Create the database from a serialized buffer.
static const int32_t kMagicValue
Magic value to use when storing inside a MultiDatabase.
Definition databases.hpp:72
const int16_t * FindPairsExact(const Catalog &, decimal min, decimal max, const int16_t **end) const
const int16_t * FindPairsLiberal(decimal min, decimal max, const int16_t **end) const
Return at least all the star pairs whose inter-star distance is between min and max.
std::vector< unsigned char > buffer
uint16_t CatalogIndex(int tetraCatalogInd) const
TetraPattern GetPattern(size_t index) const
std::vector< TetraPattern > GetPatternMatches(size_t index) const
TetraDatabase(DeserializeContext *des)
static const int32_t kMagicValue
Magic value to use when storing inside a MultiDatabase.
uint64_t PatternTableSize() const
Three dimensional vector with decimaling point components.
#define MULTI_DB_FLOAT_FLAG
Stores "inner angles" between star triples.
double decimal
Definition decimal.hpp:8
#define DECIMAL(x)
Definition decimal.hpp:18
#define DECIMAL_M_PI
Definition decimal.hpp:26
#define DECIMAL_COS(x)
Definition decimal.hpp:50
LOST starting point.
void SerializeKVectorIndex(SerializeContext *ser, const std::vector< decimal > &values, decimal min, decimal max, long numBins)
K-vector index layout.
Definition databases.cpp:65
bool AllPairsWithinFov(int numStars, SpatialAccessor spatial, decimal cosMaxFov)
void SerializePadding(SerializeContext *ser)
size_t TetraFindPatternIndex(const int *patternEdgeRatios, size_t numRatios, int numEdgeRatioBins, size_t tableSize)
decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2)
Calculate the inner angle, in radians, between two /unit/ vectors.
decimal DegToRad(decimal deg)
std::vector< MultiDatabaseEntry > MultiDatabaseDescriptor
const T * DeserializeArray(DeserializeContext *des, long arrLength)
return an array of items as a pointer. Will point into the buffer (mmap style).
bool CompareKVectorPairs(const KVectorPair &p1, const KVectorPair &p2)
Definition databases.cpp:30
decimal Angle(const Vec3 &vec1, const Vec3 &vec2)
Calculate the inner angle, in radians, between two vectors.
void SerializeTetraDatabase(SerializeContext *ser, const Catalog &catalog, decimal maxFovDeg, const std::vector< uint16_t > &catalogPatternStarInds, int numEdgeRatioBins)
std::vector< decimal > TetraEdgeRatios(int numStars, SpatialAccessor spatial)
std::vector< KVectorPair > CatalogToPairDistances(const Catalog &catalog, decimal minDistance, decimal maxDistance)
pair K-vector database layout.
T DeserializePrimitive(DeserializeContext *des)
void SerializePairDistanceKVector(SerializeContext *ser, const Catalog &catalog, decimal minDistance, decimal maxDistance, long numBins)
Serialize a pair-distance KVector into buffer.
bool isFlagSet(uint32_t dbFlags, uint32_t flag)
Definition databases.cpp:20
void SerializePrimitive(SerializeContext *ser, const T &val)
std::vector< uint16_t > TetraPreprocessCatalog(const Catalog &catalog, const decimal maxFovDeg, int numPatternStarsPerFov)
std::array< uint16_t, TetraConstants::numPatternStars > TetraPattern
Definition databases.hpp:93
void DeserializePadding(DeserializeContext *des)
Move the cursor forward past any padding that would appear before a value of type T.
void SerializeMultiDatabase(SerializeContext *ser, const MultiDatabaseDescriptor &dbs, uint32_t flags)
decimal Clamp(decimal num, decimal low, decimal high)
Return the value in the range [low,high] which is closest to num.
std::vector< CatalogStar > Catalog
static constexpr int numPatternStars