Add fusion trajectory replay view

src/MapWidget.cpp

@@ -108,6 +108,41 @@ void MapWidget::clearPoints()
     update();
 }
 
+void MapWidget::fitToPoints()
+{
+    if (m_points.isEmpty()) {
+        return;
+    }
+    double minimumLatitude = m_points.first().latitude;
+    double maximumLatitude = minimumLatitude;
+    double minimumLongitude = m_points.first().longitude;
+    double maximumLongitude = minimumLongitude;
+    for (const TelemetryPoint &point : m_points) {
+        minimumLatitude = std::min(minimumLatitude, point.latitude);
+        maximumLatitude = std::max(maximumLatitude, point.latitude);
+        minimumLongitude = std::min(minimumLongitude, point.longitude);
+        maximumLongitude = std::max(maximumLongitude, point.longitude);
+    }
+    m_centerLat = 0.5 * (minimumLatitude + maximumLatitude);
+    m_centerLon = 0.5 * (minimumLongitude + maximumLongitude);
+    const double availableWidth = std::max(100.0, width() * 0.82);
+    const double availableHeight = std::max(100.0, height() * 0.82);
+    for (int zoom = MaxZoom; zoom >= MinZoom; --zoom) {
+        const QPointF topLeft = coordinateToWorld(maximumLatitude, minimumLongitude, zoom);
+        const QPointF bottomRight = coordinateToWorld(minimumLatitude, maximumLongitude, zoom);
+        if (std::fabs(bottomRight.x() - topLeft.x()) <= availableWidth
+            && std::fabs(bottomRight.y() - topLeft.y()) <= availableHeight) {
+            m_zoom = zoom;
+            break;
+        }
+    }
+    if (m_autoFollow) {
+        m_autoFollow = false;
+        emit autoFollowChanged(false);
+    }
+    update();
+}
+
 void MapWidget::setAverageMarker(double latitude, double longitude, double radiusMeters)
 {
     m_hasAverageMarker = true;
@@ -216,6 +251,7 @@ void MapWidget::paintEvent(QPaintEvent *)
     painter.fillRect(rect(), QColor(219, 225, 220));
     drawTiles(painter);
     drawTelemetry(painter);
+    drawLegend(painter);
 
     if (m_points.isEmpty()) {
         drawEmptyState(painter);
@@ -387,23 +423,31 @@ void MapWidget::drawTelemetry(QPainter &painter)
 
     drawCepOverlay(painter);
 
-    QPainterPath path;
-    for (int i = 0; i < m_points.size(); ++i) {
-        const QPointF screen = coordinateToScreen(m_points[i].latitude, m_points[i].longitude);
-        if (i == 0) {
-            path.moveTo(screen);
-        } else {
-            path.lineTo(screen);
+    const auto drawSourcePath = [this, &painter](LocationSource source, const QColor &color) {
+        QPainterPath path;
+        bool havePoint = false;
+        int pointCount = 0;
+        for (const TelemetryPoint &point : m_points) {
+            if (point.source != source) {
+                continue;
+            }
+            const QPointF screen = coordinateToScreen(point.latitude, point.longitude);
+            havePoint ? path.lineTo(screen) : path.moveTo(screen);
+            havePoint = true;
+            ++pointCount;
         }
-    }
-
-    if (m_showRecentPoints && m_points.size() > 1) {
-        QPen pathPen(QColor(210, 105, 30), 3.0);
-        pathPen.setCapStyle(Qt::RoundCap);
-        pathPen.setJoinStyle(Qt::RoundJoin);
-        painter.setPen(pathPen);
-        painter.drawPath(path);
-    }
+        if (m_showRecentPoints && pointCount > 1) {
+            QPen pathPen(color, 3.0);
+            pathPen.setCapStyle(Qt::RoundCap);
+            pathPen.setJoinStyle(Qt::RoundJoin);
+            painter.setPen(pathPen);
+            painter.drawPath(path);
+        }
+    };
+    drawSourcePath(LocationSource::Trajectory, QColor(22, 163, 74));
+    drawSourcePath(LocationSource::Generic, QColor(210, 105, 30));
+    drawSourcePath(LocationSource::Gps, QColor(37, 99, 235));
+    drawSourcePath(LocationSource::ImuCorrected, QColor(220, 38, 38));
 
     drawAverageOverlay(painter);
     drawCepMarker(painter);
@@ -415,8 +459,15 @@ void MapWidget::drawTelemetry(QPainter &painter)
         const QPointF screen = coordinateToScreen(m_points[i].latitude, m_points[i].longitude);
         const double radius = isLast ? 8.0 : 5.0;
 
+        const QColor pointColor = m_points[i].source == LocationSource::ImuCorrected
+            ? QColor(220, 38, 38)
+            : (m_points[i].source == LocationSource::Gps
+                   ? QColor(37, 99, 235)
+                   : (m_points[i].source == LocationSource::Trajectory
+                          ? QColor(22, 163, 74)
+                          : (isLast ? QColor(210, 105, 30) : QColor(234, 88, 12))));
         painter.setPen(QPen(Qt::white, isLast ? 3.0 : 2.0));
-        painter.setBrush(isLast ? QColor(31, 150, 82) : QColor(37, 99, 235));
+        painter.setBrush(pointColor);
         painter.drawEllipse(screen, radius, radius);
 
         if (drawNumbers) {
@@ -436,6 +487,7 @@ void MapWidget::drawTelemetry(QPainter &painter)
         ? QString::number(last.longitude, 'f', 15) : last.longitudeText;
     lines << QStringLiteral("Lat: %1").arg(latitudeText);
     lines << QStringLiteral("Long: %1").arg(longitudeText);
+    lines << QStringLiteral("Kaynak: %1").arg(locationSourceText(last.source));
     if (last.hheight.has_value()) {
         lines << QStringLiteral("HHeight: %1").arg(*last.hheight, 0, 'f', 2);
     }
@@ -466,6 +518,37 @@ void MapWidget::drawTelemetry(QPainter &painter)
     painter.drawText(bubble.adjusted(9, 7, -9, -7), Qt::AlignLeft | Qt::AlignVCenter, label);
 }
 
+void MapWidget::drawLegend(QPainter &painter)
+{
+    if (m_points.isEmpty() || !m_showTelemetryOverlay) {
+        return;
+    }
+    const struct {
+        const char *text;
+        QColor color;
+    } entries[] = {
+        {"Trajectory (truth)", QColor(22, 163, 74)},
+        {"GPS (4 satellite)", QColor(37, 99, 235)},
+        {"IMU + ESKF corrected", QColor(220, 38, 38)}
+    };
+    const QRectF box(12.0, 12.0, 190.0, 76.0);
+    painter.setPen(QPen(QColor(50, 58, 65, 180), 1.0));
+    painter.setBrush(QColor(255, 255, 255, 232));
+    painter.drawRoundedRect(box, 6.0, 6.0);
+    painter.setFont(QFont(painter.font().family(), 9, QFont::DemiBold));
+    for (int index = 0; index < 3; ++index) {
+        const double y = 27.0 + index * 22.0;
+        painter.setPen(QPen(entries[index].color, 3.0));
+        painter.drawLine(QPointF(23.0, y), QPointF(43.0, y));
+        painter.setBrush(entries[index].color);
+        painter.drawEllipse(QPointF(33.0, y), 4.0, 4.0);
+        painter.setPen(QColor(25, 32, 38));
+        painter.drawText(QRectF(50.0, y - 9.0, 142.0, 18.0),
+                         Qt::AlignLeft | Qt::AlignVCenter,
+                         QString::fromLatin1(entries[index].text));
+    }
+}
+
 void MapWidget::drawCepOverlay(QPainter &painter)
 {
     if (!m_hasCepMarker || !m_cepRadiusMeters.has_value() || *m_cepRadiusMeters <= 0.0) {