3#include <eigen3/Eigen/Dense>
4#include <eigen3/Eigen/Eigenvalues>
10#define EPSILON DECIMAL(0.0001)
11#define QUEST_MAX_ITERATIONS 20
17 if (starIdentifiers.size() < 2) {
20 assert(stars.size() >= 2);
23 #ifdef LOST_FLOAT_MODE
32 Star bStar = stars[s.starIndex];
35 #ifdef LOST_FLOAT_MODE
43 bi << bStarSpatial.
x, bStarSpatial.
y, bStarSpatial.
z;
49 B += ri * bi.transpose() * s.weight;
53 #ifdef LOST_FLOAT_MODE
54 Eigen::Matrix3f S = B + B.transpose();
56 Eigen::Matrix3d S = B + B.transpose();
63 #ifdef LOST_FLOAT_MODE
74 #ifdef LOST_FLOAT_MODE
80 K << sigma, Z(0), Z(1), Z(2),
81 Z(0), S(0,0) - sigma, S(0,1), S(0,2),
82 Z(1), S(1,0), S(1,1) - sigma, S(1,2),
83 Z(2), S(2,0), S(2,1), S(2,2) - sigma;
87 #ifdef LOST_FLOAT_MODE
88 Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
89 Eigen::Vector4cf values = solver.eigenvalues();
90 Eigen::Matrix4cf vectors = solver.eigenvectors();
92 Eigen::EigenSolver<Eigen::Matrix4d> solver(K);
93 Eigen::Vector4cd values = solver.eigenvalues();
94 Eigen::Matrix4cd vectors = solver.eigenvectors();
98 decimal maxEigenvalue = values(0).real();
99 for (
int i = 1; i < values.size(); i++) {
100 if (values(i).real() > maxEigenvalue) {
102 maxEigenvalue = values(i).real();
107 #ifdef LOST_FLOAT_MODE
108 Eigen::Vector4cf maxEigenvector = vectors.col(maxIndex);
110 Eigen::Vector4cd maxEigenvector = vectors.col(maxIndex);
118 maxEigenvector[1].real(),
119 maxEigenvector[2].real(),
120 maxEigenvector[3].real()));
139 if (starIds.size() < 2) {
142 assert(stars.size() >= 2);
147 b = starIds[starIds.size()/2];
152 catalog[b.catalogIndex].spatial);
194 if (starIdentifiers.size() < 2) {
197 assert(stars.size() >= 2);
203 Mat3 B = {0, 0, 0, 0, 0, 0, 0, 0, 0};
205 Star bStar = stars[s.starIndex];
213 B = B + (rStarSpatial.
OuterProduct(bStarSpatial) * s.weight);
225 B.
At(1,2) - B.
At(2,1),
226 B.
At(2,0) - B.
At(0,2),
227 B.
At(0,1) - B.
At(1,0)
235 decimal c = delta + (Z * S * Z);
248 decimal gamma = (eig + sigma) * alpha - delta;
#define QUEST_MAX_ITERATIONS
The attitude (orientation) of a spacecraft.
A full description of a camera. Enough information to reconstruct the camera matrix and then some.
Vec3 CameraToSpatial(const Vec2 &) const
Gives a point in 3d space that could correspond to the given vector, using the same coordinate system...
A star from the Bright Star Catalog.
Vec3 spatial
The point on the unit sphere where the star lies.
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
3x3 vector with decimaling point components
Mat3 Inverse() const
Inverse of a matrix.
decimal Det() const
Determinant of a matrix.
Mat3 Transpose() const
Transpose of a matrix.
decimal Trace() const
Trace of a matrix.
decimal At(int i, int j) const
Access the i,j-th element of the matrix.
A quaternion is a common way to represent a 3d rotation.
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
Records that a certain Star (detected in the image) corresponds to a certain CatalogStar.
int catalogIndex
An index into an array of CatalogStar objects.
int starIndex
An index into an array of Star objects.
A "centroid" detected in an image.
Vec2 position
The (x,y) pixel coordinates in the image (top left is 0,0).
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
Three dimensional vector with decimaling point components.
Vec3 Normalize() const
Create a vector pointing in the same direction with magnitude 1.
decimal MagnitudeSq() const
The square of the magnitude.
Mat3 OuterProduct(const Vec3 &) const
The outer product of two vectors.
Vec3 CrossProduct(const Vec3 &) const
Usual vector cross product.
#define DECIMAL_POW(base, power)
std::vector< StarIdentifier > StarIdentifiers
std::vector< Star > Stars
const Mat3 kIdentityMat3
3x3 identity matrix
bool QuestEigenvalueEstimator(decimal guess, decimal a, decimal b, decimal c, decimal d, decimal s, decimal *eig)
Approximates roots of a real function using the Newton-Raphson algorithm.
decimal QuestCharPoly(decimal x, decimal a, decimal b, decimal c, decimal d, decimal s)
Characteristic polynomial of the quest K-matrix.
decimal QuestCharPolyPrime(decimal x, decimal a, decimal b, decimal c)
Derivitive of the characteristic polynomial of the quest K-matrix.
std::vector< CatalogStar > Catalog