Tracking mode

.gitignore

@@ -5,6 +5,8 @@
 *.o
 *.d
 *.png
+*.dat
+*.txt
 
 /documentation/man-*.h
 

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.

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 (--tracking-compare-threshold \fIthreshold\fP))] [--attitude-algo \fIalgo\fP] [--plot \fIoutput-path\fB]
 
 .SH DESCRIPTION
 
@@ -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,
@@ -69,6 +69,18 @@ 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. Defaults to 0.001 if not selected. Must be nonnegative.
+
+.TP
+\fB--tracking-compare-threshold\fP \fIthreshold\fP
+\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
 Runs the \fIalgo\fP algorithm for the attitude stage of the pipeline. Current options are "dqm" (Davenport Q) and "triad". Defaults to dqm.

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)

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;
@@ -314,6 +314,124 @@ MultiDatabaseBuilder::~MultiDatabaseBuilder() {
     free(buffer);
 }
 
+
+/*** 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;
+}
+
+// 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<TrackingStar> 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);
+
+    // serialize into buffer
+    unsigned char *bufferStart = buffer;
+
+    // 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);
+    }
+
+    // verify length
+    assert(buffer - bufferStart == SerializeLengthTrackingCatalog(catalog));
+}
+
+// deserialize database
+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);
+    }
+}
+
+// query database (returns list of indices into the catalog that have stars within radius of point)
+std::vector<int16_t> TrackingSortedDatabase::QueryNearestStars(const Catalog& catalog, const Vec3 point, float radius) {
+    assert(radius >= 0);
+
+    std::vector<int16_t> query_ind;
+
+    //  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;
+
+    while (left <= right) {
+        int16_t mid = left + (right - left) / 2;
+        CatalogStar s = catalog[indices[mid]];
+        Vec3 diff = s.spatial - point;
+        if (abs(diff.x) <= radius) {
+            index = mid;
+            break;
+        } else if (s.spatial.x < point.x) {
+            left = mid + 1;
+        } else {
+            right = mid - 1;
+        }
+    }
+
+    // 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;
+    }
+
+    // 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;
+    }
+
+    return query_ind;
+}
+
 }
 
 // TODO: after creating the database, print more statistics, such as average number of pairs per

src/databases.hpp

@@ -82,6 +82,23 @@ 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 kMagicValue = 0x2536f0A9;
+    std::vector<int16_t> QueryNearestStars(const Catalog &, const Vec3 point, float radius);
+
+private:
+    int16_t length;                     // length of catalog
+    std::vector<int16_t> indices;       // sorted list of catalog indices
+};
+
+long SerializeLengthTrackingCatalog(const Catalog &catalog);
+void SerializeTrackingCatalog(const Catalog &catalog, unsigned char *buffer);
+
 // maximum number of databases in a MultiDatabase
 const int kMultiDatabaseMaxDatabases = 64;
 const long kMultiDatabaseTocLength = 8*kMultiDatabaseMaxDatabases;

src/io.cpp

@@ -249,6 +249,15 @@ void BuildKVectorDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog,
 
 }
 
+void BuildTrackingDatabase(MultiDatabaseBuilder &builder, const Catalog &catalog) {
+    long length = SerializeLengthTrackingCatalog(catalog);
+    unsigned char *buffer = builder.AddSubDatabase(TrackingSortedDatabase::kMagicValue, length);
+    if (buffer == NULL) {
+        std::cerr << "No room for another database." << std::endl;
+    }
+    SerializeTrackingCatalog(catalog, buffer);
+}
+
 
 void GenerateDatabases(MultiDatabaseBuilder &builder, const Catalog &catalog, const DatabaseOptions &values) {
 
@@ -257,6 +266,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);
@@ -473,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));
             }
         }
     }
@@ -684,6 +695,28 @@ Pipeline SetPipeline(const PipelineOptions &values) {
         result.starIdAlgorithm = std::unique_ptr<StarIdAlgorithm>(new GeometricVotingStarIdAlgorithm(DegToRad(values.angularTolerance)));
     } else if (values.idAlgo == "py") {
         result.starIdAlgorithm = std::unique_ptr<StarIdAlgorithm>(new PyramidStarIdAlgorithm(DegToRad(values.angularTolerance), values.estimatedNumFalseStars, values.maxMismatchProb, 1000));
+    } else if (values.idAlgo == "tracking") {
+
+        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<float> 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 attitude
+        Quaternion q = SphericalToQuaternion(DegToRad(attInputs[0]), DegToRad(attInputs[1]), DegToRad(attInputs[2]));
+        Attitude a(q);
+
+        // set prev attitude and id algo
+        PrevAttitude prev(a, values.uncertainty, values.trackingCompareThreshold);
+        result.starIdAlgorithm = std::unique_ptr<StarIdAlgorithm>(new TrackingModeStarIdAlgorithm(prev));
+
     } else if (values.idAlgo != "") {
         std::cout << "Illegal id algorithm." << std::endl;
         exit(1);
@@ -762,7 +795,6 @@ PipelineOutput Pipeline::Go(const PipelineInput &input) {
 std::vector<PipelineOutput> Pipeline::Go(const PipelineInputList &inputs) {
     std::vector<PipelineOutput> result;
 
-
     for (const std::unique_ptr<PipelineInput> &input : inputs) {
         result.push_back(Go(*input));
     }

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)
@@ -24,6 +24,9 @@ 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            , 0.001  , 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)

src/star-id.cpp

@@ -2,6 +2,8 @@
 #include <math.h>
 #include <assert.h>
 #include <vector>
+#include <algorithm>
+#include <map>
 
 #include "star-id.hpp"
 #include "databases.hpp"
@@ -489,7 +491,6 @@ StarIdentifiers PyramidStarIdAlgorithm::Go(
 
                         PyramidIdentifyRemainingStars(&identified, stars, catalog, vectorDatabase, camera, tolerance);
                         printf("Identified an additional %d stars\n", (int)identified.size() - 4);
-
                         return identified;
                     }
 
@@ -503,4 +504,37 @@ StarIdentifiers PyramidStarIdAlgorithm::Go(
     return identified;
 }
 
+// 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;
+    if (l.j < r.j) return true;
+    return (l.k < r.k);
+}
+
+StarIdentifiers TrackingModeStarIdAlgorithm::Go(
+    const unsigned char *database, const Stars &stars, const Catalog &catalog, const Camera &camera) const {
+
+    StarIdentifiers definite;
+    MultiDatabase multiDatabase(database);
+    const unsigned char *databaseBuffer = multiDatabase.SubDatabasePointer(TrackingSortedDatabase::kMagicValue);
+    if (databaseBuffer == NULL) {
+        return definite;
+    }
+    TrackingSortedDatabase vectorDatabase(databaseBuffer);
+
+    // 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<int16_t> possiblePrevStars = vectorDatabase.QueryNearestStars(catalog, prevPos, prevAttitude.uncertainty+prevAttitude.compareThreshold);
+        if (possiblePrevStars.size() == 1) {
+            definite.push_back(StarIdentifier(i,possiblePrevStars[0]));
+        }
+    }
+
+    return definite;
+}
+
 }