FOUND Coverage Report

src/
File: distance/distance.cpp
Date: 2025-11-20 21:57:26
Lines:
100/100
100.0%
Functions:
7/7
100.0%
Branches:
73/73
100.0%
Line Branch Exec Source
1 #include "distance/distance.hpp"
2
3 #include <assert.h>
4
5 #include <cmath>
6 #include <utility>
7 #include <random>
8 #include <memory>
9
10 #include "common/logging.hpp"
11 #include "common/spatial/attitude-utils.hpp"
12 #include "common/spatial/camera.hpp"
13 #include "common/style.hpp"
14
15 namespace found {
16
17 ///// SphericalDistanceDeterminationAlgorithm /////
18
19 40 PositionVector SphericalDistanceDeterminationAlgorithm::Run(const Points &p) {
20
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 38 times.
 40 if (p.size() < 3) return {0, 0, 0};
21
22
2/2
✓ Branch 3 taken 38 times.
✓ Branch 6 taken 38 times.
 114 const Vec3 spats[3] = {cam_.CameraToSpatial(p[0]).Normalize(),
23
2/2
✓ Branch 3 taken 38 times.
✓ Branch 6 taken 38 times.
 76 cam_.CameraToSpatial(p[p.size() / 2]).Normalize(),
24
2/2
✓ Branch 6 taken 38 times.
✓ Branch 9 taken 38 times.
 114 cam_.CameraToSpatial(p[p.size() - 1]).Normalize()};
25
26
1/1
✓ Branch 1 taken 38 times.
 38 return this->Run(spats[0], spats[1], spats[2]);
27 }
28
29 11237 PositionVector SphericalDistanceDeterminationAlgorithm::Run(const Vec3 &a, const Vec3 &b, const Vec3 &c) {
30 // Obtain the center point of the projected circle
31
1/1
✓ Branch 1 taken 11237 times.
 11237 this->center_ = getCenter(a, b, c);
32
33 // Obtain the radius of the projected circle
34
1/1
✓ Branch 1 taken 11237 times.
 11237 this->r_ = Distance(a, this->center_);
35
36 // Obtain the distance from earth
37 11237 decimal h = this->radius_/this->r_;
38
39 // You have to normalize the center vector here
40
2/2
✓ Branch 2 taken 11237 times.
✓ Branch 5 taken 11237 times.
 33711 return this->center_.Normalize() * h;
41 }
42
43 11237 Vec3 SphericalDistanceDeterminationAlgorithm::getCenter(const Vec3 &a, const Vec3 &b, const Vec3 &c) {
44
1/1
✓ Branch 2 taken 11237 times.
 11237 Vec3 diff1 = b - a;
45
1/1
✓ Branch 2 taken 11237 times.
 11237 Vec3 diff2 = c - b;
46
47 // Cross product to find the normal vector for points on earth
48
1/1
✓ Branch 2 taken 11237 times.
 11237 Vec3 circleN = diff1.CrossProduct(diff2);
49 11237 Vec3 circlePt = a;
50
51 // Mid point between 2 vectors
52
1/1
✓ Branch 2 taken 11237 times.
 11237 Vec3 mid1 = Midpoint(a, b);
53
1/1
✓ Branch 2 taken 11237 times.
 11237 Vec3 mid2 = Midpoint(b, c);
54
55 11237 Vec3 mid1N = diff1;
56 11237 Vec3 mid2N = diff2;
57
58 /**
59 * CirclePt is a vector that points to a point on the plane. We also know the center vector should point to a point on the plane.
60 * So, we get (circlePt - center) * circleN = 0. This is equivalent to (center * circleN) = (circlePt * circleN)
61 *
62 * We have (center - mid1/mid2) gives us the vector perpendicular to the mid1N/mid2N vector. Hence,
63 * (center - mid1)*mid1N = 0. This becomes (mid1N * center) = (mid1N * mid1). (This is the same for mid2)
64 * So we have:
65 * circleN * center = circleN * circlePt
66 * mid1N * center = mid1N * mid1
67 * mid2N * center = mid2N * mid2
68 * This becomes a systems of linear equation
69 */
70 11237 Mat3 matrix;
71 11237 matrix = {circleN.x, circleN.y, circleN.z, mid1N.x, mid1N.y,
72 11237 mid1N.z, mid2N.x, mid2N.y, mid2N.z};
73
74
1/1
✓ Branch 1 taken 11237 times.
 11237 decimal alpha = circleN*circlePt;
75
1/1
✓ Branch 1 taken 11237 times.
 11237 decimal beta = mid1N*mid1;
76
1/1
✓ Branch 1 taken 11237 times.
 11237 decimal gamma = mid2N*mid2;
77
78 11237 Vec3 y = {alpha, beta, gamma};
79
80
2/2
✓ Branch 2 taken 11237 times.
✓ Branch 5 taken 11237 times.
 11237 Vec3 center = matrix.Inverse() * y;
81
82 22474 return center;
83 }
84
85 ///// IterativeSphericalDistanceDeterminationAlgorithm /////
86
87 19 IterativeSphericalDistanceDeterminationAlgorithm::IterativeSphericalDistanceDeterminationAlgorithm(decimal radius,
88 Camera &&cam,
89 size_t minimumIterations,
90 size_t maximumRefreshes,
91 decimal distanceRatio,
92 decimal discriminatorRatio,
93 int pdfOrder,
94 19 int radiusLossOrder)
95 : SphericalDistanceDeterminationAlgorithm(radius, std::forward<Camera>(cam)),
96 19 minimumIterations_(minimumIterations),
97 19 maximumRefreshes_(maximumRefreshes),
98 19 distanceRatioSq_(distanceRatio * distanceRatio),
99 19 discriminatorRatio_(discriminatorRatio) {
100
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 18 times.
 19 if (pdfOrder < 2) pdfOrder = 2;
101
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 18 times.
 19 if (radiusLossOrder < 2) radiusLossOrder = 2;
102 19 this->maximumRefreshes_ = maximumRefreshes;
103 19 this->pdfOrder_ = static_cast<uint64_t>(pdfOrder + pdfOrder % 2);
104 19 this->radiusLossOrder_ = static_cast<uint64_t>(radiusLossOrder + radiusLossOrder % 2);
105
106 − assert(this->pdfOrder_ >= 2);
107 − assert(this->pdfOrder_ % 2 == 0);
108 − assert(this->radiusLossOrder_ >= 2);
109 − assert(this->radiusLossOrder_ % 2 == 0);
110 19 }
111
112 19 PositionVector IterativeSphericalDistanceDeterminationAlgorithm::Run(const Points &p) {
113 // Step -1: Return zero if the number of points is less than 0
114
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 18 times.
 19 if (p.size() < 3) return {0, 0, 0};
115
116 // Step 0: Determine the number of iterations
117
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 8 times.
 18 size_t numIterations = this->minimumIterations_ > p.size() / 3 ? this->minimumIterations_ : p.size();
118 18 size_t refreshFrequency = numIterations / (
119 30 (this->maximumRefreshes_ < numIterations / 2 ? this->maximumRefreshes_ : numIterations / 2 - 1)
120
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
 18 + 1);
121
122 // Step 1a: Get all unit vector projections of each point and setup logits
123 18 size_t i = 0;
124 18 size_t j = 0;
125 18 size_t pointsSize = p.size();
126 − std::unique_ptr<Vec3[]> projectedPoints(new Vec3[pointsSize]);
127
2/2
✓ Branch 7 taken 567 times.
✓ Branch 8 taken 18 times.
 585 for (const Vec2 &point : p) {
128
2/2
✓ Branch 2 taken 567 times.
✓ Branch 5 taken 567 times.
 567 projectedPoints[i++] = this->cam_.CameraToSpatial(point).Normalize();
129 }
130 18 i = 0;
131 − std::unique_ptr<uint64_t[]> logits(new uint64_t[pointsSize]);
132
133 // Step 2a: Use the first estimate as a reference
134
1/1
✓ Branch 2 taken 18 times.
 18 PositionVector first(SphericalDistanceDeterminationAlgorithm::Run(p));
135
1/1
✓ Branch 1 taken 18 times.
 18 decimal targetDistSq = first.MagnitudeSq();
136
1/1
✓ Branch 1 taken 18 times.
 18 decimal referenceLoss = this->GenerateLoss(first, targetDistSq, projectedPoints, pointsSize);
137 // Step 2b: Setup the cumulative loss and position. We are using softmax, normalized on the reference
138 18 decimal totalLoss = DECIMAL_EXP(-1.0);
139
1/1
✓ Branch 2 taken 18 times.
 18 PositionVector totalPosition = first * totalLoss;
140
141 // Step 3: Iterate through all triplets and run them through SDDA,
142 // generating a softmax statistic on each
143
2/2
✓ Branch 0 taken 11199 times.
✓ Branch 1 taken 18 times.
 11217 while (i != numIterations) {
144 // Step 3b: Get the position from SDDA
145
1/1
✓ Branch 2 taken 11199 times.
 11199 PositionVector position(this->ShuffledCall(projectedPoints, pointsSize, logits));
146
1/1
✓ Branch 1 taken 11199 times.
 11199 decimal loss = this->GenerateLoss(position, targetDistSq, projectedPoints, pointsSize) / referenceLoss;
147
2/2
✓ Branch 0 taken 11040 times.
✓ Branch 1 taken 159 times.
 11199 if (loss <= this->discriminatorRatio_) {
148 11040 decimal factor = DECIMAL_EXP(-loss);
149 11040 totalLoss += factor;
150
2/2
✓ Branch 2 taken 11040 times.
✓ Branch 5 taken 11040 times.
 11040 totalPosition += position * factor;
151 11040 i++;
152 }
153
154 11199 j += 3;
155
156
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 11179 times.
 11199 if (i % refreshFrequency == 0) {
157
1/1
✓ Branch 1 taken 20 times.
 20 totalPosition = totalPosition / totalLoss;
158
1/1
✓ Branch 1 taken 20 times.
 20 targetDistSq = totalPosition.MagnitudeSq();
159
1/1
✓ Branch 1 taken 20 times.
 20 referenceLoss = this->GenerateLoss(totalPosition, targetDistSq, projectedPoints, pointsSize);
160 20 totalLoss = DECIMAL_EXP(-1.0);
161
1/1
✓ Branch 1 taken 20 times.
 20 totalPosition = totalPosition * totalLoss;
162 }
163 }
164
165 // Step 4: Return the softmax of the composed algorithm via the random triplets
166
1/1
✓ Branch 1 taken 18 times.
 18 return totalPosition / totalLoss;
167 18 }
168
169 11237 decimal IterativeSphericalDistanceDeterminationAlgorithm::GenerateLoss(PositionVector &position,
170 decimal targetDistanceSq,
171 std::unique_ptr<Vec3[]> &projectedPoints,
172 size_t size) {
173 // Generate the loss on point (offset it so it won't be nan, and initialize with distance
174 // error):
175 11237 decimal loss = DECIMAL(1e-3);
176 // If the distance error is outside the ratio, then we add distance loss
177 11237 decimal distance_sq_loss_ratio = DECIMAL_ABS((targetDistanceSq - position.MagnitudeSq())) / targetDistanceSq;
178
2/2
✓ Branch 0 taken 159 times.
✓ Branch 1 taken 11078 times.
 11237 if (distance_sq_loss_ratio >= this->distanceRatioSq_) {
179 159 loss += distance_sq_loss_ratio * targetDistanceSq;
180 }
181 // Now, we obtain the radius error
182 11237 decimal targetRadiusSq = this->r_ * this->r_;
183
2/2
✓ Branch 0 taken 665181 times.
✓ Branch 1 taken 11237 times.
 676418 for (size_t k = 0; k < size; k++) {
184
2/2
✓ Branch 3 taken 665181 times.
✓ Branch 6 taken 665181 times.
 665181 decimal radius_loss_k = targetRadiusSq - (this->center_ - projectedPoints[k]).MagnitudeSq();
185 665181 loss += DECIMAL_POW(radius_loss_k, this->radiusLossOrder_);
186 }
187
188 11237 return loss;
189 }
190
191 11199 PositionVector IterativeSphericalDistanceDeterminationAlgorithm::ShuffledCall(
192 std::unique_ptr<Vec3[]> &source,
193 size_t n,
194 std::unique_ptr<uint64_t[]> &logits) {
195 // Step 0: Setup the random number generators
196 − static std::random_device device; // GCOVR_EXCL_LINE
197 − static std::mt19937 gen(device()); // GCOVR_EXCL_LINE
198 // This is okay (being static) since we always override the values
199
200 // Uniformly generate the first number
201
1/1
✓ Branch 2 taken 11199 times.
 11199 std::uniform_int_distribution<size_t> dist(0, n - 1);
202
1/1
✓ Branch 1 taken 11199 times.
 11199 size_t index1 = dist(gen);
203 11199 Vec3 &a = source[index1];
204 − assert(dist.min() == 0);
205 − assert(dist.max() == static_cast<size_t>(n - 1));
206
207 // Create logits for the second (even polynomial centered at indicies[i - 1])
208
2/2
✓ Branch 0 taken 663528 times.
✓ Branch 1 taken 11199 times.
 674727 for (uint64_t j = 0; j < n; j++) {
209 663528 logits[j] = this->Pow(j - index1, this->pdfOrder_);
210 }
211 // Sample for the next number
212
1/1
✓ Branch 4 taken 11199 times.
 11199 std::discrete_distribution<size_t> dist1(logits.get(), logits.get() + n);
213
1/1
✓ Branch 1 taken 11199 times.
 11199 size_t index2 = dist1(gen);
214 11199 Vec3 &b = source[index2];
215 − assert(dist1.min() == 0);
216 − assert(dist1.max() == n - 1);
217
218 // Update the logits for the third number (bi-even polynomial with
219 // centers at indicies[i - 1] and indicies[i - 2]). Note that
220 // the function is zero at both our chosen indicies
221
2/2
✓ Branch 0 taken 663528 times.
✓ Branch 1 taken 11199 times.
 674727 for (uint64_t j = 0; j < n; j++) {
222 663528 logits[j] *= this->Pow(j - index2, this->pdfOrder_);
223 }
224 // Sample for the last number
225
1/1
✓ Branch 4 taken 11199 times.
 11199 std::discrete_distribution<size_t> dist2(logits.get(), logits.get() + n);
226
1/1
✓ Branch 1 taken 11199 times.
 11199 Vec3 &c = source[dist2(gen)];
227 − assert(dist2.min() == 0);
228 − assert(dist2.max() == n - 1);
229
230
1/1
✓ Branch 1 taken 11199 times.
 22398 return this->SphericalDistanceDeterminationAlgorithm::Run(a, b, c);
231 11199 }
232
233 } // namespace found
234