7 #include "starcorrespondence.h"
10 #include "ekos_guide_debug.h"
16 int 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 ]);
90 StarCorrespondence::StarCorrespondence(
const QList<Edge> &stars,
int guideStar)
95 StarCorrespondence::StarCorrespondence()
100 void 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();
111 const int numRefs = references.size();
112 for (
int i = 0; i < numRefs; ++i)
114 const auto &
ref = references[i];
115 guideStarOffsets.push_back(Offsets(
ref.x - guideX,
ref.y - guideY));
118 initializeAdaptation();
123 void StarCorrespondence::reset()
126 guideStarOffsets.clear();
132 int *numFound,
int *numNotFound,
double minFraction)
const
135 constexpr
double missingRefStarCost = 100;
137 constexpr
double distanceWeight = 1.0;
148 int bestCost = offsets.
size() * missingRefStarCost * (1 - minFraction);
151 if (stars.
size() < minFraction * offsets.
size())
156 const int numStars = stars.
size();
157 int bestStarIndex = -1, bestNumFound = 0, bestNumNotFound = 0;
158 for (
int starIndex = 0; starIndex < numStars; ++starIndex)
160 const float starX = stars[starIndex].x;
161 const float starY = stars[starIndex].y;
165 int numFound = 0, numNotFound = 0;
166 for (
int offsetIndex = 0; offsetIndex < offsets.
size(); ++offsetIndex)
169 if (offsetIndex == guideStarIndex)
continue;
172 if (cost > bestCost)
break;
176 const auto &offset = offsets[offsetIndex];
178 const int closestIndex = findClosestStar(starX + offset.x, starY + offset.y,
179 stars, maxDistance, &distance);
180 if (closestIndex < 0)
183 cost += missingRefStarCost;
191 mapping[closestIndex] = offsetIndex;
197 bestStarIndex = starIndex;
198 bestNumFound = numFound;
199 bestNumNotFound = numNotFound;
202 for (
int i = 0; i < mapping.size(); ++i)
203 (*starMap)[i] = mapping[i];
204 (*starMap)[starIndex] = guideStarIndex;
207 *numFound = bestNumFound;
208 *numNotFound = bestNumNotFound;
209 return bestStarIndex;
214 int targetStar)
const
217 int xOffset = offsets[targetStar].x;
218 int yOffset = offsets[targetStar].y;
219 for (
int i = 0; i < offsets.
size(); ++i)
223 (*targetOffsets)[i] = Offsets(0, 0);
227 const double x = offsets[i].x - xOffset;
228 const double y = offsets[i].y - yOffset;
229 (*targetOffsets)[i] = Offsets(x, y);
240 for (
int i = 0; i < starMap.
size(); ++i)
242 int reference = starMap[i];
245 xPositions.
push_back(stars[i].x - offsets[reference].x);
246 yPositions.
push_back(stars[i].y - offsets[reference].y);
249 if (xPositions.
size() == 0)
250 return GuiderUtils::Vector(-1, -1, -1);
254 std::sort(xPositions.
begin(), xPositions.
end(), [] (
double d1,
double d2)
258 std::sort(yPositions.
begin(), yPositions.
end(), [] (
double d1,
double d2)
262 const int num = xPositions.
size();
263 double xVal = 0, yVal = 0;
266 const int middle = num / 2;
267 xVal = (xPositions[middle - 1] + xPositions[middle]) / 2.0;
268 yVal = (yPositions[middle - 1] + yPositions[middle]) / 2.0;
272 const int middle = num / 2;
273 xVal = xPositions[middle];
274 yVal = yPositions[middle];
276 return GuiderUtils::Vector(xVal - offset.x, yVal - offset.y, -1);
285 for (
int i = 0; i < sortedStarMap.
size(); ++i)
286 (*starMap)[sortedToOriginal[i]] = sortedStarMap[i];
290 GuiderUtils::Vector StarCorrespondence::find(
const QList<Edge> &stars,
double maxDistance,
294 if (!initialized)
return GuiderUtils::Vector(-1, -1, -1);
295 int numFound, numNotFound;
301 sortByX(stars, &sortedStars, &sortedToOriginal);
304 int bestStarIndex = findInternal(sortedStars, maxDistance, &sortedStarMap, guideStarIndex,
305 guideStarOffsets, &numFound, &numNotFound, minFraction);
307 GuiderUtils::Vector starPosition(-1, -1, -1);
308 if (bestStarIndex > -1)
311 bestStarIndex = sortedToOriginal[bestStarIndex];
312 unmapStarMap(sortedStarMap, sortedToOriginal, starMap);
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;
319 else if (allowMissingGuideStar && bestStarIndex == -1 &&
320 stars.
size() >= minFraction * guideStarOffsets.size())
324 int bestNumFound = 0;
325 int bestNumNotFound = 0;
326 GuiderUtils::Vector bestPosition(-1, -1, -1);
327 for (
int gStarIndex = 0; gStarIndex < guideStarOffsets.size(); gStarIndex++)
329 if (gStarIndex == guideStarIndex)
332 makeOffsets(guideStarOffsets, &gStarOffsets, gStarIndex);
334 int detectedStarIndex = findInternal(sortedStars, maxDistance, &newStarMap,
335 gStarIndex, gStarOffsets,
336 &numFound, &numNotFound, minFraction);
337 if (detectedStarIndex >= 0 && numFound > bestNumFound)
339 GuiderUtils::Vector position = inventStarPosition(sortedStars, newStarMap, gStarOffsets,
340 guideStarOffsets[gStarIndex]);
341 if (position.x < 0 || position.y < 0)
344 bestPosition = position;
345 bestNumFound = numFound;
346 bestNumNotFound = numNotFound;
347 sortedStarMap = newStarMap;
349 if (numNotFound <= 1)
354 if (bestNumFound > 0)
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;
363 else qCDebug(KSTARS_EKOS_GUIDE) <<
"StarCorrespondence could not invent guideStar.";
365 else qCDebug(KSTARS_EKOS_GUIDE) <<
"StarCorrespondence could not find guideStar.";
367 if (adapt && (bestStarIndex != -1))
368 adaptOffsets(stars, *starMap);
376 void StarCorrespondence::initializeAdaptation()
379 const double timeConstant = 25.0;
380 alpha = 1.0 / pow(timeConstant, 0.865);
383 originalGuideStarOffsets = guideStarOffsets;
388 const int numStars = stars.
size();
389 if (starMap.
size() != numStars)
391 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: StarMap size != stars.size()" << starMap.
size() << numStars;
396 for (
int i = 0; i < numStars; ++i)
398 if (starMap[i] == guideStarIndex)
406 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: no guide star in map";
410 for (
int i = 0; i < numStars; ++i)
414 const int refIndex = starMap[i];
415 if (refIndex == -1 || refIndex == guideStarIndex)
418 if ((refIndex >= references.size()) || (refIndex < 0))
420 qCDebug(KSTARS_EKOS_GUIDE) <<
"Adapt error: bad refIndex[" << i <<
"] =" << refIndex;
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;
434 constexpr
double maxMovement = 2.5;
435 if ((fabs(newXOffset - originalGuideStarOffsets[refIndex].x) < maxMovement) &&
436 (fabs(newYOffset - originalGuideStarOffsets[refIndex].y) < maxMovement))
438 guideStarOffsets[refIndex].x = newXOffset;
439 guideStarOffsets[refIndex].y = newYOffset;