Allow "unknown" attitudes so we can do attitude availability/error ra…

…te testing

src/attitude-estimators.cpp

@@ -10,6 +10,10 @@ Attitude DavenportQAlgorithm::Go(const Camera &camera,
                                  const Stars &stars,
                                  const Catalog &catalog,
                                  const StarIdentifiers &starIdentifiers) {
+    if (starIdentifiers.size() < 2) {
+        return Attitude();
+    }
+    assert(stars.size() >= 2);
 
     // attitude profile matrix
     Eigen::Matrix3f B;
@@ -88,9 +92,10 @@ Attitude TriadAlgorithm::Go(const Camera &camera,
                             const Stars &stars,
                             const Catalog &catalog,
                             const StarIdentifiers &starIds) {
-    if ((int)stars.size() < 2 || (int)starIds.size() < 2) {
-        return Attitude(Quaternion(1,0,0,0));
+    if (starIds.size() < 2) {
+        return Attitude();
     }
+    assert(stars.size() >= 2);
 
     // TODO: Better way of picking the two stars
     StarIdentifier
@@ -138,6 +143,11 @@ Attitude QuestAlgorithm::Go(const Camera &camera,
                             const Catalog &catalog,
                             const StarIdentifiers &starIdentifiers) {
 
+    if (starIdentifiers.size() < 2) {
+        return Attitude();
+    }
+    assert(stars.size() >= 2);
+
     // initial guess for eigenvalue (sum of the weights)
     float guess = 0;
 

src/attitude-utils.cpp

@@ -62,7 +62,13 @@ float Quaternion::Angle() const {
     return (real >= 1 ? 0 : acos(real))*2;
 }
 
-/// Change the amount of rotation in a quaternion, keeping that rotation around the same axis.
+float Quaternion::SmallestAngle() const {
+    float rawAngle = Angle();
+    return rawAngle > M_PI
+        ? 2*M_PI - rawAngle
+        : rawAngle;
+}
+
 void Quaternion::SetAngle(float newAngle) {
     real = cos(newAngle/2);
     SetVector(Vector().Normalize() * sin(newAngle/2));
@@ -336,10 +342,10 @@ Mat3 Mat3::Inverse() const {
                      0,0,1};
 
 Attitude::Attitude(const Quaternion &quat)
-    : quaternion(quat), type(QuaternionType) {}
+    : type(AttitudeType::QuaternionType), quaternion(quat) {}
 
 Attitude::Attitude(const Mat3 &matrix)
-    : dcm(matrix), type(DCMType) {}
+    : type(AttitudeType::DCMType), dcm(matrix) {}
 
 /// Convert a quaternion to a rotation matrix (Direction Cosine Matrix)
 Mat3 QuaternionToDCM(const Quaternion &quat) {
@@ -385,48 +391,58 @@ Quaternion DCMToQuaternion(const Mat3 &dcm) {
 /// Get the quaternion representing the attitude, converting from whatever format is stored.
 Quaternion Attitude::GetQuaternion() const {
     switch (type) {
-        case QuaternionType:
-            return quaternion;
-        case DCMType:
-            return DCMToQuaternion(dcm);
-        default:
-            assert(false);
+    case AttitudeType::QuaternionType:
+        return quaternion;
+    case AttitudeType::DCMType:
+        return DCMToQuaternion(dcm);
+    default:
+        assert(false);
     }
 }
 
 /// Get the rotation matrix (direction cosine matrix) representing the attitude, converting from whatever format is stored.
 Mat3 Attitude::GetDCM() const {
     switch (type) {
-        case DCMType:
-            return dcm;
-        case QuaternionType:
-            return QuaternionToDCM(quaternion);
-        default:
-            assert(false);
+    case AttitudeType::DCMType:
+        return dcm;
+    case AttitudeType::QuaternionType:
+        return QuaternionToDCM(quaternion);
+    default:
+        assert(false);
     }
 }
 
 /// Convert a vector from the reference frame to the body frame.
 Vec3 Attitude::Rotate(const Vec3 &vec) const {
     switch (type) {
-        case DCMType:
-            return dcm*vec;
-        case QuaternionType:
-            return quaternion.Rotate(vec);
-        default:
-            assert(false);
+    case AttitudeType::DCMType:
+        return dcm*vec;
+    case AttitudeType::QuaternionType:
+        return quaternion.Rotate(vec);
+    default:
+        assert(false);
     }
 }
 
 /// Get the euler angles from the attitude, converting from whatever format is stored.
 EulerAngles Attitude::ToSpherical() const {
     switch (type) {
-        case DCMType:
-            return GetQuaternion().ToSpherical();
-        case QuaternionType:
-            return quaternion.ToSpherical();
-        default:
-            assert(false);
+    case AttitudeType::DCMType:
+        return GetQuaternion().ToSpherical();
+    case AttitudeType::QuaternionType:
+        return quaternion.ToSpherical();
+    default:
+        assert(false);
+    }
+}
+
+bool Attitude::IsKnown() const {
+    switch (type) {
+    case AttitudeType::DCMType:
+    case AttitudeType::QuaternionType:
+        return true;
+    default:
+        return false;
     }
 }
 

src/attitude-utils.hpp

@@ -107,6 +107,9 @@ class Quaternion {
     void SetVector(const Vec3 &);
     Vec3 Rotate(const Vec3 &) const;
     float Angle() const;
+    /// Returns the smallest angle that can be used to represent the rotation represented by the
+    /// quaternion. I.e, min(Angle, 2pi-Angle).
+    float SmallestAngle() const;
     void SetAngle(float);
     EulerAngles ToSpherical() const;
     bool IsUnit(float tolerance) const;
@@ -121,13 +124,17 @@ class Quaternion {
 //
 /**
  * The attitude (orientation) of a spacecraft.
- * The Attitude object stores either a rotation matrix (direction cosine matrix) or a quaternion, and converts automatically to the other format when needed.
- * @note When porting to an embedded device, you'll probably want to get rid of this class and adapt to
+ * The Attitude object stores either a rotation matrix (direction cosine matrix) or a quaternion,
+ * and converts automatically to the other format when needed. Importantly, an attitude may also be
+ * "unknown", representing for example if there weren't enough stars to determine an attitude. When
+ * set to unknown, it is illegal to try and convert it to anything, so check IsKnown first!
+ * @note When porting to an embedded device, you may want to get rid of this class and adapt to
  * either quaternions or DCMs exclusively, depending on the natural output format of whatever
  * attitude estimation algorithm you're using.
  */
 class Attitude {
 public:
+    /// constructs unknown attitude:
     Attitude() = default;
     explicit Attitude(const Quaternion &); // NOLINT
     explicit Attitude(const Mat3 &dcm);
@@ -136,33 +143,34 @@ class Attitude {
     Mat3 GetDCM() const;
     EulerAngles ToSpherical() const;
     Vec3 Rotate(const Vec3 &) const;
+    bool IsKnown() const;
 
 private:
-    enum AttitudeType {
-        NullType,
+    enum class AttitudeType {
+        UnknownType, /// Doesn't mean "unknown type", but rather "we have no fucking clue where we're pointing"
         QuaternionType,
         DCMType,
     };
 
+    AttitudeType type;
     Quaternion quaternion;
     Mat3 dcm; // direction cosine matrix
-    AttitudeType type;
 };
 
 Mat3 QuaternionToDCM(const Quaternion &);
 Quaternion DCMToQuaternion(const Mat3 &);
 
-// Return a quaternion that will reorient the coordinate axes so that the x-axis points at the given
-// right ascension and declination, then roll the coordinate axes counterclockwise (i.e., the stars
-// will appear to rotate clockwise). This is an "improper" z-y'-x' Euler rotation.
+/// Return a quaternion that will reorient the coordinate axes so that the x-axis points at the given
+/// right ascension and declination, then roll the coordinate axes counterclockwise (i.e., the stars
+/// will appear to rotate clockwise). This is an "improper" z-y'-x' Euler rotation.
 Quaternion SphericalToQuaternion(float ra, float dec, float roll);
 
-// returns unit vector
+/// returns unit vector
 Vec3 SphericalToSpatial(float ra, float de);
 void SpatialToSpherical(const Vec3 &, float *ra, float *de);
-// angle between two vectors, using dot product and magnitude division
+/// angle between two vectors, using dot product and magnitude division
 float Angle(const Vec3 &, const Vec3 &);
-// angle between two vectors, /assuming/ that they are already unit length
+/// angle between two vectors, /assuming/ that they are already unit length
 float AngleUnit(const Vec3 &, const Vec3 &);
 
 float RadToDeg(float);

src/io.cpp

@@ -1455,16 +1455,23 @@ static void PipelineComparatorStarIds(std::ostream &os,
 }
 
 static void PrintAttitude(std::ostream &os, const std::string &prefix, const Attitude &attitude) {
-    EulerAngles spherical = attitude.ToSpherical();
-    os << prefix << "attitude_ra " << RadToDeg(spherical.ra) << std::endl;
-    os << prefix << "attitude_de " << RadToDeg(spherical.de) << std::endl;
-    os << prefix << "attitude_roll " << RadToDeg(spherical.roll) << std::endl;
-
-    Quaternion q = attitude.GetQuaternion();
-    os << prefix << "attitude_i " << q.i << std::endl;
-    os << prefix << "attitude_j " << q.j << std::endl;
-    os << prefix << "attitude_k " << q.k << std::endl;
-    os << prefix << "attitude_real " << q.real << std::endl;
+    if (attitude.IsKnown()) {
+        os << prefix << "attitude_known 1" << std::endl;
+
+        EulerAngles spherical = attitude.ToSpherical();
+        os << prefix << "attitude_ra " << RadToDeg(spherical.ra) << std::endl;
+        os << prefix << "attitude_de " << RadToDeg(spherical.de) << std::endl;
+        os << prefix << "attitude_roll " << RadToDeg(spherical.roll) << std::endl;
+
+        Quaternion q = attitude.GetQuaternion();
+        os << prefix << "attitude_i " << q.i << std::endl;
+        os << prefix << "attitude_j " << q.j << std::endl;
+        os << prefix << "attitude_k " << q.k << std::endl;
+        os << prefix << "attitude_real " << q.real << std::endl;
+
+    } else {
+        os << prefix << "attitude_known 0" << std::endl;
+    }
 }
 
 /// Print the identifed attitude to `os` in Euler angle format.
@@ -1499,25 +1506,31 @@ static void PipelineComparatorAttitude(std::ostream &os,
 
     float attitudeErrorSum = 0.0f;
     int numCorrect = 0;
+    int numIncorrect = 0;
 
     for (int i = 0; i < (int)expected.size(); i++) {
-        Quaternion expectedQuaternion = expected[i]->ExpectedAttitude()->GetQuaternion();
-        Quaternion actualQuaternion = actual[i].attitude->GetQuaternion();
-        float attitudeError = (expectedQuaternion * actualQuaternion.Conjugate()).Angle();
-        assert(attitudeError >= 0);
-
-        attitudeErrorSum += attitudeError;
-        if (attitudeError <= angleThreshold) {
-            numCorrect++;
+        if (actual[i].attitude->IsKnown()) {
+            Quaternion expectedQuaternion = expected[i]->ExpectedAttitude()->GetQuaternion();
+            Quaternion actualQuaternion = actual[i].attitude->GetQuaternion();
+            float attitudeError = (expectedQuaternion * actualQuaternion.Conjugate()).SmallestAngle();
+            assert(attitudeError >= 0);
+
+            if (attitudeError <= angleThreshold) {
+                attitudeErrorSum += attitudeError;
+                numCorrect++;
+            } else {
+                numIncorrect++;
+            }
         }
     }
 
     float attitudeErrorMean = attitudeErrorSum / expected.size();
     float fractionCorrect = (float)numCorrect / expected.size();
+    float fractionIncorrect = (float)numIncorrect / expected.size();
 
     os << "attitude_error_mean " << attitudeErrorMean << std::endl;
-    os << "attitude_num_correct " << numCorrect << std::endl;
-    os << "attitude_fraction_correct " << fractionCorrect << std::endl;
+    os << "attitude_availability " << fractionCorrect << std::endl;
+    os << "attitude_error_rate " << fractionIncorrect << std::endl;
 }
 
 static void PrintTimeStats(std::ostream &os, const std::string &prefix, const std::vector<long> &times) {
@@ -1583,7 +1596,9 @@ static void PipelineComparatorPrintSpeed(std::ostream &os,
     if (attitudeTimes.size() > 0) {
         PrintTimeStats(os, "attitude", attitudeTimes);
     }
-    PrintTimeStats(os, "total", totalTimes);
+    if (centroidingTimes.size() > 0 || starIdTimes.size() > 0 || attitudeTimes.size() > 0) {
+        PrintTimeStats(os, "total", totalTimes);
+    }
 }
 
 // TODO: add these debug comparators back in!