7#include "starcorrespondence.h"
10#include "ekos_guide_debug.h"
16int StarCorrespondence::findClosestStar(
double x,
double y,
const QList<Edge> sortedStars,
17 double maxDistance,
double *distance)
const
19 if (x < -maxDistance || y < -maxDistance ||
20 x > imageWidth + maxDistance || y > imageWidth + maxDistance)
23 const int size = sortedStars.
size();
28 const int coarseIncrement = size / 10;
29 if (coarseIncrement > 1)
31 for (
int i = 0; i < size; i += coarseIncrement)
33 if (sortedStars[i].x < x - maxDistance)
43 double bestSquaredDistance = maxDistance * maxDistance;
44 for (
int i = startPoint; i < size; ++i)
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)
55 bestSquaredDistance = squaredDistance;
58 if (distance !=
nullptr) *
distance = sqrt(bestSquaredDistance);
71 sortedToOriginal->
clear();
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)
79 return a.second < b.second;
81 for (
int i = 0; i < stars.
size(); ++i)
83 sortedStars->
append(stars[ rx[i].first ]);
90StarCorrespondence::StarCorrespondence(
const QList<Edge> &stars,
int guideStar)
92 initialize(stars, guideStar);
95StarCorrespondence::StarCorrespondence()
100void StarCorrespondence::initialize(
const QList<Edge> &stars,
int guideStar)
102 qCDebug(KSTARS_EKOS_GUIDE) <<
"StarCorrespondence initialized with " << stars.
size() << guideStar;
104 guideStarIndex = guideStar;
105 float guideX = references[guideStarIndex].x;
106 float guideY = references[guideStarIndex].y;
107 guideStarOffsets.clear();
108 referenceSums.clear();
109 referenceNumPixels.clear();
113 const int numRefs = references.
size();
114 for (
int i = 0; i < numRefs; ++i)
116 const auto &ref = references[i];
117 guideStarOffsets.push_back(Offsets(ref.x - guideX, ref.y - guideY));
118 referenceSums.push_back(ref.sum);
119 referenceNumPixels.push_back(ref.numPixels);
122 initializeAdaptation();
127void StarCorrespondence::reset()
130 guideStarOffsets.clear();
131 referenceSums.clear();
132 referenceNumPixels.clear();
138 int *numFound,
int *numNotFound,
double minFraction)
const
141 constexpr double missingRefStarCost = 100;
143 constexpr double distanceWeight = 1.0;
154 int bestCost = offsets.
size() * missingRefStarCost * (1 - minFraction);
157 if (stars.
size() < minFraction * offsets.
size())
162 const int numStars = stars.
size();
163 int bestStarIndex = -1, bestNumFound = 0, bestNumNotFound = 0;
164 for (
int starIndex = 0; starIndex < numStars; ++starIndex)
166 const float starX = stars[starIndex].x;
167 const float starY = stars[starIndex].y;
171 int numFound = 0, numNotFound = 0;
172 for (
int offsetIndex = 0; offsetIndex < offsets.
size(); ++offsetIndex)
175 if (offsetIndex == guideStarIndex)
continue;
178 if (cost > bestCost)
break;
182 const auto &offset = offsets[offsetIndex];
184 const int closestIndex = findClosestStar(starX + offset.
x, starY + offset.
y,
185 stars, maxDistance, &distance);
186 if (closestIndex < 0)
189 cost += missingRefStarCost;
197 mapping[closestIndex] = offsetIndex;
203 bestStarIndex = starIndex;
204 bestNumFound = numFound;
205 bestNumNotFound = numNotFound;
208 for (
int i = 0; i < mapping.size(); ++i)
209 (*starMap)[i] = mapping[i];
210 (*starMap)[starIndex] = guideStarIndex;
213 *numFound = bestNumFound;
214 *numNotFound = bestNumNotFound;
215 return bestStarIndex;
220 int targetStar)
const
223 int xOffset = offsets[targetStar].x;
224 int yOffset = offsets[targetStar].y;
225 for (
int i = 0; i < offsets.
size(); ++i)
229 (*targetOffsets)[i] = Offsets(0, 0);
233 const double x = offsets[i].x - xOffset;
234 const double y = offsets[i].y - yOffset;
235 (*targetOffsets)[i] = Offsets(x, y);
246 inventedStar.invalidate();
249 float refSum = 0, origSum = 0, refNumPixels = 0, origNumPixels = 0;
250 for (
int i = 0; i < starMap.
size(); ++i)
252 int reference = starMap[i];
255 xPositions.
push_back(stars[i].x - offsets[reference].x);
256 yPositions.
push_back(stars[i].y - offsets[reference].y);
259 refSum += stars[i].sum;
260 refNumPixels += stars[i].numPixels;
261 origSum += referenceSums[reference];
262 origNumPixels += referenceNumPixels[reference];
265 float inventedSum = 0;
266 float inventedNumPixels = 0;
267 if (origSum > 0 && origNumPixels > 0)
270 inventedSum = (refSum / origSum) * referenceSums[guideStarIndex];
271 inventedNumPixels = (refNumPixels / origNumPixels) * referenceNumPixels[guideStarIndex];
274 if (xPositions.
size() == 0)
279 std::sort(xPositions.
begin(), xPositions.
end(), [] (
double d1,
double d2)
283 std::sort(yPositions.
begin(), yPositions.
end(), [] (
double d1,
double d2)
287 const int num = xPositions.
size();
288 double xVal = 0, yVal = 0;
291 const int middle = num / 2;
292 xVal = (xPositions[middle - 1] + xPositions[middle]) / 2.0;
293 yVal = (yPositions[middle - 1] + yPositions[middle]) / 2.0;
297 const int middle = num / 2;
298 xVal = xPositions[middle];
299 yVal = yPositions[middle];
301 inventedStar.x = xVal - offset.x;
302 inventedStar.y = yVal - offset.y;
303 inventedStar.sum = inventedSum;
304 inventedStar.numPixels = inventedNumPixels;
314 for (
int i = 0; i < sortedStarMap.
size(); ++i)
315 (*starMap)[sortedToOriginal[i]] = sortedStarMap[i];
319Edge StarCorrespondence::find(
const QList<Edge> &stars,
double maxDistance,
322 m_NumReferencesFound = 0;
325 foundStar.invalidate();
326 if (!initialized)
return foundStar;
327 int numFound, numNotFound;
333 sortByX(stars, &sortedStars, &sortedToOriginal);
336 int bestStarIndex = findInternal(sortedStars, maxDistance, &sortedStarMap, guideStarIndex,
337 guideStarOffsets, &numFound, &numNotFound, minFraction);
339 if (bestStarIndex > -1)
342 bestStarIndex = sortedToOriginal[bestStarIndex];
343 unmapStarMap(sortedStarMap, sortedToOriginal, starMap);
345 foundStar = stars[bestStarIndex];
346 qCDebug(KSTARS_EKOS_GUIDE)
347 <<
"StarCorrespondence found guideStar at " << bestStarIndex <<
"found/not"
348 << numFound << numNotFound;
349 m_NumReferencesFound = numFound;
351 else if (allowMissingGuideStar && bestStarIndex == -1 &&
352 stars.
size() >= minFraction * guideStarOffsets.size())
354 qCDebug(KSTARS_EKOS_GUIDE)
355 <<
"Trying in invent. StarCorrespondence did NOT find guideStar. Found/not"
356 << numFound << numNotFound <<
"minFraction" << minFraction <<
"maxDistance" << maxDistance;
359 int bestNumFound = 0;
360 int bestNumNotFound = 0;
362 bestInvented.invalidate();
363 for (
int gStarIndex = 0; gStarIndex < guideStarOffsets.size(); gStarIndex++)
365 if (gStarIndex == guideStarIndex)
368 makeOffsets(guideStarOffsets, &gStarOffsets, gStarIndex);
370 int detectedStarIndex = findInternal(sortedStars, maxDistance, &newStarMap,
371 gStarIndex, gStarOffsets,
372 &numFound, &numNotFound, minFraction);
373 if (detectedStarIndex >= 0 && numFound > bestNumFound)
375 Edge invented = inventStarPosition(sortedStars, newStarMap, gStarOffsets,
376 guideStarOffsets[gStarIndex]);
377 if (invented.x < 0 || invented.y < 0)
380 bestInvented = invented;
381 bestNumFound = numFound;
382 bestNumNotFound = numNotFound;
383 sortedStarMap = newStarMap;
385 if (numNotFound <= 1)
390 if (bestNumFound > 0)
393 unmapStarMap(sortedStarMap, sortedToOriginal, starMap);
394 qCDebug(KSTARS_EKOS_GUIDE)
395 <<
"StarCorrespondence found guideStar (invented) at "
396 << bestInvented.x << bestInvented.y <<
"found/not" << bestNumFound << bestNumNotFound;
397 m_NumReferencesFound = bestNumFound;
400 else qCDebug(KSTARS_EKOS_GUIDE) <<
"StarCorrespondence could not invent guideStar.";
402 else qCDebug(KSTARS_EKOS_GUIDE) <<
"StarCorrespondence could not find guideStar.";
404 if (adapt && (bestStarIndex != -1))
405 adaptOffsets(stars, *starMap);
413void StarCorrespondence::initializeAdaptation()
416 const double timeConstant = 25.0;
417 alpha = 1.0 / pow(timeConstant, 0.865);
420 originalGuideStarOffsets = guideStarOffsets;
425 const int numStars = stars.
size();
426 if (starMap.
size() != numStars)
428 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: StarMap size != stars.size()" << starMap.
size() << numStars;
433 for (
int i = 0; i < numStars; ++i)
435 if (starMap[i] == guideStarIndex)
443 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: no guide star in map";
447 for (
int i = 0; i < numStars; ++i)
451 const int refIndex = starMap[i];
452 if (refIndex == -1 || refIndex == guideStarIndex)
455 if ((refIndex >= references.
size()) || (refIndex < 0))
457 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: bad refIndex[" << i <<
"] =" << refIndex;
462 const double xOffset = stars[i].x - stars[guideStar].x;
463 const double yOffset = stars[i].y - stars[guideStar].y;
464 const double currentXOffset = guideStarOffsets[refIndex].x;
465 const double currentYOffset = guideStarOffsets[refIndex].y;
466 const double newXOffset = alpha * xOffset + (1 - alpha) * currentXOffset;
467 const double newYOffset = alpha * yOffset + (1 - alpha) * currentYOffset;
471 constexpr double maxMovement = 2.5;
472 if ((fabs(newXOffset - originalGuideStarOffsets[refIndex].x) < maxMovement) &&
473 (fabs(newYOffset - originalGuideStarOffsets[refIndex].y) < maxMovement))
475 guideStarOffsets[refIndex].x = newXOffset;
476 guideStarOffsets[refIndex].y = newYOffset;
KOSM_EXPORT double distance(const std::vector< const OSM::Node * > &path, Coordinate coord)
void append(QList< T > &&value)
void push_back(parameter_type value)
void resize(qsizetype size)
qsizetype size() const const