"MATHS" --Sean Murray

Makefile

@@ -22,14 +22,17 @@
 # https://stackoverflow.com/q/2394609)
 
 SRCS := $(wildcard src/*.cpp)
+TESTS := $(wildcard test/*.cpp)
 OBJS := $(patsubst %.cpp,%.o,$(SRCS))
+TEST_OBJS := $(patsubst %.cpp,%.o,$(TESTS) $(filter-out %/main.o, $(OBJS)))
 DEPS := $(patsubst %.cpp,%.d,$(SRCS))
 BIN  := lost
+TEST_BIN := ./lost-test
 
 BSC  := bright-star-catalog.tsv
 
 LIBS     := -lcairo
-CXXFLAGS := $(CXXFLAGS) -Ivendor -Wall --std=c++11
+CXXFLAGS := $(CXXFLAGS) -Ivendor -Isrc -Wall --std=c++11
 
 all: $(BIN) $(BSC)
 
@@ -44,8 +47,14 @@ $(BIN): $(OBJS)
 
 -include $(DEPS)
 
+test: $(TEST_BIN)
+	$(TEST_BIN)
+
+$(TEST_BIN): $(TEST_OBJS)
+	$(CXX) $(LDFLAGS) -o $(TEST_BIN) $(TEST_OBJS) $(LIBS)
+
 clean:
 	rm -f $(OBJS) $(DEPS)
 	rm -i $(BSC)
 
-.PHONY: all clean
+.PHONY: all clean test

compile_flags.txt

@@ -1 +1,3 @@
 -std=c++11
+-Ivendor
+-Isrc

src/attitude-estimators.cpp

@@ -3,64 +3,66 @@
 #include <eigen3/Eigen/Eigenvalues>
 
 namespace lost {
-    Quaternion DavenportQAlgorithm::Go(const Camera &cameraBoy, const Stars &starBoy, const Catalog &catalogBoy, const StarIdentifiers &StarIdentifiersBoy) {
-        //create a vector that'll hold {bi} (Stars in our frame)
-        //create a vector that'll hold {ri} (Stars in catalog frame)
-        Eigen::Matrix3f B;
-        B.setZero();
-        for (const StarIdentifier &s: StarIdentifiersBoy) {
-            Star bStar = starBoy[s.starIndex];
-            float ra;
-            float dej;
-            cameraBoy.CoordinateAngles({bStar.x, bStar.y}, &ra, &dej);
-            Eigen::Vector3f bi;
-            bi << cos(ra)*cos(dej),
-                sin(ra)*cos(dej),
-                sin(dej);
 
-            CatalogStar rStar = catalogBoy[s.catalogIndex];
-            Eigen::Vector3f ri;
-            ri << cos(rStar.raj2000)*cos(rStar.dej2000),
-                sin(rStar.raj2000)*cos(rStar.dej2000),
-                sin(rStar.dej2000);
+Quaternion DavenportQAlgorithm::Go(const Camera &camera,
+                                   const Stars &stars,
+                                   const Catalog &catalog,
+                                   const StarIdentifiers &starIdentifiers) {
 
-            //Weight = 1 (can be changed later, in which case we want to make a vector to hold all weights {ai})
-            //Calculate matrix B = sum({ai}{bi}{ri}T)
-            B += ri * bi.transpose() * s.weight;
-        }
-        // S = B + Transpose(B)
-        Eigen::Matrix3f S = B + B.transpose();
-        //sigma = B[0][0] + B[1][1] + B[2][2]
-        float sigma = B.trace();
-        //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
-        Eigen::Vector3f Z;
-        Z << B(1,2) - B(2,1), 
-            B(2,0) - B(0,2), 
-            B(0,1) - B(1,0);
-        //K =  [[[sigma], [Z[0]], [Z[1]], [Z[2]]], [[Z[0]], [S[0][0] - sigma], [S[0][1]], [S[0][2]]], [[Z[1]], [S[1][0]], [S[1][1] - sigma], [S[1][2]]], [[Z[2]], [S[2][0]], [S[2][1]], [S[2][2] - sigma]]]
-        Eigen::Matrix4f K;
-        K << sigma, Z(0), Z(1), Z(2), 
-            Z(0), S(0,0) - sigma, S(0,1), S(0,2),
-            Z(1), S(1,0), S(1,1) - sigma, S(1,2),
-            Z(2), S(2,0), S(2,1), S(2,2) - sigma;
-        //Find eigenvalues of K, store the largest one as lambda 
-        //find the maximum index
-        Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
-        Eigen::Vector4cf values = solver.eigenvalues();
-        Eigen::Matrix4cf vectors = solver.eigenvectors();
-        int maxIndex = 0;
-        std::complex<float> maxIndexValue = values(0);
-        for (int i = 1; i < values.size(); i++) {
-            if (values(i).real() > maxIndexValue.real()) {
-                maxIndex = i;
-            }
+    //create a vector that'll hold {bi} (Stars in our frame)
+    //create a vector that'll hold {ri} (Stars in catalog frame)
+    Eigen::Matrix3f B;
+    B.setZero();
+    for (const StarIdentifier &s: starIdentifiers) {
+        Star bStar = stars[s.starIndex];
+        Vec3 bStarSpatial = camera.CameraToSpatial({bStar.x, bStar.y});
+        Eigen::Vector3f bi;
+        bi << bStarSpatial.y, bStarSpatial.x, bStarSpatial.z;
+
+        CatalogStar rStar = catalog[s.catalogIndex];
+        Eigen::Vector3f ri;
+        ri << cos(rStar.raj2000)*cos(rStar.dej2000),
+            sin(rStar.raj2000)*cos(rStar.dej2000),
+            sin(rStar.dej2000);
+
+        //Weight = 1 (can be changed later, in which case we want to make a vector to hold all weights {ai})
+        //Calculate matrix B = sum({ai}{bi}{ri}T)
+        B += ri * bi.transpose() * s.weight;
+    }
+    // S = B + Transpose(B)
+    Eigen::Matrix3f S = B + B.transpose();
+    //sigma = B[0][0] + B[1][1] + B[2][2]
+    float sigma = B.trace();
+    //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
+    Eigen::Vector3f Z;
+    Z << B(1,2) - B(2,1), 
+        B(2,0) - B(0,2), 
+        B(0,1) - B(1,0);
+    //K =  [[[sigma], [Z[0]], [Z[1]], [Z[2]]], [[Z[0]], [S[0][0] - sigma], [S[0][1]], [S[0][2]]], [[Z[1]], [S[1][0]], [S[1][1] - sigma], [S[1][2]]], [[Z[2]], [S[2][0]], [S[2][1]], [S[2][2] - sigma]]]
+    Eigen::Matrix4f K;
+    K << sigma, Z(0), Z(1), Z(2), 
+        Z(0), S(0,0) - sigma, S(0,1), S(0,2),
+        Z(1), S(1,0), S(1,1) - sigma, S(1,2),
+        Z(2), S(2,0), S(2,1), S(2,2) - sigma;
+    //Find eigenvalues of K, store the largest one as lambda 
+    //find the maximum index
+    Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
+    Eigen::Vector4cf values = solver.eigenvalues();
+    Eigen::Matrix4cf vectors = solver.eigenvectors();
+    int maxIndex = 0;
+    std::complex<float> maxIndexValue = values(0);
+    for (int i = 1; i < values.size(); i++) {
+        if (values(i).real() > maxIndexValue.real()) {
+            maxIndex = i;
         }
-        //The return quaternion components = eigenvector assocaited with lambda 
-        Eigen::Vector4cf maxAmount = vectors.col(maxIndex);
-        // IMPORTANT: The matrix K is symmetric -- clearly first row and first column are equal.
-        // Furthermore, S is symmetric because s_i,j = b_i,j + b_j,i and s_j,i=b_j,i + b_i,j, so the
-        // bottom right 3x3 block of K is symmetric too. Thus all its eigenvalues and eigenvectors
-        // are real.
-        return Quaternion(maxAmount[0].real(), maxAmount[1].real(), maxAmount[2].real(), maxAmount[3].real());
     }
+    //The return quaternion components = eigenvector assocaited with lambda 
+    Eigen::Vector4cf maxAmount = vectors.col(maxIndex);
+    // IMPORTANT: The matrix K is symmetric -- clearly first row and first column are equal.
+    // Furthermore, S is symmetric because s_i,j = b_i,j + b_j,i and s_j,i=b_j,i + b_i,j, so the
+    // bottom right 3x3 block of K is symmetric too. Thus all its eigenvalues and eigenvectors
+    // are real.
+    return Quaternion(maxAmount[0].real(), maxAmount[1].real(), maxAmount[2].real(), maxAmount[3].real());
+}
+
 }

src/attitude-utils.cpp

@@ -2,6 +2,7 @@
 
 #include <math.h>
 #include <assert.h>
+#include <iostream>
 
 namespace lost {
 
@@ -64,6 +65,14 @@ Quaternion SphericalToQuaternion(float ra, float dec, float roll) {
     return (a*b*c).Conjugate();
 }
 
+Vec3 SphericalToSpatial(float ra, float de) {
+    return {
+        cos(ra)*cos(de),
+        sin(ra)*cos(de),
+        sin(de),
+    };
+}
+
 float RadToDeg(float rad) {
     return rad*180.0/M_PI;
 }
@@ -85,4 +94,29 @@ float GreatCircleDistance(float ra1, float de1, float ra2, float de2) {
                          + cos(de1)*cos(de2)*pow(sin(abs(ra1-ra2)/2.0), 2.0)));
 }
 
+float Vec3::Magnitude() const {
+    return sqrt(x*x+y*y+z*z);
+}
+
+Vec3 Vec3::Normalize() const {
+    float mag = Magnitude();
+    return {
+        x/mag, y/mag, z/mag,
+    };
+}
+
+float Vec3::operator*(const Vec3 &other) const {
+    return x*other.x + y*other.y + z*other.z;
+}
+
+float Angle(const Vec3 &vec1, const Vec3 &vec2) {
+    return AngleUnit(vec1.Normalize(), vec2.Normalize());
+}
+
+float AngleUnit(const Vec3 &vec1, const Vec3 &vec2) {
+    // TODO: we shouldn't need this nonsense, right? how come acos sometimes gives nan?
+    float dot = vec1*vec2;
+    return dot >= 1 ? 0 : dot <= -1 ? -M_PI : acos(dot);
+}
+
 }

src/attitude-utils.hpp

@@ -14,10 +14,16 @@ struct Vec2 {
     float y;
 };
 
-struct Vec3 {
+class Vec3 {
+public:
     float x;
     float y;
     float z;
+
+    float Magnitude() const;
+    Vec3 Normalize() const;
+
+    float operator*(const Vec3 &) const;
 };
 
 class Quaternion {
@@ -46,6 +52,12 @@ class Quaternion {
 // will appear to rotate clockwise). This is an "improper" z-y'-x' Euler rotation.
 Quaternion SphericalToQuaternion(float ra, float dec, float roll);
 
+Vec3 SphericalToSpatial(float ra, float de);
+// 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
+float AngleUnit(const Vec3 &, const Vec3 &);
+
 float RadToDeg(float);
 float DegToRad(float);
 float RadToArcSec(float);

src/camera.cpp

@@ -6,36 +6,58 @@
 
 namespace lost {
 
-Vec2 Camera::ConvertCoordinates(const Vec3 &vector) const {
+Vec2 Camera::SpatialToCamera(const Vec3 &vector) const {
     // can't handle things behind the camera.
     assert(vector.x > 0);
-    assert(xFov > 0 && xFov < M_PI);
     // TODO: is there any sort of accuracy problem when vector.y and vector.z are small?
+
     float yTangent = vector.y/vector.x;
     float zTangent = vector.z/vector.x;
-    // these pixels are measured using 0,0 as the /center/
 
-    // TODO: analyze off-by-one errors (is fov for the center of the pixels, in which case we should
-    // use xResolution-1??)
-    float yPixel = yTangent/tan(xFov/2)*(xResolution-1)/2;
-    float zPixel = zTangent/tan(xFov/2)*(xResolution-1)/2;
+    float yPixel = yTangent*xFocalLength;
+    float zPixel = zTangent*yFocalLength;
 
-    // now convert to using 0,0 as the top left, as usual. TODO: analyze off-by-one errors here too.
-    // Right now, if we have 5x5, the center becomes (2,2), and if we have 4x4, the center becomes
-    // (1.5, 1.5). I think this is right?
-    return { -yPixel + (xResolution-1)/2.0f, -zPixel + (yResolution-1)/2.0f };
+    return { -yPixel + xCenter, -zPixel + yCenter };
 }
 
-void Camera::CoordinateAngles(const Vec2 &vector, float *ra, float *de) const {
-    // TODO: off-by-one with xResolution - 1?
-    // TODO: minimize floating point error?
-    *ra = atan((xResolution/2.0-vector.x)/(xResolution/2.0/tan(xFov/2.0)));
-    *de = atan((yResolution/2.0-vector.y)/(xResolution/2.0/tan(xFov/2.0)));
+// we'll just place the points at 1 unit away from the pinhole (x=1)
+Vec3 Camera::CameraToSpatial(const Vec2 &vector) const {
+    assert(InSensor(vector));
+
+    // isn't it interesting: To convert from center-based to left-corner-based coordinates is the
+    // same formula; f(x)=f^{-1}(x) !
+    float xPixel = -vector.x + xCenter;
+    float yPixel = -vector.y + yCenter;
+
+    return {
+        1,
+        xPixel / xFocalLength,
+        yPixel / yFocalLength,
+    };
 }
 
+// TODO: reimplement or determine we don't need it
+// void Camera::CoordinateAngles(const Vec2 &vector, float *ra, float *de) const {
+//     // TODO: off-by-one with xResolution - 1?
+//     // TODO: minimize floating point error?
+
+//     // *ra = atan((xResolution/2.0-vector.x)/(xResolution/2.0/tan(xFov/2.0)));
+//     // *de = atan((yResolution/2.0-vector.y)/(xResolution/2.0/tan(xFov/2.0)));
+// }
+
 bool Camera::InSensor(const Vec2 &vector) const {
+    // if vector.x == xResolution, then it is at the leftmost point of the pixel that's "hanging
+    // off" the edge of the image, so vector is still in the image.
     return vector.x >= 0 && vector.x <= xResolution
         && vector.y >= 0 && vector.y <= yResolution;
 }
 
+int Camera::GetXResolution() const {
+    return xResolution;
+}
+
+int Camera::GetYResolution() const {
+    return yResolution;
+}
+
 }

src/camera.hpp

@@ -7,24 +7,40 @@ namespace lost {
 
 class Camera {
 public:
-    Camera() = default;
-    Camera(float xFov, int xResolution, int yResolution)
-        : xFov(xFov), xResolution(xResolution), yResolution(yResolution) { };
+    // Takes focal lengths in pixels
+    Camera(float xFocalLength, float yFocalLength,
+           float xCenter, float yCenter,
+           int xResolution, int yResolution)
+        : xFocalLength(xFocalLength), yFocalLength(yFocalLength),
+          xCenter(xCenter), yCenter(yCenter),
+          xResolution(xResolution), yResolution(yResolution) { };
+    Camera(float xFocalLength, int xResolution, int yResolution)
+        : Camera(xFocalLength, xFocalLength,
+                 xResolution/(float)2.0, yResolution/(float)2.0,
+                 xResolution, yResolution) { };
 
     // Converts from a 3D point in space to a 2D point on the camera sensor. Assumes that X is the
     // depth direction and that it points away from the center of the sensor, i.e., any vector (x,
     // 0, 0) will be at (xResolution/2, yResolution/2) on the sensor.
-    Vec2 ConvertCoordinates(const Vec3 &vector) const;
+    Vec2 SpatialToCamera(const Vec3 &) const;
+    // Gives /a/ point in 3d space that could correspond to the given vector, using the same
+    // coordinate system described for SpatialToCamera. Not all vectors returned by this function
+    // will necessarily have the same magnitude.
+    Vec3 CameraToSpatial(const Vec2 &) const;
     // converts from a 2d point in the camera sensor to right ascension and declination relative to
     // the center of the camera.
-    void CoordinateAngles(const Vec2 &vector, float *ra, float *de) const;
+    // void CoordinateAngles(const Vec2 &vector, float *ra, float *de) const;
     // returns whether a given pixel is actually in the camera's field of view
     bool InSensor(const Vec2 &vector) const;
 
-    float xFov;
-    int xResolution;
-    int yResolution;
+    int GetXResolution() const;
+    int GetYResolution() const;
+
+private:
     // TODO: distortion
+    float xFocalLength; float yFocalLength;
+    float xCenter; float yCenter;
+    int xResolution; int yResolution;
 };
 
 }