FOUND Coverage Report

src/
File: distance/distance.hpp
Date: 2025-11-20 21:57:26
Lines:
13/13
100.0%
Functions:
6/6
100.0%
Branches:
4/4
100.0%
Line Branch Exec Source
1 #ifndef SRC_DISTANCE_DISTANCE_HPP_
2 #define SRC_DISTANCE_DISTANCE_HPP_
3
4 #include <utility>
5 #include <memory>
6
7 #include "common/style.hpp"
8 #include "common/pipeline/stages.hpp"
9 #include "common/spatial/attitude-utils.hpp"
10 #include "common/spatial/camera.hpp"
11
12 namespace found {
13
14 /**
15 * The DistanceDeterminationAlgorithm class houses the Distance Determination Algorithm. This
16 * algorithm calculates the distance from Earth based on the pixels of Earth's Edge found in the image.
17 *
18 * @note This algorithm performs optimally when the given Points is in polar order, i.e.
19 * if we define the centroid of the points as P, for any
20 * three consecutive points A B and C, angle APB is less than
21 * angle APC
22 */
23 class DistanceDeterminationAlgorithm : public FunctionStage<Points, PositionVector> {
24 public:
25 // Constructs this
26 46 DistanceDeterminationAlgorithm() = default;
27 // Destroys this
28 92 virtual ~DistanceDeterminationAlgorithm() {}
29 };
30
31 /**
32 * The DistanceDeterminationAlgorithm class houses the Distance Determination Algorithm. This
33 * algorithm calculates the distance from Earth based on the pixels of Earth's Edge found in the image.
34 *
35 * @note This class assumes that Earth is a perfect sphere
36 */
37 class SphericalDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
38 public:
39 /**
40 * Creates a SphericalDeterminationAlgorithm, which deduces
41 * the Position vector of a sattelite from Earth by modeling
42 * Earth as a sphere
43 *
44 * @param radius The radius of Earth
45 * @param cam The camera used to capture the picture of Earth
46 */
47 41 SphericalDistanceDeterminationAlgorithm(decimal radius, Camera &&cam) : cam_(cam), radius_(radius) {}
48 96 ~SphericalDistanceDeterminationAlgorithm() {}
49
50 /**
51 * Obtains the position of the planet relative to the camera
52 *
53 * @param p The points on the edge of Earth (in the image taken
54 * by the camera given to this)
55 *
56 * @return PositionVector The position vector of the Earth relative
57 * to the camera
58 *
59 * @pre p must refer to points taken by the camera that was passed to
60 * this
61 *
62 * @post If p.size() < 3, then the result is exactly the zero vector
63 * */
64 PositionVector Run(const Points &p) override;
65
66 protected:
67 /**
68 * Returns the center of earth as a 3d Vector
69 *
70 * @param a The first point on the edge of Earth
71 * @param b The second point on the edge of Earth
72 * @param c The third point on the edge of Earth
73 *
74 * @return The center of earth as a 3d Vector
75 *
76 * @pre spats.size() >= 3
77 *
78 * @pre a, b and c are normalized, projected points from the camera
79 */
80 Vec3 getCenter(const Vec3 &a, const Vec3 &b, const Vec3 &c);
81
82 /**
83 * Returns the position of the planet relative to the camera
84 *
85 * @param a The first point on the edge of Earth
86 * @param b The second point on the edge of Earth
87 * @param c The third point on the edge of Earth
88 *
89 * @return PositionVector The position vector of the Earth relative
90 * to the camera
91 *
92 * @pre p must refer to points taken by the camera that was passed to
93 * this
94 *
95 * @pre p.size() >= 3
96 *
97 * @pre a, b and c are normalized, projected points from the camera
98 */
99 PositionVector Run(const Vec3 &a, const Vec3 &b, const Vec3 &c);
100
101 /**
102 * cam_ field instance describes the camera settings used for the photo taken
103 */
104 Camera cam_;
105
106 /**
107 * radius_ field instance describes the defined radius of earth. Should be 6378.0 (km)
108 */
109 decimal radius_;
110
111 /**
112 * Calculated center vector
113 */
114 Vec3 center_;
115
116 /**
117 * Calculated radius
118 */
119 decimal r_;
120 };
121
122 /**
123 * The IterativeSphericalDistanceDeterminationAlgorithm is a variation of the
124 * SphericalDistanceDeterminationAlgorithm algorithm in that it runs it repeatedly
125 * to use all the points given to it.
126 *
127 * It uses
128 * - selective randomization of Points, using a even polynomial
129 * distributions to prioritize points farther from selected
130 * points within triplets
131 * - loss criterion to evaluate each guess
132 * - softmax activation to figure out the plausibility of each guess
133 *
134 * @note Testing data on `test/common/assets/example_earth1.png`:
135 *
136 * SDDA -> (1.0456e+07, -67903.8, -972.935) m
137 * - Distance Error: 0.752384891562%
138 * - Angle Error: 1339.6805912772 arcseconds
139 * - Execution Time: < 1 sec
140 *
141 * ISDDA(100000, 0.8, INF, Quadratic Radius Loss AND Randomization) -> (1.0384e+07, -12571.3, -1057.05) m
142 * - Distance Error: 0.0565676042517%
143 * - Angle Error: 250.59497104116 arcseconds
144 * - Execution Time: 11 sec
145 *
146 * ISDDA(100000, 0.8, INF, Quartic Radius Loss OR Randomization) -> (1.03781e+07, -11536.7, -927.331) m
147 * - Distance Error: 0.000294332681557%
148 * - Angle Error: 230.031583013 arcseconds
149 * - Execution Time: 11 sec
150 *
151 * In optimized mode (-O3), all algorithms are less than 1 second.
152 */
153 class IterativeSphericalDistanceDeterminationAlgorithm : public SphericalDistanceDeterminationAlgorithm {
154 public:
155 /**
156 * Creates a IterativeSphericalDistanceDeterminationAlgorithm
157 *
158 * @param radius The radius of Earth
159 * @param cam The camera used to capture the picture of Earth
160 * @param minimumIterations The minimum number of iterations to perform
161 * @param maximumRefreshes The maximum number of times to refresh the reference position
162 * @param distanceRatio The maximum distance error between the evaluated and reference
163 * positions to be considered "the same" distance
164 * @param discriminatorRatio The maximum ratio between the evaluated and reference loss
165 * to accept for data
166 * @param pdfOrder The Shuffle Randomization Distribution Order
167 * @param radiusLossOrder The Loss Radius Error Order
168 *
169 * @note Setting distanceRatio to DECIMAL_INF will exclude distance loss from loss
170 * calculations
171 *
172 * @note Setting discriminatorRatio to DECIMAL_INF will include all generated points
173 * in the final point
174 *
175 * @post If pdfOrder or radiusLossOrder less than 2, they will be made 2. Then if
176 * they are odd, they will be incremented
177 */
178 IterativeSphericalDistanceDeterminationAlgorithm(decimal radius,
179 Camera &&cam,
180 size_t minimumIterations,
181 size_t maximumRefreshes,
182 decimal distanceRatio,
183 decimal discriminatorRatio,
184 int pdfOrder,
185 int radiusLossOrder);
186 40 ~IterativeSphericalDistanceDeterminationAlgorithm() = default;
187
188 /**
189 * Obtains the position of the planet relative to the camera
190 *
191 * @param p The points on the edge of Earth (in the image taken
192 * by the camera given to this)
193 *
194 * @return PositionVector The position vector of the Earth relative
195 * to the camera
196 *
197 * @pre p must refer to points taken by the camera that was passed to
198 * this
199 * @pre p is radially sorted, i.e.
200 * if we define the centroid of the points as P, for any
201 * three consecutive points A B and C, angle APB is less than
202 * angle APC
203 *
204 * @post If p.size() < 3, then the result is exactly the zero vector
205 *
206 * @note If minimumIterations (from constructor) is less than the size of
207 * p, then it will increase the number of iterations to cover all of p
208 * */
209 PositionVector Run(const Points &p) override;
210
211 private:
212 /**
213 * Generates a loss on a position vector
214 *
215 * @param position The vector to evaluate
216 * @param targetDistanceSq The target distance squared of the position
217 * @param projectedPoints The projected points to evaluate against
218 * @param size The size of the projected points
219 *
220 * @return The loss of position
221 *
222 * @pre targetRadiusSq is not the true radius, but rather the
223 * distance obtained between the radius vector and a circle
224 * point when projected onto the unit sphere (normalized).
225 * It still functions the same way.
226 */
227 decimal GenerateLoss(PositionVector &position,
228 decimal targetDistanceSq,
229 std::unique_ptr<Vec3[]> &projectedPoints,
230 size_t size);
231
232 /**
233 * Calls SphericalDistanceDeterminationAlgorithm::Run with
234 * randomized triplets of indicies from source
235 *
236 * @param source The source of Vec3 objects
237 * @param n The number of elements in source
238 * @param logits The logits array to use for randomization
239 *
240 * @return The resulting PositionVector from the 3 randomized
241 * points
242 *
243 * @pre Any precondition from this->Run
244 * @pre source.size() >= 3
245 * @pre source[i] is normalized for all i in [0, source.size())
246 *
247 * @note This algorithm uses a even polynomial distribution to
248 * prioritize points far away from a given index. We like that
249 * because it helps deal with noise. To exaggerate the difference
250 * in probability between points, you can use a function that grows
251 * much faster by changing the macro PDF which is defined within.
252 * Make sure the distribution function though is zero where you
253 * need it to be (i.e. At points already generated within a triplet
254 * so that you do not draw those points again)
255 *
256 * @note The assumption of p from this->Run(p) being in polar
257 * order is quite important in this algorithm. Should that not
258 * be true, instead of using index differences in our polynomial
259 * distribution, we'd need to instead use the distance between
260 * the pixels corresponding to those indicies. This is not a
261 * terrible change in terms of code, but is more compuationally
262 * complex
263 */
264 PositionVector ShuffledCall(std::unique_ptr<Vec3[]> &source, size_t n, std::unique_ptr<uint64_t[]> &logits);
265
266 /**
267 * Performs exponentiation for uint64_t
268 *
269 * @param base The base
270 * @param power The power
271 *
272 * @return base ^ power
273 */
274 1327056 inline uint64_t Pow(uint64_t base, uint64_t power) {
275 1327056 uint64_t result = 1;
276
2/2
✓ Branch 0 taken 2654112 times.
✓ Branch 1 taken 1327056 times.
 3981168 while (power > 0) {
277
2/2
✓ Branch 0 taken 1327056 times.
✓ Branch 1 taken 1327056 times.
 2654112 if (power % 2 == 1)
278 1327056 result *= base;
279 2654112 base *= base;
280 2654112 power /= 2;
281 }
282 1327056 return result;
283 }
284
285 /// The minimum number of iterations to use
286 size_t minimumIterations_;
287 /// The maximum number of times to refresh the reference position
288 size_t maximumRefreshes_;
289 /// The maximum distance ratio to accept
290 decimal distanceRatioSq_;
291 /// The maximum loss ratio to accept
292 decimal discriminatorRatio_;
293 /// The Shuffle randomization order
294 uint64_t pdfOrder_;
295 /// The Loss Radius error order
296 uint64_t radiusLossOrder_;
297 };
298
299 /**
300 * The DistanceDeterminationAlgorithm class houses the Distance Determination Algorithm. This
301 * algorithm calculates the distance from Earth based on the pixels of Earth's Edge found in the image.
302 *
303 * @note This class assumes that Earth is a perfect ellipse
304 */
305 class EllipticDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
306 public:
307 /**
308 * Initializes an EllipticDistanceDeterminationAlgorithm
309 *
310 * @param radius The distance from Earth to use
311 */
312 explicit EllipticDistanceDeterminationAlgorithm(PositionVector radius);
313 ~EllipticDistanceDeterminationAlgorithm();
314
315 /**
316 * Place documentation here. Press enter to automatically make a new line
317 *
318 * @param p The points in the image on Earth's horizon
319 *
320 * @return The position vector of the satellite with respect
321 * to the camera's coordinate system
322 * */
323 PositionVector Run(const Points &p) override;
324 private:
325 // Fields specific to this algorithm, and helper methods
326 };
327
328 } // namespace found
329
330 #endif // SRC_DISTANCE_DISTANCE_HPP_
331