diff --git a/src/distance/distance.cpp b/src/distance/distance.cpp new file mode 100644 index 0000000..bf5a560 --- /dev/null +++ b/src/distance/distance.cpp @@ -0,0 +1,71 @@ +#include +#include +#include + +#include "spatial/attitude-utils.hpp" +#include "spatial/camera.hpp" +#include "style/style.hpp" + +#include "distance/distance.hpp" + +namespace found { + +PositionVector SphericalDistanceDeterminationAlgorithm::Run(const Points &p) { + // We do not normalize here, because the assumption for getting the distance + // is dependent on the fact that vec.x is 1. It is also not cost effective + // as the tradeoff is to nromalize center instead (3 norms v. 1 norm) + Vec3 spats[3] = {cam_.CameraToSpatial(p[0]), + cam_.CameraToSpatial(p[1]), + cam_.CameraToSpatial(p[2])}; + + // Obtain the center point of the projected circle + Vec3 center = std::move(getCenter(spats)); + + // Obtain the radius of the projected circle + decimal r = getRadius(spats, center); + + // Obtain the distance from earth + decimal h = getDistance(r); + + // You have to normalize the center vector here + return center.Normalize() * h; +} + +Vec3 SphericalDistanceDeterminationAlgorithm::getCenter(Vec3 spats[3]) { + Vec3 diff1 = std::move(spats[1] - spats[0]); + Vec3 diff2 = std::move(spats[2] - spats[1]); + + Vec3 circleN = std::move(diff1.CrossProduct(diff2)); + Vec3 circlePt = spats[0]; + + Vec3 mid1 = std::move(midpoint(spats[0], spats[1])); + Vec3 mid2 = std::move(midpoint(spats[1], spats[2])); + + Vec3 mid1N = std::move(diff1); + Vec3 mid2N = std::move(diff2); + + Mat3 matrix; + matrix = {circleN.x, circleN.y, circleN.z, mid1N.x, mid1N.y, + mid1N.z, mid2N.x, mid2N.y, mid2N.z}; + + decimal alpha = circleN*circlePt; + decimal beta = mid1N*mid1; + decimal gamma = mid2N*mid2; + + Vec3 y = {alpha, beta, gamma}; + + Vec3 center = std::move(matrix.Inverse() * y); + + return center; +} + +decimal SphericalDistanceDeterminationAlgorithm::getRadius(Vec3* spats, +Vec3 center) { + return Distance(spats[0], center); +} + +decimal SphericalDistanceDeterminationAlgorithm::getDistance(decimal r) { + return radius_*sqrt(r * r + 1)/r; +} + +} // namespace found diff --git a/src/distance/distance.hpp b/src/distance/distance.hpp index 3778feb..2b4f3fe 100644 --- a/src/distance/distance.hpp +++ b/src/distance/distance.hpp @@ -3,6 +3,8 @@ #include "style/style.hpp" #include "pipeline/pipeline.hpp" +#include "spatial/attitude-utils.hpp" +#include "spatial/camera.hpp" namespace found { @@ -10,12 +12,12 @@ namespace found { * The DistanceDeterminationAlgorithm class houses the Distance Determination Algorithm. This * algorithm calculates the distance from Earth based on the pixels of Earth's Edge found in the image. */ -class DistanceDeterminationAlgorithm : public Stage { +class DistanceDeterminationAlgorithm : public Stage { public: // Constructs this DistanceDeterminationAlgorithm() = default; // Destroys this - virtual ~DistanceDeterminationAlgorithm(); + virtual ~DistanceDeterminationAlgorithm() = default; }; /** @@ -26,20 +28,61 @@ class DistanceDeterminationAlgorithm : public Stage { */ class SphericalDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm { public: - /** - * Initializes this SphericalDistanceDeterminationAlgorithm - * - * @param radius The radius of Earth to use - */ - explicit SphericalDistanceDeterminationAlgorithm(float radius); - ~SphericalDistanceDeterminationAlgorithm(); + /** + * Creates a SphericalDeterminationAlgorithm, which deduces + * the Position vector of a sattelite from Earth by modeling + * Earth as a sphere + * + * @param radius The radius of Earth + * @param cam The camera used to capture the picture of Earth + */ + SphericalDistanceDeterminationAlgorithm(float radius, Camera &cam) : cam_(cam), radius_(radius) {} + ~SphericalDistanceDeterminationAlgorithm() {} /** * Place documentation here. Press enter to automatically make a new line * */ - distFromEarth Run(const Points &p) override; + PositionVector Run(const Points &p) override; + private: // Fields specific to this algorithm, and helper methods + /** + *Returns the center of earth as a 3d Vector + * + * @param spats The normalized spatial coordinates used to find the center + * + * @return The center of earth as a 3d Vector + */ + Vec3 getCenter(Vec3* spats); + + /** + * Returns the radius of the calculated "earth" (normalized) + * + * @param spats The normalized spatial coordinates + * @param center The center of the earth as a 3d Vector + * + * @return The radius of earth normalized + */ + decimal getRadius(Vec3* spats, Vec3 center); + + /** + * Returns the scaled distance from earth + * + * @param r The normalized radius + * + * @return The distance from earth as a Scalar + */ + decimal getDistance(decimal r); + + /** + * cam_ field instance describes the camera settings used for the photo taken + */ + Camera cam_; + + /** + * radius_ field instance describes the defined radius of earth. Should be 6378.0 (km) + */ + float radius_; }; /** @@ -55,17 +98,17 @@ class EllipticDistanceDeterminationAlgorithm : public DistanceDeterminationAlgor * * @param radius The distance from Earth to use */ - explicit EllipticDistanceDeterminationAlgorithm(distFromEarth radius); + explicit EllipticDistanceDeterminationAlgorithm(PositionVector radius); ~EllipticDistanceDeterminationAlgorithm(); /** * Place documentation here. Press enter to automatically make a new line * */ - distFromEarth Run(const Points &p) override; + PositionVector Run(const Points &p) override; private: // Fields specific to this algorithm, and helper methods }; } // namespace found -#endif +#endif // DISTANCE_H diff --git a/src/spatial/attitude-utils.cpp b/src/spatial/attitude-utils.cpp index 659dfd9..5ee0f68 100644 --- a/src/spatial/attitude-utils.cpp +++ b/src/spatial/attitude-utils.cpp @@ -257,7 +257,6 @@ decimal DegToRad(decimal deg) { * @pre rad is in radians * * @warning rad must be in radians - * */ decimal RadToArcSec(decimal rad) { return RadToDeg(rad) * 3600.0; @@ -271,7 +270,6 @@ decimal RadToArcSec(decimal rad) { * * @return A possible angle value, in radians, corresponding * to the arcsecant value arcSec - * */ decimal ArcSecToRad(decimal arcSec) { return DegToRad(arcSec / 3600.0); @@ -384,7 +382,6 @@ Vec3 Vec3::operator-(const Vec3 &other) const { * * @return A vector that is the cross product between * this and other - * */ Vec3 Vec3::CrossProduct(const Vec3 &other) const { return { @@ -401,7 +398,6 @@ Vec3 Vec3::CrossProduct(const Vec3 &other) const { * * @return A matrix that is the outer product between this * and other - * */ Mat3 Vec3::OuterProduct(const Vec3 &other) const { return { @@ -432,6 +428,44 @@ Vec3 Vec3::operator*(const Mat3 &other) const { ///// VECTOR UTILITY FUNCTIONS //// /////////////////////////////////// +/** + * Finds the midpoint between two different vectors + * + * @param vec1 The first vector + * @param vec2 The second vector + * + * @return The midpoint vector +*/ +Vec2 midpoint(const Vec2 &vec1, const Vec2 &vec2) { + return {(vec1.x + vec2.x)/2, (vec1.y + vec2.y)/2}; +} + +/** + * Finds the midpoint between two different vectors + * + * @param vec1 The first vector + * @param vec2 The second vector + * + * @return The midpoint vector +*/ +Vec3 midpoint(const Vec3 &vec1, const Vec3 &vec2) { + return {(vec1.x + vec2.x)/2, (vec1.y + vec2.y)/2, (vec1.z + vec2.z)/2}; +} + +/** + * Finds the midpoint between three different vectors + * + * @param vec1 The first vector + * @param vec2 The second vector + * @param vec3 The third vector + * + * @return The midpoint vector +*/ +Vec3 midpoint(const Vec3 &vec1, const Vec3 &vec2, const Vec3 &vec3) { + return {(vec1.x + vec2.x + vec3.x)/3, (vec1.y + vec2.y + vec3.y)/3, (vec1.z + vec2.z + vec3.z)/3}; +} + + /** * Determines the angle between two different vectors * @@ -439,7 +473,6 @@ Vec3 Vec3::operator*(const Mat3 &other) const { * @param vec2 The second vector * * @return The angle, in radians, between vec1 and vec2 - * */ decimal Angle(const Vec3 &vec1, const Vec3 &vec2) { return AngleUnit(vec1.Normalize(), vec2.Normalize()); @@ -464,29 +497,27 @@ decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2) { } /** - * Determines the distance between two vectors + * Determines the Distance between two vectors * * @param v1 The first vector * @param v2 The second vector * * @return The distance between v1 and v2 - * */ decimal Distance(const Vec2 &v1, const Vec2 &v2) { - return sqrt(pow(v1.x-v2.x, 2) + pow(v1.y-v2.y, 2)); + return (v1-v2).Magnitude(); } /** - * Determines the distance between two vectors + * Determines the Distance between two vectors * * @param v1 The first vector * @param v2 The second vector * * @return The distance between v1 and v2 - * */ decimal Distance(const Vec3 &v1, const Vec3 &v2) { - return sqrt(pow(v1.x-v2.x, 2) + pow(v1.y-v2.y, 2) + pow(v1.z-v2.z, 2)); + return (v1-v2).Magnitude(); } /////////////////////////////////// @@ -500,7 +531,6 @@ decimal Distance(const Vec3 &v1, const Vec3 &v2) { * @param j The column of the entry * * @return The value of the entry in this at (i, j) - * */ decimal Mat3::At(int i, int j) const { return x[3*i+j]; @@ -570,7 +600,6 @@ Mat3 Mat3::operator*(const decimal &s) const { * Obtains the transpose of this Matrix * * @return The transpose Matrix of this - * */ Mat3 Mat3::Transpose() const { return { @@ -584,7 +613,6 @@ Mat3 Mat3::Transpose() const { * Obtains the trace of this Matrix * * @return The trace of this - * */ decimal Mat3::Trace() const { return At(0,0) + At(1,1) + At(2,2); @@ -594,7 +622,6 @@ decimal Mat3::Trace() const { * Obtains the determinant of this Matrix * * @return The determinant of this - * */ decimal Mat3::Det() const { return (At(0,0) * (At(1,1)*At(2,2) - At(2,1)*At(1,2))) - @@ -605,8 +632,7 @@ decimal Mat3::Det() const { /** * Obtains the inverse of this Matrix * - * @return The inverse Matrix of this - * + * @return The inverse Matrix of this */ Mat3 Mat3::Inverse() const { // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ @@ -634,8 +660,7 @@ const Mat3 kIdentityMat3 = {1,0,0, /** * Constructs an Attitude object from Quaternion information * - * @param quat The quaternion to base the attitude off of - * + * @param quat The quaternion to base the attitude off of */ Attitude::Attitude(const Quaternion &quat) : quaternion(quat), type(QuaternionType) {} @@ -645,7 +670,6 @@ Attitude::Attitude(const Quaternion &quat) * matrix holding the direction cosines for an attitude) * * @param matrix The matrix holding the direction cosines - * */ Attitude::Attitude(const Mat3 &matrix) : dcm(matrix), type(DCMType) {} @@ -679,7 +703,6 @@ Mat3 QuaternionToDCM(const Quaternion &quat) { * @param dcm The matrix holding the direction cosines * * @return A Quaternion that expresses the rotation defined in dcm - * */ Quaternion DCMToQuaternion(const Mat3 &dcm) { // Make a quaternion that rotates the reference frame X-axis into the dcm's X-axis, just like @@ -714,7 +737,6 @@ Quaternion DCMToQuaternion(const Mat3 &dcm) { * * @return A Quaternion that holds the attitude information * of this - * */ Quaternion Attitude::GetQuaternion() const { switch (type) { @@ -732,7 +754,6 @@ Quaternion Attitude::GetQuaternion() const { * * @return A matrix containing the direction cosines * indicated by this - * */ Mat3 Attitude::GetDCM() const { switch (type) { @@ -752,7 +773,6 @@ Mat3 Attitude::GetDCM() const { * @param vec The vector to rotate * * @return A new vector that is rotated from vec based on this - * */ Vec3 Attitude::Rotate(const Vec3 &vec) const { switch (type) { @@ -770,7 +790,6 @@ Vec3 Attitude::Rotate(const Vec3 &vec) const { * * @return An EulerAngles object that holds the Euler * Angles of this - * */ EulerAngles Attitude::ToSpherical() const { switch (type) { @@ -791,7 +810,6 @@ EulerAngles Attitude::ToSpherical() const { * Computes the size, in bytes, that a Vec3 object will take up * * @return The number of bytes that a Vec3 occupies - * */ int64_t SerializeLengthVec3() { return sizeof(decimal)*3; @@ -809,7 +827,6 @@ int64_t SerializeLengthVec3() { * @note A buffer is a very long character array that holds information * that the user defines. Serialization of data means inputting certain * data into a buffer. - * */ void SerializeVec3(const Vec3 &vec, unsigned char *buffer) { decimal *fBuffer = reinterpret_cast(buffer); @@ -831,7 +848,6 @@ void SerializeVec3(const Vec3 &vec, unsigned char *buffer) { * * @warning Returns nonsense if buffer does not point to a valid location * that stores a Vec3 - * */ Vec3 DeserializeVec3(const unsigned char *buffer) { Vec3 result; diff --git a/src/spatial/attitude-utils.hpp b/src/spatial/attitude-utils.hpp index 1931f93..4dc9f21 100644 --- a/src/spatial/attitude-utils.hpp +++ b/src/spatial/attitude-utils.hpp @@ -270,6 +270,11 @@ class Attitude { AttitudeType type; }; +// Vector operations +Vec2 midpoint(const Vec2 &, const Vec2 &); +Vec3 midpoint(const Vec3 &, const Vec3 &); +Vec3 midpoint(const Vec3 &, const Vec3 &, const Vec3 &); + // DCM-Quaternion-Spherical Conversions Mat3 QuaternionToDCM(const Quaternion &); diff --git a/src/spatial/camera.hpp b/src/spatial/camera.hpp index 5158e9a..e57f710 100644 --- a/src/spatial/camera.hpp +++ b/src/spatial/camera.hpp @@ -106,4 +106,4 @@ decimal FocalLengthToFov(decimal focalLength, decimal xResolution, decimal pixel } // namespace found -#endif +#endif // CAMERA_H diff --git a/src/style/style.hpp b/src/style/style.hpp index 937e84a..2a6ee46 100644 --- a/src/style/style.hpp +++ b/src/style/style.hpp @@ -18,10 +18,6 @@ namespace found { /// to a vector of 2D points on the image, according to image coordinate systems typedef std::vector Points; -/// The output for Distance Determination Algorithms (distance.hpp/cpp). Currently -/// set to a floating point value that represents the distance from Earth -typedef decimal distFromEarth; - /// The output for Vector Assembly Algorithms (vectorize.hpp). Currently set /// to a 3D Vector that represents the satellite's position relative to Earth's /// coordinate system. @@ -111,7 +107,6 @@ typedef struct OrbitParams OrbitParams; /// The output for Kinematic Profile Completion. Currently set to two functions that /// will tell you the position and velocity of the satellite at any given time typedef std::pair,std::function> KinematicPrediction; - } // namespace found -#endif +#endif // STYLE_H_ diff --git a/test/distance/distance-test.cpp b/test/distance/distance-test.cpp new file mode 100644 index 0000000..03ae52a --- /dev/null +++ b/test/distance/distance-test.cpp @@ -0,0 +1,89 @@ +#include + +#include +#include +#include + +#include "src/style/style.hpp" +#include "src/spatial/attitude-utils.hpp" +#include "src/spatial/camera.hpp" +#include "src/distance/distance.hpp" + + +/* Using Directives */ +using found::Camera; +using found::Vec3; +using found::Points; +using found::PositionVector; +using found::decimal; +using found::SphericalDistanceDeterminationAlgorithm; + + +/* Common Constants */ + + +// Radius of Earth (km) +#define RADIUS_OF_EARTH 6378.0 +// Default DoubleEquals Tolerance (So big because of floating point problems) +#define DEFAULT_TOLERANCE 1 + + +/* Test Macros */ + +/** + * Requires that vec1 == vec2 (using DecimalEquals) + * + * @param vec1 A Vec3 object + * @param vec2 A Vec3 object + * @param tolerance The tolerance for vec1 to be + * "equal" to vec2 + * + * @post Will have REQUIRE'd that vec1 is equal to + * vec2, on a component basis, within tolerance +*/ +#define VECTOR_EQUALS(vec1, vec2, tolerance) \ + EXPECT_LT(abs(vec1.x - vec2.x), tolerance); \ + EXPECT_LT(abs(vec1.y - vec2.y), tolerance); \ + EXPECT_LT(abs(vec1.z - vec2.z), tolerance); + +std::ostream &operator<<(std::ostream &stream, const Vec3 &vector) { + stream << std::fixed << std::setprecision(5) << "(" << vector.x << ", " << vector.y << ", " << vector.z << ")"; + return stream; +} + +// Base Case I: The image captured contains an edge centered about the image + +TEST(SphericalDistanceDeterminationAlgorithmTest, TestCenteredEarth) { + // Step I: Pick some distance (km) and a Camera + decimal x_E = RADIUS_OF_EARTH + 1000; + int imageWidth = 1024; + int imageHeight = 1024; + Camera cam(0.012, imageWidth, imageHeight); // Focal length of 12 m + PositionVector expected = {x_E, 0, 0}; + + // Step II: Figure out my projection points + + // a) Find the angle + decimal alpha = asin(RADIUS_OF_EARTH / x_E); + + // b) Find the distance away from each projection point + decimal p = sqrt(x_E * x_E - RADIUS_OF_EARTH * RADIUS_OF_EARTH); + + // c) Use 3 easy projections + Vec3 p1 = {static_cast(p * cos(alpha)), static_cast(p * sin(alpha)), 0}; + Vec3 p2 = {static_cast(p * cos(alpha)), static_cast(-p * sin(alpha)), 0}; + Vec3 p3 = {static_cast(p * cos(alpha)), 0, static_cast(p * sin(alpha))}; + + // Step III: Use CTS to convert to 2D vectors + Points pts = {cam.SpatialToCamera(p1), + cam.SpatialToCamera(p2), + cam.SpatialToCamera(p3)}; + + // Step IV: Run It and Test! + SphericalDistanceDeterminationAlgorithm algo = + SphericalDistanceDeterminationAlgorithm(RADIUS_OF_EARTH, cam); + + PositionVector actual = algo.Run(pts); + + VECTOR_EQUALS(expected, actual, DEFAULT_TOLERANCE); +}