Skip to content

Commit

Permalink
refactoring for tetra stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@zeddie888
zeddie888 committed Nov 14, 2023
1 parent 3834a3a commit bdcc396
Show file tree
Hide file tree
Showing 8 changed files with 76 additions and 101 deletions.
1 change: 1 addition & 0 deletions reg.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
# CLI:
# test directory name, output log name
# (optional) attitude estimator
# Example usage: python reg.py samples/ log.txt -att_estimator quest

import subprocess
import argparse
Expand Down
19 changes: 7 additions & 12 deletions src/databases.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -289,7 +289,6 @@ std::pair<std::vector<uint16_t>, std::vector<uint16_t>> TetraPreparePattCat(cons

// Same thing here, we should test each new star's angular distance to all
// stars we've already selected to be kept for verification
// std::vector<float> angsVerifying;
for (int j = 0; j < i; j++) {
if (keepForVerifying[j]) {
float angle = Angle(vec, catalog[j].spatial);
Expand Down Expand Up @@ -329,9 +328,9 @@ std::pair<std::vector<uint16_t>, std::vector<uint16_t>> TetraPreparePattCat(cons
// Find which stars in the final star table
// should be used for pattern construction later in Tetra's database generation step
// pattStarIndices will be a double-index:
// our "final star table" is a list of indices into the original (unchanged) catalog
// pattStarIndices is a list of indices into the final star table to tell Tetra which ones
// to use for pattern construction
// our "final star table" is a list of indices into the original (unchanged) catalog
// pattStarIndices is a list of indices into the final star table to tell Tetra which ones
// to use for pattern construction
int cumulativeSum = -1;
for (int i = 0; i < (int)keepForVerifying.size(); i++) {
if (keepForVerifying[i]) {
Expand Down Expand Up @@ -466,18 +465,14 @@ std::vector<float> PairDistanceKVectorDatabase::StarDistances(int16_t star,
return result;
}

///////////////////// Tetra database //////////////////////

void SerializeTetraDatabase(SerializeContext *ser, const Catalog &catalog, float maxFovDeg,
const std::vector<uint16_t> &pattStarIndices,
const std::vector<uint16_t> &catIndices) {
const float maxFovRad = DegToRad(maxFovDeg);

// TODO: these are hardcoded values
// pattBins here and numPattBins in TetraStarIDAlgorithm::numPattBins must be the same
const int pattBins = 50;
const int pattBins = TetraConstants::numPattBins;
const int pattSize = TetraConstants::numPattStars;
const int tempBins = 4;
const int pattSize = 4;

Catalog tetraCatalog;
for (int ind : catIndices){
Expand Down Expand Up @@ -538,7 +533,7 @@ void SerializeTetraDatabase(SerializeContext *ser, const Catalog &catalog, float
}
return nearbyStarIDs;
};
using Pattern = std::array<uint16_t, pattSize>;
using Pattern = std::array<uint16_t, 4>;
std::vector<Pattern> pattList;
Pattern patt{0, 0, 0, 0};

Expand Down Expand Up @@ -587,7 +582,7 @@ void SerializeTetraDatabase(SerializeContext *ser, const Catalog &catalog, float
}

std::cerr << "Tetra found " << pattList.size() << " patterns" << std::endl;
// Ensure load factor just < 0.5
// Ensure load factor just < 0.5 to prevent against cycling
long long pattCatalogLen = 2 * (int)pattList.size() + 1;
std::vector<Pattern> pattCatalog(pattCatalogLen);
for (Pattern patt : pattList) {
Expand Down
2 changes: 1 addition & 1 deletion src/io.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ const Catalog &CatalogRead() {
std::sort(catalog.begin(), catalog.end(), [](const CatalogStar &a, const CatalogStar &b) {
return a.spatial.x < b.spatial.x;
});
for (int i = catalog.size(); i > 0; i--) {
for (int i = catalog.size()-1; i > 0; i--) {
if ((catalog[i].spatial - catalog[i-1].spatial).Magnitude() < 5e-5) { // 70 stars removed at this threshold.
if (catalog[i].magnitude > catalog[i-1].magnitude) {
catalog.erase(catalog.begin() + i);
Expand Down
21 changes: 0 additions & 21 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,27 +33,6 @@ static void DatabaseBuild(const DatabaseOptions &values) {
SerializeContext ser = serFromDbValues(values);
SerializeMultiDatabase(&ser, dbEntries);

// if (values.tetra) {
// std::cerr << "Tetra max angle is: " << values.tetraMaxAngle << std::endl;
// auto tetraStuff = TetraPreparePattCat(narrowedCatalog, values.tetraMaxAngle);
// std::vector<uint16_t> catIndices = tetraStuff.first;
// std::vector<uint16_t> pattStars = tetraStuff.second;

// std::cerr << "Tetra processed catalog has " << catIndices.size() << " stars." << std::endl;
// std::cerr << "Number of pattern stars: " << pattStars.size() << std::endl;

// GenerateTetraDatabases(&builder, narrowedCatalog, values, pattStars, catIndices);
// std::cerr << "Generated TETRA database with " << builder.BufferLength() << " bytes"
// << std::endl;
// }
// // We should allow for multiple databases at the same tme
// // Do NOT make this an if...else if...else
// if (values.kvector) {
// GenerateDatabases(&builder, narrowedCatalog, values);
// std::cerr << "Generated kvector database with " << builder.BufferLength() << " bytes"
// << std::endl;
// }

std::cerr << "Generated database with " << ser.buffer.size() << " bytes" << std::endl;

UserSpecifiedOutputStream pos = UserSpecifiedOutputStream(values.outputPath, true);
Expand Down
108 changes: 47 additions & 61 deletions src/star-id.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,10 @@ namespace lost {
* Divide all edge lengths by the largest edge (except the LE itself) to get edge ratios
* Return the pattern = vector of edge ratios in sorted order
*/
static std::vector<float> TetraConstructPattern(const std::vector<Vec3> &spats);
static std::vector<double> TetraConstructPattern(const std::vector<Vec3> &spats);

static std::vector<float> TetraConstructPattern(const std::vector<Vec3> &spats) {
std::vector<float> edgeLengths;
static std::vector<double> TetraConstructPattern(const std::vector<Vec3> &spats) {
std::vector<double> edgeLengths;
// C(numPattStars, 2) edge lengths calculated
for (int i = 0; i < (int)spats.size(); i++) {
for (int j = i + 1; j < (int)spats.size(); j++) {
Expand All @@ -39,29 +39,29 @@ static std::vector<float> TetraConstructPattern(const std::vector<Vec3> &spats)
}

std::sort(edgeLengths.begin(), edgeLengths.end());
float largestEdge = edgeLengths[(int)(edgeLengths.size()) - 1]; // largest edge value
double largestEdge = edgeLengths.back(); // largest edge value

// size==C(numPattStars, 2) - 1
// We don't calculate ratio for largest edge, which would just be 1
std::vector<float> edgeRatios; // size() = C(numPattStars, 2) - 1
for (int i = 0; i < (int)edgeLengths.size() - 1; i++) {
std::vector<double> edgeRatios; // size() = C(numPattStars, 2) - 1
for (int i = 0; i < edgeLengths.size() - 1; i++) {
edgeRatios.push_back(edgeLengths[i] / largestEdge);
}
return edgeRatios;
}

class TetraCentroidComboIterator{
public:
TetraCentroidComboIterator(int numPattStars, int numCentroids)
: firstTime_(true), pattSize_(numPattStars), numCentroids_(numCentroids) {}

/**
* @brief Tetra only: get indices of centroids for new star pattern, store in res
*
* Assumes that your centroids have already been sorted in descending order of brightness
* Bright stars tend to have lower centroiding error
*/
bool getCentroidCombo(std::vector<int> *const res) {
class TetraCentroidComboIterator {
public:
TetraCentroidComboIterator(int numPattStars, int numCentroids)
: firstTime_(true), pattSize_(numPattStars), numCentroids_(numCentroids) {}

/**
* @brief Tetra only: get indices of centroids for new star pattern, store in res
*
* Assumes that your centroids have already been sorted in decreasing order of brightness
* Bright stars tend to have lower centroiding error
*/
bool getCentroidCombo(std::vector<int> *const res) {
if (numCentroids_ < pattSize_) {
return false;
}
Expand Down Expand Up @@ -90,7 +90,7 @@ class TetraCentroidComboIterator{
}
*res = std::vector<int>(indices_.begin() + 1, indices_.end() - 1);
return true;
}
}

private:
bool firstTime_;
Expand All @@ -103,9 +103,6 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
const Catalog &catalog, const Camera &camera) const {
// Result format: (centroidIndex, catalogIndex)
StarIdentifiers result;
// std::cerr << "TETRA" << std::endl;

////////////////////////////////////////////////////////

MultiDatabase multiDatabase(database);
const unsigned char *databaseBuffer =
Expand All @@ -120,31 +117,26 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
const long long catLength = tetraDatabase.PattCatSize();
const float maxFov = tetraDatabase.MaxAngle();

// std::cout << "Tetra can handle max FOV: " << maxFov << std::endl;

std::vector<int> centroidIndices;
for (int i = 0; i < (int)stars.size(); i++) {
for (int i = 0; i < stars.size(); i++) {
centroidIndices.push_back(i);
}

// Sort centroided stars by brightness, high to low. Larger is brighter
std::sort(centroidIndices.begin(), centroidIndices.end(),
[&stars](int a, int b) { return stars[a].magnitude > stars[b].magnitude; });
[&stars](int a, int b) { return stars[a].magnitude > stars[b].magnitude; });

// Index of centroid indices list
std::vector<int> chosenCentroidIndices(numPattStars);

TetraCentroidComboIterator tetraCentroidComboIt(numPattStars, centroidIndices.size());
std::vector<int> chosenCentroidIndices(TetraConstants::numPattStars);
TetraCentroidComboIterator tetraCentroidComboIt(TetraConstants::numPattStars, centroidIndices.size());

// TODO: In practice, maybe cap this at some number of combinations, maybe 10 or so
while (tetraCentroidComboIt.getCentroidCombo(&chosenCentroidIndices)) {
// std::cerr << "new combo" << std::endl;

// Index in centroid indices list
std::vector<int> chosenIndIndices;
std::vector<Star> chosenStars;

for (int i = 0; i < numPattStars; i++) {
for (int i = 0; i < TetraConstants::numPattStars; i++) {
chosenIndIndices.push_back(centroidIndices[chosenCentroidIndices[i]]);
chosenStars.push_back(stars[chosenIndIndices[i]]);
}
Expand Down Expand Up @@ -175,24 +167,26 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St

if (!angleAcceptable) {
// std::cerr << "Skipping combination, some angle is too large" << std::endl;
// Try again
// Try again, some angle is too large
continue;
}

std::vector<float> pattEdgeRatios = TetraConstructPattern(pattStarVecs);
std::vector<double> pattEdgeRatios = TetraConstructPattern(pattStarVecs);

// Binning step - account for potential error in calculated values by testing hash codes
// in a range
// Binning step
// Account for potential error in calculated values by
// testing hash codes in a range
std::vector<std::pair<int, int>> hcSpace;
for (float edgeRatio : pattEdgeRatios) {
for (double edgeRatio : pattEdgeRatios) {
std::pair<int, int> range;
int lo = int((edgeRatio - pattErrorRange) * numPattBins);
int lo =
int((edgeRatio - TetraConstants::pattErrorRange) * TetraConstants::numPattBins);
lo = std::max(lo, 0);
int hi = int((edgeRatio + pattErrorRange) * numPattBins);
hi = std::min(hi + 1, numPattBins);
int hi =
int((edgeRatio + TetraConstants::pattErrorRange) * TetraConstants::numPattBins);
hi = std::min(hi + 1, TetraConstants::numPattBins);
range = std::make_pair(lo, hi);
hcSpace.push_back(range);
// std::cerr << range.first << ", " << range.second << std::endl;
}

std::set<std::vector<int>> finalCodes;
Expand All @@ -212,18 +206,16 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
}

for (std::vector<int> code : finalCodes) {
// std::cerr << "new code" << std::endl;

// Must clear results list here
result.clear();

int hashIndex = KeyToIndex(code, numPattBins, catLength);
int hashIndex = KeyToIndex(code, TetraConstants::numPattBins, catLength);
// Get a list of Pattern Catalog rows with hash code == hashIndex
// One of these Patterns in the database could be a match to our constructed Pattern
std::vector<TetraPatt> matches = tetraDatabase.GetPatternMatches(hashIndex);

if ((int)matches.size() == 0) {
// std::cerr << "Alert: matches size = 0, continuing" << std::endl;
if (matches.size() == 0) {
// No matches for this pattern
continue;
}

Expand All @@ -237,27 +229,25 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
int catInd = tetraDatabase.GetTrueCatInd(star);
CatalogStar catStar = catalog[catInd];
Vec3 catVec = catStar.spatial;
// catStarIDs.push_back(catStar.name);
catStarInds.push_back(catInd);
catStarVecs.push_back(catVec);
}

std::vector<float> catEdgeRatios = TetraConstructPattern(catStarVecs);
std::vector<double> catEdgeRatios = TetraConstructPattern(catStarVecs);

bool skipMatchRow = false;
for (int i = 0; i < (int)catEdgeRatios.size(); i++) {
float val = catEdgeRatios[i] - pattEdgeRatios[i];
val = std::abs(val);
// Test that our pattern and PC pattern roughly match up
// For now, just compare edge ratios
if (val > pattMaxError) {
if (val > TetraConstants::pattMaxError) {
skipMatchRow = true;
}
}

// Very common to skip here, database Pattern is NOT a match to ours
if (skipMatchRow) {
// std::cerr << "Alert: not a match!" << std::endl;
continue;
}

Expand All @@ -270,11 +260,11 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
continue; // If we already found a match, don't add new ID'd stars
alrFoundMatch = true;

Vec3 pattCentroid(0, 0, 0);
Vec3 pattCentroid{0, 0, 0};
for (Vec3 pattStarVec : pattStarVecs) {
pattCentroid = pattCentroid + pattStarVec;
}
pattCentroid = pattCentroid * (1.0 / (int)pattStarVecs.size());
pattCentroid = pattCentroid * (1.0 / pattStarVecs.size());

std::vector<float> pattRadii;
for (Vec3 pattStarVec : pattStarVecs) {
Expand All @@ -287,37 +277,33 @@ StarIdentifiers TetraStarIdAlgorithm::Go(const unsigned char *database, const St
std::vector<Vec3> pattSortedVecs =
ArgsortVector<Vec3, float>(pattStarVecs, pattRadii);

Vec3 catCentroid(0, 0, 0);
Vec3 catCentroid{0, 0, 0};
for (Vec3 catStarVec : catStarVecs) {
catCentroid = catCentroid + catStarVec;
}
catCentroid = catCentroid * (1.0 / (int)catStarVecs.size());
catCentroid = catCentroid * (1.0 / catStarVecs.size());

std::vector<float> catRadii;
for (Vec3 catStarVec : catStarVecs) {
catRadii.push_back((catStarVec - catCentroid).Magnitude());
}


std::vector<int> catSortedStarIDs = ArgsortVector<int, float>(catStarInds, catRadii);
std::vector<Vec3> catSortedVecs = ArgsortVector<Vec3, float>(catStarVecs, catRadii);

for (int i = 0; i < numPattStars; i++) {
for (int i = 0; i < TetraConstants::numPattStars; i++) {
int centroidIndex = sortedCentroidIndices[i];

int catInd = catSortedStarIDs[i];

// std::cout << "Centroid Index: " << centroidIndex
// std::cerr << "Centroid Index: " << centroidIndex
// << ", Result StarID: " << resultStarID << std::endl;

result.push_back(StarIdentifier(centroidIndex, catInd));
}

// Note: StarIdentifier wants the catalog INDEX, not the real star ID
// std::cout << "SUCCESS: stars successfully matched" << std::endl;

// TODO: restore this line for early return
// return result;
// TODO: we used to consider early return here
}

// If we've looked at all possible matches and found a unique one, return it
Expand Down
10 changes: 5 additions & 5 deletions src/star-id.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,11 @@ class TetraStarIdAlgorithm : public StarIdAlgorithm {
const Catalog &catalog, const Camera &) const;

private:
// Do NOT modify these parameters unless you know what you're doing
const int numPattStars = 4;
const int numPattBins = 50;
const float pattErrorRange = 0.001; // default: 0.002
const float pattMaxError = 0.001; // default: 0.002
// static const int numPattStars;
// // Do NOT modify the following parameters unless you know what you're doing
// static const int numPattBins;
// const float pattErrorRange = 0.001;
// const float pattMaxError = 0.001;

};

Expand Down
5 changes: 5 additions & 0 deletions src/star-utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,11 @@

namespace lost {

const int TetraConstants::numPattStars = 4;
const int TetraConstants::numPattBins = 50;
const float TetraConstants::pattErrorRange = 0.001;
const float TetraConstants::pattMaxError = 0.001;

// brightest star first
bool CatalogStarMagnitudeCompare(const CatalogStar &a, const CatalogStar &b) {
return a.magnitude < b.magnitude;
Expand Down
Loading

0 comments on commit bdcc396

Please sign in to comment.