Kstars

starcorrespondence.cpp
1 /*
2  SPDX-FileCopyrightText: 2020 Hy Murveit <[email protected]>
3 
4  SPDX-License-Identifier: GPL-2.0-or-later
5 */
6 
7 #include "starcorrespondence.h"
8 
9 #include <math.h>
10 #include "ekos_guide_debug.h"
11 
12 // Finds the star in sortedStars that's closest to x,y and within maxDistance pixels.
13 // Returns the index of the closest star in sortedStars, or -1 if none satisfies the criteria.
14 // sortedStars must be sorted by x, from lower x to higher x.
15 // Fills distance to the pixel distance to the closest star.
16 int StarCorrespondence::findClosestStar(double x, double y, const QList<Edge> sortedStars,
17  double maxDistance, double *distance) const
18 {
19  if (x < -maxDistance || y < -maxDistance ||
20  x > imageWidth + maxDistance || y > imageWidth + maxDistance)
21  return -1;
22 
23  const int size = sortedStars.size();
24  int startPoint = 0;
25 
26  // Efficiency improvement to quickly find the start point.
27  // Increments by size/10 to find a better start point.
28  const int coarseIncrement = size / 10;
29  if (coarseIncrement > 1)
30  {
31  for (int i = 0; i < size; i += coarseIncrement)
32  {
33  if (sortedStars[i].x < x - maxDistance)
34  startPoint = i;
35  else
36  break;
37  }
38  }
39 
40  // Iterate through the stars starting with the star with lowest x where x >= x - maxDistance
41  // through the star with greatest x where x <= x + maxDistance. Find the closest star to x,y.
42  int bestIndex = -1;
43  double bestSquaredDistance = maxDistance * maxDistance;
44  for (int i = startPoint; i < size; ++i)
45  {
46  auto &star = sortedStars[i];
47  if (star.x < x - maxDistance) continue;
48  if (star.x > x + maxDistance) break;
49  const double xDiff = star.x - x;
50  const double yDiff = star.y - y;
51  const double squaredDistance = xDiff * xDiff + yDiff * yDiff;
52  if (squaredDistance <= bestSquaredDistance)
53  {
54  bestIndex = i;
55  bestSquaredDistance = squaredDistance;
56  }
57  }
58  if (distance != nullptr) *distance = sqrt(bestSquaredDistance);
59  return bestIndex;
60 }
61 
62 namespace
63 {
64 // Sorts stars by their x values, places the sorted stars into sortedStars.
65 // sortedToOriginal is a map whose index is the index of a star in sortedStars and whose
66 // value is the index of a star in stars.
67 // Therefore, sortedToOrigial[a] = b, means that sortedStars[a] corresponds to stars[b].
68 void sortByX(const QList<Edge> &stars, QList<Edge> *sortedStars, QVector<int> *sortedToOriginal)
69 {
70  sortedStars->clear();
71  sortedToOriginal->clear();
72 
73  // Sort the stars by their x-positions, lower to higher.
75  for (int i = 0; i < stars.size(); ++i)
76  rx.push_back(std::pair<int, float>(i, stars[i].x));
77  std::sort(rx.begin(), rx.end(), [](const std::pair<int, float> &a, const std::pair<int, double> &b)
78  {
79  return a.second < b.second;
80  });
81  for (int i = 0; i < stars.size(); ++i)
82  {
83  sortedStars->append(stars[ rx[i].first ]);
84  sortedToOriginal->push_back(rx[i].first);
85  }
86 }
87 
88 } // namespace
89 
90 StarCorrespondence::StarCorrespondence(const QList<Edge> &stars, int guideStar)
91 {
92  initialize(stars, guideStar);
93 }
94 
95 StarCorrespondence::StarCorrespondence()
96 {
97 }
98 
99 // Initialize with the reference stars and the guide-star index.
100 void StarCorrespondence::initialize(const QList<Edge> &stars, int guideStar)
101 {
102  qCDebug(KSTARS_EKOS_GUIDE) << "StarCorrespondence initialized with " << stars.size() << guideStar;
103  references = stars;
104  guideStarIndex = guideStar;
105  float guideX = references[guideStarIndex].x;
106  float guideY = references[guideStarIndex].y;
107  guideStarOffsets.clear();
108 
109  // Compute the x and y offsets from the guide star to all reference stars
110  // and store them in guideStarOffsets.
111  const int numRefs = references.size();
112  for (int i = 0; i < numRefs; ++i)
113  {
114  const auto &ref = references[i];
115  guideStarOffsets.push_back(Offsets(ref.x - guideX, ref.y - guideY));
116  }
117 
118  initializeAdaptation();
119 
120  initialized = true;
121 }
122 
123 void StarCorrespondence::reset()
124 {
125  references.clear();
126  guideStarOffsets.clear();
127  initialized = false;
128 }
129 
130 int StarCorrespondence::findInternal(const QList<Edge> &stars, double maxDistance, QVector<int> *starMap,
131  int guideStarIndex, const QVector<Offsets> &offsets,
132  int *numFound, int *numNotFound, double minFraction) const
133 {
134  // This is the cost of not finding one of the reference stars.
135  constexpr double missingRefStarCost = 100;
136  // This weight multiplies the distance between a reference star position and the closest star in stars.
137  constexpr double distanceWeight = 1.0;
138 
139  // Initialize all stars to not-corresponding to any reference star.
140  *starMap = QVector<int>(stars.size(), -1);
141 
142  // We won't accept a solution worse than bestCost.
143  // In the default case, we need to find about half the reference stars.
144  // Another possibility is a constraint on the input stars being mapped
145  // E.g. that the more likely solution is one where the stars are close to the references.
146  // This can be an issue if the number of input stars is way less than the number of reference stars
147  // but in that case we can fail and go to the default star-finding algorithm.
148  int bestCost = offsets.size() * missingRefStarCost * (1 - minFraction);
149  // Note that the above implies that if stars.size() < offsets.size() * minFraction
150  // then it is impossible to succeed.
151  if (stars.size() < minFraction * offsets.size())
152  return -1;
153 
154  // Assume the guide star corresponds to each of the stars.
155  // Score the assignment, pick the best, and then assign the rest.
156  const int numStars = stars.size();
157  int bestStarIndex = -1, bestNumFound = 0, bestNumNotFound = 0;
158  for (int starIndex = 0; starIndex < numStars; ++starIndex)
159  {
160  const float starX = stars[starIndex].x;
161  const float starY = stars[starIndex].y;
162 
163  double cost = 0.0;
164  QVector<int> mapping(stars.size(), -1);
165  int numFound = 0, numNotFound = 0;
166  for (int offsetIndex = 0; offsetIndex < offsets.size(); ++offsetIndex)
167  {
168  // Of course, the guide star has offsets 0 to itself.
169  if (offsetIndex == guideStarIndex) continue;
170 
171  // We're already worse than the best cost. No need to search any more.
172  if (cost > bestCost) break;
173 
174  // Look for an input star at the offset position.
175  // Note the index value returned is the sorted index, which is converted back later.
176  const auto &offset = offsets[offsetIndex];
177  double distance;
178  const int closestIndex = findClosestStar(starX + offset.x, starY + offset.y,
179  stars, maxDistance, &distance);
180  if (closestIndex < 0)
181  {
182  // This reference star position had no corresponding input star.
183  cost += missingRefStarCost;
184  numNotFound++;
185  continue;
186  }
187  numFound++;
188 
189  // If starIndex is the star that corresponds to guideStarIndex, then
190  // stars[index] corresponds to references[offsetIndex]
191  mapping[closestIndex] = offsetIndex;
192  cost += distance * distanceWeight;
193  }
194  if (cost < bestCost)
195  {
196  bestCost = cost;
197  bestStarIndex = starIndex;
198  bestNumFound = numFound;
199  bestNumNotFound = numNotFound;
200 
201  *starMap = QVector<int>(stars.size(), -1);
202  for (int i = 0; i < mapping.size(); ++i)
203  (*starMap)[i] = mapping[i];
204  (*starMap)[starIndex] = guideStarIndex;
205  }
206  }
207  *numFound = bestNumFound;
208  *numNotFound = bestNumNotFound;
209  return bestStarIndex;
210 }
211 
212 // Converts offsets that are from the point-of-view of the guidestar, to offsets from targetStar.
213 void StarCorrespondence::makeOffsets(const QVector<Offsets> &offsets, QVector<Offsets> *targetOffsets,
214  int targetStar) const
215 {
216  targetOffsets->resize(offsets.size());
217  int xOffset = offsets[targetStar].x;
218  int yOffset = offsets[targetStar].y;
219  for (int i = 0; i < offsets.size(); ++i)
220  {
221  if (i == targetStar)
222  {
223  (*targetOffsets)[i] = Offsets(0, 0);
224  }
225  else
226  {
227  const double x = offsets[i].x - xOffset;
228  const double y = offsets[i].y - yOffset;
229  (*targetOffsets)[i] = Offsets(x, y);
230  }
231  }
232 
233 }
234 
235 // We create an imaginary star from the ones we did find.
236 GuiderUtils::Vector StarCorrespondence::inventStarPosition(const QList<Edge> &stars, QVector<int> &starMap,
237  QVector<Offsets> offsets, Offsets offset) const
238 {
239  QVector<double> xPositions, yPositions;
240  for (int i = 0; i < starMap.size(); ++i)
241  {
242  int reference = starMap[i];
243  if (reference >= 0)
244  {
245  xPositions.push_back(stars[i].x - offsets[reference].x);
246  yPositions.push_back(stars[i].y - offsets[reference].y);
247  }
248  }
249  if (xPositions.size() == 0)
250  return GuiderUtils::Vector(-1, -1, -1);
251 
252  // Compute the median x and y values. After gathering the values above,
253  // we sort them and use the middle positions.
254  std::sort(xPositions.begin(), xPositions.end(), [] (double d1, double d2)
255  {
256  return d1 < d2;
257  });
258  std::sort(yPositions.begin(), yPositions.end(), [] (double d1, double d2)
259  {
260  return d1 < d2;
261  });
262  const int num = xPositions.size();
263  double xVal = 0, yVal = 0;
264  if (num % 2 == 0)
265  {
266  const int middle = num / 2;
267  xVal = (xPositions[middle - 1] + xPositions[middle]) / 2.0;
268  yVal = (yPositions[middle - 1] + yPositions[middle]) / 2.0;
269  }
270  else
271  {
272  const int middle = num / 2; // because it's 0-based
273  xVal = xPositions[middle];
274  yVal = yPositions[middle];
275  }
276  return GuiderUtils::Vector(xVal - offset.x, yVal - offset.y, -1);
277 }
278 
279 namespace
280 {
281 // This utility converts the starMap to refer to indexes in the original star QVector
282 // instead of the sorted-by-x version.
283 void unmapStarMap(const QVector<int> &sortedStarMap, const QVector<int> &sortedToOriginal, QVector<int> *starMap)
284 {
285  for (int i = 0; i < sortedStarMap.size(); ++i)
286  (*starMap)[sortedToOriginal[i]] = sortedStarMap[i];
287 }
288 }
289 
290 GuiderUtils::Vector StarCorrespondence::find(const QList<Edge> &stars, double maxDistance,
291  QVector<int> *starMap, bool adapt, double minFraction)
292 {
293  *starMap = QVector<int>(stars.size(), -1);
294  if (!initialized) return GuiderUtils::Vector(-1, -1, -1);
295  int numFound, numNotFound;
296 
297  // findClosestStar needs an input with stars sorted by their x.
298  // Do this outside of the loops.
299  QList<Edge> sortedStars;
300  QVector<int> sortedToOriginal;
301  sortByX(stars, &sortedStars, &sortedToOriginal);
302 
303  QVector<int> sortedStarMap;
304  int bestStarIndex = findInternal(sortedStars, maxDistance, &sortedStarMap, guideStarIndex,
305  guideStarOffsets, &numFound, &numNotFound, minFraction);
306 
307  GuiderUtils::Vector starPosition(-1, -1, -1);
308  if (bestStarIndex > -1)
309  {
310  // Convert back to the unsorted index value.
311  bestStarIndex = sortedToOriginal[bestStarIndex];
312  unmapStarMap(sortedStarMap, sortedToOriginal, starMap);
313 
314  starPosition = GuiderUtils::Vector(stars[bestStarIndex].x, stars[bestStarIndex].y, -1);
315  qCDebug(KSTARS_EKOS_GUIDE)
316  << " StarCorrespondence found guideStar at " << bestStarIndex << "found/not"
317  << numFound << numNotFound;
318  }
319  else if (allowMissingGuideStar && bestStarIndex == -1 &&
320  stars.size() >= minFraction * guideStarOffsets.size())
321  {
322  // If we didn't find a good solution, perhaps the guide star was not detected.
323  // See if we can get a reasonable solution from the other stars.
324  int bestNumFound = 0;
325  int bestNumNotFound = 0;
326  GuiderUtils::Vector bestPosition(-1, -1, -1);
327  for (int gStarIndex = 0; gStarIndex < guideStarOffsets.size(); gStarIndex++)
328  {
329  if (gStarIndex == guideStarIndex)
330  continue;
331  QVector<Offsets> gStarOffsets;
332  makeOffsets(guideStarOffsets, &gStarOffsets, gStarIndex);
333  QVector<int> newStarMap;
334  int detectedStarIndex = findInternal(sortedStars, maxDistance, &newStarMap,
335  gStarIndex, gStarOffsets,
336  &numFound, &numNotFound, minFraction);
337  if (detectedStarIndex >= 0 && numFound > bestNumFound)
338  {
339  GuiderUtils::Vector position = inventStarPosition(sortedStars, newStarMap, gStarOffsets,
340  guideStarOffsets[gStarIndex]);
341  if (position.x < 0 || position.y < 0)
342  continue;
343 
344  bestPosition = position;
345  bestNumFound = numFound;
346  bestNumNotFound = numNotFound;
347  sortedStarMap = newStarMap;
348 
349  if (numNotFound <= 1)
350  // We can't do better than this.
351  break;
352  }
353  }
354  if (bestNumFound > 0)
355  {
356  // Convert back to the unsorted index value.
357  unmapStarMap(sortedStarMap, sortedToOriginal, starMap);
358  qCDebug(KSTARS_EKOS_GUIDE)
359  << "StarCorrespondence found guideStar (invented) at "
360  << bestPosition.x << bestPosition.y << "found/not" << bestNumFound << bestNumNotFound;
361  return bestPosition;
362  }
363  else qCDebug(KSTARS_EKOS_GUIDE) << "StarCorrespondence could not invent guideStar.";
364  }
365  else qCDebug(KSTARS_EKOS_GUIDE) << "StarCorrespondence could not find guideStar.";
366 
367  if (adapt && (bestStarIndex != -1))
368  adaptOffsets(stars, *starMap);
369  return starPosition;
370 }
371 
372 // Compute the alpha cooeficient for a simple IIR filter used in adaptOffsets()
373 // that causes the offsets to be, roughtly, the moving average of the last timeConstant samples.
374 // See the discussion here:
375 // https://dsp.stackexchange.com/questions/378/what-is-the-best-first-order-iir-ar-filter-approximation-to-a-moving-average-f
376 void StarCorrespondence::initializeAdaptation()
377 {
378  // Approximately average the last 25 samples.
379  const double timeConstant = 25.0;
380  alpha = 1.0 / pow(timeConstant, 0.865);
381 
382  // Don't let the adapted offsets move far from the original ones.
383  originalGuideStarOffsets = guideStarOffsets;
384 }
385 
386 void StarCorrespondence::adaptOffsets(const QList<Edge> &stars, const QVector<int> &starMap)
387 {
388  const int numStars = stars.size();
389  if (starMap.size() != numStars)
390  {
391  qCDebug(KSTARS_EKOS_GUIDE) << "Adapt error: StarMap size != stars.size()" << starMap.size() << numStars;
392  return;
393  }
394  // guideStar will be the index in stars corresponding to the reference guide star.
395  int guideStar = -1;
396  for (int i = 0; i < numStars; ++i)
397  {
398  if (starMap[i] == guideStarIndex)
399  {
400  guideStar = i;
401  break;
402  }
403  }
404  if (guideStar < 0)
405  {
406  qCDebug(KSTARS_EKOS_GUIDE) << "Adapt error: no guide star in map";
407  return;
408  }
409 
410  for (int i = 0; i < numStars; ++i)
411  {
412  // We don't adapt if the ith star doesn't correspond to any of the references,
413  // or if it corresponds to the guide star (whose offsets are, of course, always 0).
414  const int refIndex = starMap[i];
415  if (refIndex == -1 || refIndex == guideStarIndex)
416  continue;
417 
418  if ((refIndex >= references.size()) || (refIndex < 0))
419  {
420  qCDebug(KSTARS_EKOS_GUIDE) << "Adapt error: bad refIndex[" << i << "] =" << refIndex;
421  return;
422  }
423  // Adapt the x and y offsets using the following IIR filter:
424  // output[t] = alpha * offset[t] + (1-alpha) * output[t-1]
425  const double xOffset = stars[i].x - stars[guideStar].x;
426  const double yOffset = stars[i].y - stars[guideStar].y;
427  const double currentXOffset = guideStarOffsets[refIndex].x;
428  const double currentYOffset = guideStarOffsets[refIndex].y;
429  const double newXOffset = alpha * xOffset + (1 - alpha) * currentXOffset;
430  const double newYOffset = alpha * yOffset + (1 - alpha) * currentYOffset;
431 
432  // The adaptation is meant for small movements of at most a few pixels.
433  // We don't want it to move enough to find a new star.
434  constexpr double maxMovement = 2.5; // pixels
435  if ((fabs(newXOffset - originalGuideStarOffsets[refIndex].x) < maxMovement) &&
436  (fabs(newYOffset - originalGuideStarOffsets[refIndex].y) < maxMovement))
437  {
438  guideStarOffsets[refIndex].x = newXOffset;
439  guideStarOffsets[refIndex].y = newYOffset;
440  }
441  }
442 }
void append(const T &value)
QVector::iterator begin()
void ref()
void push_back(const T &value)
void initialize(StandardShortcut id)
int size() const const
void clear()
KOSM_EXPORT double distance(const std::vector< const OSM::Node * > &path, Coordinate coord)
void resize(int size)
QVector::iterator end()
void clear()
int size() const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2022 The KDE developers.
Generated on Thu Aug 11 2022 04:00:06 by doxygen 1.8.17 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.