diff --git a/.gitignore b/.gitignore index d4c3a57e..86feb6f3 100644 Binary files a/.gitignore and b/.gitignore differ diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 340843ca..2dfee84a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -84,6 +84,14 @@ public class RecordingFragment extends Fragment { private float previousPosX = 0f; private float previousPosY = 0f; + // Fused trajectory update loop (1-second interval) + private Handler fusedTrajectoryHandler; + private LatLng lastFusedPos = null; + private LatLng lastGnssObsPos = null; + private LatLng lastWifiObsPos = null; + private float previousObsPosX = 0f; + private float previousObsPosY = 0f; + // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -96,6 +104,15 @@ public void run() { } }; + /** Updates the fused best-estimate marker and trajectory polyline every 1 second. */ + private final Runnable fusedTrajectoryTask = new Runnable() { + @Override + public void run() { + updateFusedDisplay(); + fusedTrajectoryHandler.postDelayed(this, 1000); + } + }; + public RecordingFragment() { // Required empty public constructor } @@ -107,6 +124,7 @@ public void onCreate(Bundle savedInstanceState) { Context context = requireActivity(); this.settings = PreferenceManager.getDefaultSharedPreferences(context); this.refreshDataHandler = new Handler(); + this.fusedTrajectoryHandler = new Handler(); } @Nullable @@ -193,6 +211,9 @@ public void onViewCreated(@NonNull View view, // The blinking effect for recIcon blinkingRecordingIcon(); + // Start the 1-second fused position + trajectory update loop + fusedTrajectoryHandler.postDelayed(fusedTrajectoryTask, 1000); + // Start the timed or indefinite UI refresh if (this.settings.getBoolean("split_trajectory", false)) { // A maximum recording time is set @@ -228,7 +249,7 @@ private void onAddTestPoint() { LatLng cur = trajectoryMapFragment.getCurrentLocation(); if (cur == null) { Toast.makeText(requireContext(), "" + - "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", + "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", Toast.LENGTH_SHORT).show(); return; } @@ -247,6 +268,26 @@ private void onAddTestPoint() { trajectoryMapFragment.addTestPointMarker(idx, ts, cur); } + /** + * Updates the purple fused trajectory polyline every 1 second. + * Uses the WiFi fix as the best estimate when available; falls back to the current + * PDR-derived position. Raw GNSS is intentionally excluded here to avoid polyline jumps. + * The position marker (arrow) is updated separately in the 200ms loop via + * {@link #updateUIandPosition}. + */ + private void updateFusedDisplay() { + if (trajectoryMapFragment == null) return; + + LatLng bestEstimate = sensorFusion.getFusedPosition(); + if (bestEstimate == null) return; + + // Append to fused trajectory only if the position has moved > 0.3 m + if (lastFusedPos == null + || UtilFunctions.distanceBetweenPoints(lastFusedPos, bestEstimate) > 0.3) { + trajectoryMapFragment.updateFusedTrajectory(bestEstimate); + lastFusedPos = bestEstimate; + } + } /** * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. @@ -276,10 +317,15 @@ private void updateUIandPosition() { new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); - // Pass the location + orientation to the map + // Update the red PDR polyline and move the position marker. + // Priority: particle filter → WiFi fix → PDR-derived fallback. if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + float orientation = (float) Math.toDegrees(sensorFusion.passOrientation()); + trajectoryMapFragment.updateUserLocation(newLocation, orientation); + LatLng fusedPos = sensorFusion.getFusedPosition(); + if (fusedPos == null) fusedPos = sensorFusion.getLatLngWifiPositioning(); + trajectoryMapFragment.updateFusedPosition( + fusedPos != null ? fusedPos : newLocation, orientation); } } @@ -302,6 +348,44 @@ private void updateUIandPosition() { } } + // --- Colour-coded observation markers --- + + // GNSS observation: add a blue marker whenever the raw GNSS fix changes + float[] gnssRaw = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); + if (gnssRaw != null) { + LatLng gnssObs = new LatLng(gnssRaw[0], gnssRaw[1]); + if (!gnssObs.equals(lastGnssObsPos)) { + trajectoryMapFragment.addObservationMarker(gnssObs, + TrajectoryMapFragment.ObservationSource.GNSS); + lastGnssObsPos = gnssObs; + } + } + + // WiFi observation: add an orange marker and correct the particle filter + // whenever a new WiFi fix arrives + LatLng wifiObs = sensorFusion.getLatLngWifiPositioning(); + if (wifiObs != null && !wifiObs.equals(lastWifiObsPos)) { + trajectoryMapFragment.addObservationMarker(wifiObs, + TrajectoryMapFragment.ObservationSource.WIFI); + sensorFusion.correctWithWifiPosition(wifiObs); + lastWifiObsPos = wifiObs; + } + + // PDR observation: add a red marker whenever PDR position has moved ≥ 1 m + LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + if (currentLoc != null) { + double pdrDelta = Math.sqrt( + Math.pow(pdrValues[0] - previousObsPosX, 2) + + Math.pow(pdrValues[1] - previousObsPosY, 2)); + if (pdrDelta >= 1.0) { + trajectoryMapFragment.addObservationMarker(currentLoc, + TrajectoryMapFragment.ObservationSource.PDR); + previousObsPosX = pdrValues[0]; + previousObsPosY = pdrValues[1]; + } + } + + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; @@ -323,6 +407,7 @@ private void blinkingRecordingIcon() { public void onPause() { super.onPause(); refreshDataHandler.removeCallbacks(refreshDataTask); + fusedTrajectoryHandler.removeCallbacks(fusedTrajectoryTask); } @Override @@ -331,6 +416,7 @@ public void onResume() { if(!this.settings.getBoolean("split_trajectory", false)) { refreshDataHandler.postDelayed(refreshDataTask, 500); } + fusedTrajectoryHandler.postDelayed(fusedTrajectoryTask, 1000); } private int testPointIndex = 0; @@ -352,4 +438,4 @@ private static class TestPoint { private final List testPoints = new ArrayList<>(); -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java index d15a4a83..253c84b3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java @@ -25,17 +25,15 @@ /** * Sub fragment of Replay Activity. Fragment that replays trajectory data on a map. - *

- * The ReplayFragment is responsible for visualizing and replaying trajectory data captured during - * previous recordings. It loads trajectory data from a JSON file, updates the map with user movement, + * Loads trajectory data from a JSON file, updates the map with user movement, * and provides UI controls for playback, pause, and seek functionalities. - *

+ * * Features: * - Loads trajectory data from a file and displays it on a map. * - Provides playback controls including play, pause, restart, and go to end. * - Updates the trajectory dynamically as playback progresses. * - Allows users to manually seek through the recorded trajectory. - * - Integrates with {@link TrajectoryMapFragment} for map visualization. + * - Integrates with TrajectoryMapFragment for map visualization. * * @see TrajectoryMapFragment The map fragment displaying the trajectory. * @see ReplayActivity The activity managing the replay workflow. diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..f5174feb 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -30,17 +30,24 @@ import com.google.android.gms.maps.SupportMapFragment; import com.google.android.gms.maps.model.*; +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Paint; +import android.graphics.Typeface; + +import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Deque; import java.util.List; /** * A fragment responsible for displaying a trajectory map using Google Maps. - *

+ * * The TrajectoryMapFragment provides a map interface for visualizing movement trajectories, * GNSS tracking, and indoor mapping. It manages map settings, user interactions, and real-time * updates to user location and GNSS markers. - *

+ * * Key Features: * - Displays a Google Map with support for different map types (Hybrid, Normal, Satellite). * - Tracks and visualizes user movement using polylines. @@ -57,6 +64,18 @@ public class TrajectoryMapFragment extends Fragment { + /** Sources for colour-coded position observations on the map. */ + public enum ObservationSource { GNSS, WIFI, PDR } + + // Observation marker colours + private static final int COLOR_GNSS_OBS = 0xFF2196F3; // blue (unused directly; hue used below) + private static final int COLOR_WIFI_OBS = 0xFFFF9800; // orange + private static final int COLOR_PDR_OBS = 0xFFF44336; // red + private static final int COLOR_FUSED = 0xFF9C27B0; // purple – fused trajectory + + /** Weight applied to the newest sample in the EMA smoothing filter (0 < α ≤ 1). */ + private static final float EMA_ALPHA = 0.3f; + private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading @@ -71,6 +90,21 @@ public class TrajectoryMapFragment extends Fragment { private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location + // Fused trajectory – updated every 1 s or on movement + private Polyline fusedTrajectoryPolyline; + private LatLng lastFusedTrajectoryPoint = null; + + // Last 3 observation circle-markers per source; index 0 = newest (label "1") + private static final int OBS_HISTORY = 3; + private final Deque gnssObsMarkers = new ArrayDeque<>(); + private final Deque wifiObsMarkers = new ArrayDeque<>(); + private final Deque pdrObsMarkers = new ArrayDeque<>(); + + // EMA smoothing state + private boolean smoothingEnabled = false; + private double smoothedLat = Double.NaN; + private double smoothedLng = Double.NaN; + private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -91,11 +125,15 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothSwitch; + private SwitchMaterial pdrPathSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + private android.widget.LinearLayout switchesPanel; + private android.widget.ImageButton toggleControlsButton; public TrajectoryMapFragment() { @@ -120,10 +158,23 @@ public void onViewCreated(@NonNull View view, switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); + smoothSwitch = view.findViewById(R.id.smoothSwitch); + pdrPathSwitch = view.findViewById(R.id.pdrPathSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); switchColorButton = view.findViewById(R.id.lineColorButton); + switchesPanel = view.findViewById(R.id.switchesPanel); + toggleControlsButton = view.findViewById(R.id.toggleControlsButton); + toggleControlsButton.setOnClickListener(v -> { + if (switchesPanel.getVisibility() == View.VISIBLE) { + switchesPanel.setVisibility(View.GONE); + toggleControlsButton.setImageResource(android.R.drawable.arrow_down_float); + } else { + switchesPanel.setVisibility(View.VISIBLE); + toggleControlsButton.setImageResource(android.R.drawable.arrow_up_float); + } + }); // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -183,6 +234,19 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); + // Smoothing toggle + if (smoothSwitch != null) { + smoothSwitch.setOnCheckedChangeListener((btn, isChecked) -> + setSmoothingEnabled(isChecked)); + } + + // PDR path toggle — hidden by default, shown only when user enables it + if (pdrPathSwitch != null) { + pdrPathSwitch.setOnCheckedChangeListener((btn, isChecked) -> { + if (polyline != null) polyline.setVisible(isChecked); + }); + } + // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> { @@ -213,11 +277,8 @@ public void onMapReady(@NonNull GoogleMap googleMap) { /** * Initialize the map settings with the provided GoogleMap instance. - *

- * The method sets basic map settings, initializes the indoor map manager, - * and creates an empty polyline for user movement tracking. - * The method also initializes the GNSS polyline for tracking GNSS path. - * The method sets the map type to Hybrid and initializes the map with these settings. + * Sets basic map settings, initializes the indoor map manager, + * and creates an empty polyline for user movement tracking. * * @param map */ @@ -233,11 +294,12 @@ private void initMapSettings(GoogleMap map) { // Initialize indoor manager indoorMapManager = new IndoorMapManager(map); - // Initialize an empty polyline + // Initialize an empty polyline — hidden by default, toggled via pdrPathSwitch polyline = map.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) - .add() // start empty + .visible(false) + .add() ); // GNSS path in blue @@ -246,23 +308,18 @@ private void initMapSettings(GoogleMap map) { .width(5f) .add() // start empty ); + + // Fused best-estimate trajectory in purple + fusedTrajectoryPolyline = map.addPolyline(new PolylineOptions() + .color(COLOR_FUSED) + .width(8f) + .add() // start empty + ); } /** - * Initialize the map type spinner with the available map types. - *

- * The spinner allows the user to switch between different map types - * (e.g. Hybrid, Normal, Satellite) to customize their map view. - * The spinner is populated with the available map types and listens - * for user selection to update the map accordingly. - * The map type is updated directly on the GoogleMap instance. - *

- * Note: The spinner is initialized with the default map type (Hybrid). - * The map type is updated on user selection. - *

- *

- * @see com.google.android.gms.maps.GoogleMap The GoogleMap instance to update map type. + * Initialize the map type spinner with the available map types (Hybrid, Normal, Satellite). */ private void initMapTypeSpinner() { if (switchMapSpinner == null) return; @@ -301,60 +358,31 @@ public void onNothingSelected(AdapterView parent) {} } /** - * Update the user's current location on the map, create or move orientation marker, - * and append to polyline if the user actually moved. + * Records the user’s current PDR-derived location and extends the red trajectory polyline. + * Does NOT move the orientation marker — marker updates are handled exclusively by + * updateFusedPosition() to avoid competing updates from different loops. * - * @param newLocation The new location to plot. - * @param orientation The user’s heading (e.g. from sensor fusion). + * @param newLocation The new PDR-derived location. + * @param orientation Unused here; kept for API compatibility. */ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; - // Keep track of current location LatLng oldLocation = this.currentLocation; this.currentLocation = newLocation; - // If no marker, create it - if (orientationMarker == null) { - orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) - .flat(true) - .title("Current Position") - .icon(BitmapDescriptorFactory.fromBitmap( - UtilFunctions.getBitmapFromVector(requireContext(), - R.drawable.ic_baseline_navigation_24))) - ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); - } else { - // Update marker position + orientation - orientationMarker.setPosition(newLocation); - orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); - } - - // Extend polyline if movement occurred - /*if (oldLocation != null && !oldLocation.equals(newLocation) && polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); - points.add(newLocation); - polyline.setPoints(points); - }*/ - // Extend polyline + // Extend the red PDR polyline if (polyline != null) { List points = new ArrayList<>(polyline.getPoints()); - - // First position fix: add the first polyline point if (oldLocation == null) { points.add(newLocation); polyline.setPoints(points); } else if (!oldLocation.equals(newLocation)) { - // Subsequent movement: append a new polyline point points.add(newLocation); polyline.setPoints(points); } } - // Update indoor map overlay if (indoorMapManager != null) { indoorMapManager.setCurrentLocation(newLocation); @@ -366,12 +394,8 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { /** * Set the initial camera position for the map. - *

- * The method sets the initial camera position for the map when it is first loaded. - * If the map is already ready, the camera is moved immediately. - * If the map is not ready, the camera position is stored until the map is ready. - * The method also tracks if there is a pending camera move. - *

+ * If the map is not ready yet, the position is stored and applied once it is. + * * @param startLocation The initial camera position to set. */ public void setInitialCameraPosition(@NonNull LatLng startLocation) { @@ -443,6 +467,153 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } + /** + * Updates the user's best-estimate position on the map. + * When smoothing is enabled an EMA filter is applied before moving the orientation marker. + * + * @param pos Raw best-estimate position from the fusion / fallback pipeline. + * @param orientation Device heading in degrees (clockwise from north). + */ + public void updateFusedPosition(@NonNull LatLng pos, float orientation) { + if (gMap == null) return; + + LatLng displayPos; + if (smoothingEnabled) { + if (Double.isNaN(smoothedLat)) { + // Seed the filter on first call + smoothedLat = pos.latitude; + smoothedLng = pos.longitude; + } else { + smoothedLat = EMA_ALPHA * pos.latitude + (1.0 - EMA_ALPHA) * smoothedLat; + smoothedLng = EMA_ALPHA * pos.longitude + (1.0 - EMA_ALPHA) * smoothedLng; + } + displayPos = new LatLng(smoothedLat, smoothedLng); + } else { + displayPos = pos; + } + + if (orientationMarker == null) { + orientationMarker = gMap.addMarker(new MarkerOptions() + .position(displayPos) + .flat(true) + .title("Current Position") + .icon(BitmapDescriptorFactory.fromBitmap( + UtilFunctions.getBitmapFromVector(requireContext(), + R.drawable.ic_baseline_navigation_24)))); + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayPos, 19f)); + } else { + orientationMarker.setPosition(displayPos); + orientationMarker.setRotation(orientation); + gMap.moveCamera(CameraUpdateFactory.newLatLng(displayPos)); + } + + currentLocation = displayPos; + + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(displayPos); + setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + } + } + + /** + * Appends a point to the purple fused-trajectory polyline. + * Only adds a point when the position has actually changed to avoid duplicate vertices. + * + * @param pos New fused position to record. + */ + public void updateFusedTrajectory(@NonNull LatLng pos) { + if (gMap == null || fusedTrajectoryPolyline == null) return; + if (pos.equals(lastFusedTrajectoryPoint)) return; + + List points = new ArrayList<>(fusedTrajectoryPolyline.getPoints()); + points.add(pos); + fusedTrajectoryPolyline.setPoints(points); + lastFusedTrajectoryPoint = pos; + } + + // Show a numbered circle marker for the given source, keeping last 3 positions. + // Circle 1 = latest, 3 = oldest. + public void addObservationMarker(@NonNull LatLng pos, @NonNull ObservationSource source) { + if (gMap == null) return; + + Deque deque; + int solidColor; + String title; + switch (source) { + case GNSS: + deque = gnssObsMarkers; + solidColor = COLOR_GNSS_OBS; + title = "GNSS"; + break; + case WIFI: + deque = wifiObsMarkers; + solidColor = COLOR_WIFI_OBS; + title = "WiFi"; + break; + default: // PDR + deque = pdrObsMarkers; + solidColor = COLOR_PDR_OBS; + title = "PDR"; + break; + } + + if (deque.size() >= OBS_HISTORY) { + deque.removeLast().remove(); + } + + // Bump labels on existing markers + int newLabel = deque.size() + 1; + for (Marker m : deque) { + m.setIcon(BitmapDescriptorFactory.fromBitmap(makeObsCircleBitmap(solidColor, newLabel))); + newLabel--; + } + + Marker newest = gMap.addMarker(new MarkerOptions() + .position(pos) + .title(title) + .anchor(0.5f, 0.5f) + .zIndex(1f) + .icon(BitmapDescriptorFactory.fromBitmap(makeObsCircleBitmap(solidColor, 1)))); + deque.addFirst(newest); + } + + // Creates a semi-transparent filled circle bitmap with a number in the centre + private Bitmap makeObsCircleBitmap(int solidColor, int number) { + int size = 64; + Bitmap bmp = Bitmap.createBitmap(size, size, Bitmap.Config.ARGB_8888); + Canvas canvas = new Canvas(bmp); + + int fillColor = Color.argb(160, Color.red(solidColor), Color.green(solidColor), Color.blue(solidColor)); + Paint fillPaint = new Paint(Paint.ANTI_ALIAS_FLAG); + fillPaint.setStyle(Paint.Style.FILL); + fillPaint.setColor(fillColor); + canvas.drawCircle(size / 2f, size / 2f, size / 2f - 2, fillPaint); + + Paint textPaint = new Paint(Paint.ANTI_ALIAS_FLAG); + textPaint.setColor(Color.WHITE); + textPaint.setTypeface(Typeface.DEFAULT_BOLD); + textPaint.setTextSize(size * 0.45f); + textPaint.setTextAlign(Paint.Align.CENTER); + float textY = size / 2f - (textPaint.descent() + textPaint.ascent()) / 2f; + canvas.drawText(String.valueOf(number), size / 2f, textY, textPaint); + + return bmp; + } + + /** + * Enables or disables the EMA position smoothing filter. + * Disabling resets the filter so the next call re-seeds from the raw position. + * + * @param enabled true to enable smoothing. + */ + public void setSmoothingEnabled(boolean enabled) { + this.smoothingEnabled = enabled; + if (!enabled) { + smoothedLat = Double.NaN; + smoothedLng = Double.NaN; + } + } + /** * Remove GNSS marker if user toggles it off */ @@ -509,36 +680,46 @@ public void clearMapAndReset() { } testPointMarkers.clear(); + // Clear all per-source observation circle-markers + for (Marker m : gnssObsMarkers) m.remove(); + gnssObsMarkers.clear(); + for (Marker m : wifiObsMarkers) m.remove(); + wifiObsMarkers.clear(); + for (Marker m : pdrObsMarkers) m.remove(); + pdrObsMarkers.clear(); + + // Clear fused trajectory + if (fusedTrajectoryPolyline != null) { + fusedTrajectoryPolyline.remove(); + fusedTrajectoryPolyline = null; + } + lastFusedTrajectoryPoint = null; + + // Reset EMA filter + smoothedLat = Double.NaN; + smoothedLng = Double.NaN; // Re-create empty polylines with your chosen colors if (gMap != null) { polyline = gMap.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) + .visible(pdrPathSwitch != null && pdrPathSwitch.isChecked()) .add()); gnssPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.BLUE) .width(5f) .add()); + fusedTrajectoryPolyline = gMap.addPolyline(new PolylineOptions() + .color(COLOR_FUSED) + .width(8f) + .add()); } } /** - * Draw the building polygon on the map - *

- * The method draws a polygon representing the building on the map. - * The polygon is drawn with specific vertices and colors to represent - * different buildings or areas on the map. - * The method removes the old polygon if it exists and adds the new polygon - * to the map with the specified options. - * The method logs the number of vertices in the polygon for debugging. - *

- * - * Note: The method uses hard-coded vertices for the building polygon. - * - *

- * - * See: {@link com.google.android.gms.maps.model.PolygonOptions} The options for the new polygon. + * Draw the building polygon on the map using hard-coded vertices. + * Removes any existing polygon before adding the new one. */ private void drawBuildingPolygon() { if (gMap == null) { @@ -584,7 +765,7 @@ private void drawBuildingPolygon() { .add(nkml1, nkml2, nkml3, nkml4, nkml1) .strokeColor(Color.BLUE) // Blue border .strokeWidth(10f) // Border width - // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill + // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays PolygonOptions buildingPolygonOptions3 = new PolygonOptions() @@ -653,8 +834,10 @@ private void applyImmediateFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); + // Priority: MapMatcher fused floor + // Falls back to raw WiFi, then barometric estimate if neither is available. Will work when in lift as it detects it. + if (sensorFusion.getLatLngWifiPositioning() != null || sensorFusion.getMapMatcherFloor() >= 0) { + candidateFloor = sensorFusion.getMapMatcherFloor(); } else { float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); @@ -684,7 +867,7 @@ private void stopAutoFloor() { /** * Evaluates the current floor using WiFi positioning (priority) or * barometric elevation (fallback). Applies a 3-second debounce window - * to prevent jittery floor switching. + * to prevent jittery floor switching so it becomes smoother. */ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -692,9 +875,10 @@ private void evaluateAutoFloor() { int candidateFloor; - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); + // Priority 1: MapMatcher fused floor (barometer override > WiFi > raw barometric). + // getMapMatcherFloor() returns WiFi floor as fallback when MapMatcher not yet loaded. + if (sensorFusion.getLatLngWifiPositioning() != null || sensorFusion.getMapMatcherFloor() >= 0) { + candidateFloor = sensorFusion.getMapMatcherFloor(); } else { // Fallback: barometric elevation estimate float elevation = sensorFusion.getElevation(); @@ -712,12 +896,17 @@ private void evaluateAutoFloor() { } if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) { - indoorMapManager.setCurrentFloor(candidateFloor, true); - updateFloorLabel(); - // Reset timer so we don't keep re-applying the same floor + // Only apply a floor *change* when near stairs or a lift. + // Same-floor confirmations are always allowed (no movement required). + int currentFloor = indoorMapManager.getCurrentFloor(); + if (candidateFloor == currentFloor || sensorFusion.isNearTransition()) { + indoorMapManager.setCurrentFloor(candidateFloor, true); + updateFloorLabel(); + } + // Reset timer regardless, so we re-evaluate next second lastCandidateTime = now; } } //endregion -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java new file mode 100644 index 00000000..6e1d29c2 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java @@ -0,0 +1,157 @@ +package com.openpositioning.PositionMe.sensors; + +import android.util.Log; + +import java.util.ArrayList; +import java.util.List; + +/** + * Seperated in a different class for modularity. + * Static geometry helpers used by MapMatcher and ParticleFilter. + * Handles point-in-polygon checks and wall-crossing detection. + * Coordinates are local metres, same frame as ParticleFilter. + */ +class MapGeometry { + + private static final String TAG = "MapGeometry"; + + // No instances needed + private MapGeometry() {} + + + + /** + * Tests whether point (x, y) is inside the given polygon using ray-casting. + * + * Casts a ray horizontally to the left from (x, y) and counts how many polygon + * edges it crosses. An odd count means inside, even means outside. + * Returns false for null or degenerate polygons (fewer than 3 vertices). + * + * @param x easting in local metres + * @param y northing in local metres + * @param polygon list of float[]{eastingM, northingM} vertices in order + * @return true if the point is inside the polygon + */ + static boolean isPointInsidePolygon(float x, float y, List polygon) { + if (polygon == null || polygon.size() < 3) return false; + + int n = polygon.size(); + boolean inside = false; + int j = n - 1; + + for (int i = 0; i < n; i++) { + float xi = polygon.get(i)[0], yi = polygon.get(i)[1]; + float xj = polygon.get(j)[0], yj = polygon.get(j)[1]; + + if (((yi > y) != (yj > y)) + && (x < (xj - xi) * (y - yi) / (yj - yi) + xi)) { + inside = !inside; + } + j = i; + } + return inside; + } + + + + /** + * Returns true if the segment from (x1,y1) to (x2,y2) crosses any edge of the polygon. + * + * Called by ParticleFilter.predict() after each particle is displaced. If the move + * segment crosses a wall edge, the particle is snapped back to its previous position. + * Iterates all consecutive vertex pairs of the polygon and checks each edge with + * segmentsIntersect(). The last edge joins the final vertex back to the first. + * + * Returns false for null or single-point polygons. + * + * @param x1 start easting in local metres + * @param y1 start northing in local metres + * @param x2 end easting in local metres + * @param y2 end northing in local metres + * @param polygon wall vertices as float[]{eastingM, northingM} + */ + static boolean doesSegmentCrossPolygon( + float x1, float y1, float x2, float y2, + List polygon) { + if (polygon == null || polygon.size() < 2) return false; + int n = polygon.size(); + for (int i = 0; i < n; i++) { + float[] a = polygon.get(i); + float[] b = polygon.get((i + 1) % n); + if (segmentsIntersect(x1, y1, x2, y2, a[0], a[1], b[0], b[1])) { + return true; + } + } + return false; + } + + /** + * Returns true if segment AB and segment CD intersect. + * Uses parametric cross-product; returns false for parallel/collinear segments. + * + * @param ax start of segment A - easting + * @param ay start of segment A - northing + * @param bx end of segment A - easting + * @param by end of segment A - northing + * @param cx start of segment B - easting + * @param cy start of segment B - northing + * @param dx end of segment B - easting + * @param dy end of segment B - northing + */ + private static boolean segmentsIntersect( + float ax, float ay, float bx, float by, + float cx, float cy, float dx, float dy) { + float d1x = bx - ax, d1y = by - ay; // direction of segment A + float d2x = dx - cx, d2y = dy - cy; // direction of segment B + float cross = d1x * d2y - d1y * d2x; + if (Math.abs(cross) < 1e-6f) return false; // parallel or collinear + float t = ((cx - ax) * d2y - (cy - ay) * d2x) / cross; + float u = ((cx - ax) * d1y - (cy - ay) * d1x) / cross; + return t >= 0f && t <= 1f && u >= 0f && u <= 1f; + } + + + + /** + * Runs a set of known-answer checks against isPointInsidePolygon and + * doesSegmentCrossPolygon. Called once from MapMatcher.tryLoadBuilding() + * after building data is parsed. Results are written to Logcat under the + * MapGeometry tag so any geometry regression shows up immediately. + */ + static void selfTest() { + // Test polygon: 10x10 unit square with corners at (0,0), (10,0), (10,10), (0,10) + List square = new ArrayList<>(); + square.add(new float[]{0f, 0f}); + square.add(new float[]{10f, 0f}); + square.add(new float[]{10f, 10f}); + square.add(new float[]{0f, 10f}); + + check("isPointInsidePolygon(5,5) == true", + isPointInsidePolygon(5f, 5f, square), true); + + check("isPointInsidePolygon(15,5) == false", + isPointInsidePolygon(15f, 5f, square), false); + + // Segment crossing the left edge of the square (x=-2 to x=5, y=5): should cross + check("doesSegmentCrossPolygon(-2,5 -> 5,5) == true", + doesSegmentCrossPolygon(-2f, 5f, 5f, 5f, square), true); + + // Segment that starts and ends inside the square - no edge crossing + check("doesSegmentCrossPolygon(2,2 -> 8,8) == false", + doesSegmentCrossPolygon(2f, 2f, 8f, 8f, square), false); + + // Segment completely outside the square - no crossing + check("doesSegmentCrossPolygon(12,0 -> 15,10) == false", + doesSegmentCrossPolygon(12f, 0f, 15f, 10f, square), false); + } + + // Logs PASS or FAIL for a single self-test case. + private static void check(String name, boolean result, boolean expected) { + if (result == expected) { + Log.d(TAG, "PASS: " + name); + } else { + Log.e(TAG, "FAIL: " + name + + " (got " + result + ", expected " + expected + ")"); + } + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java new file mode 100644 index 00000000..72cab52c --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java @@ -0,0 +1,451 @@ +package com.openpositioning.PositionMe.sensors; + +import android.util.Log; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; +import com.openpositioning.PositionMe.utils.BuildingPolygon; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Seperated in a different class for modularity. + * Loads and stores floorplan geometry for the current building. + * Provides wall data to ParticleFilter, stair/lift centroids to SensorFusion, + * and floor index resolution combining WiFi with barometer overrides. + * Handles buildings where the API floor list is not in physical order + * (e.g. Nucleus stores GF at index 4). + * + * Call tryLoadBuilding() once at the start of a recording to initialise. + */ +class MapMatcher { + + private static final String TAG_LOAD = "MapMatcher"; + + // Equatorial approximation: metres per degree of lat/lon. Matches the constant in ParticleFilter. + private static final double METERS_PER_DEGREE = 111111.0; + + + + // A single wall polygon or polyline from the floorplan API, converted to local metres. + static class WallFeature { + // Vertices as {eastingM, northingM} relative to the particle filter origin + final List localPoints; + // True for filled Polygon/MultiPolygon shapes, false for LineString wall segments + final boolean isPolygon; + + WallFeature(List pts, boolean isPolygon) { + this.localPoints = pts; + this.isPolygon = isPolygon; + } + } + + + private final SensorFusion sensorFusion; + + // Set by tryLoadBuilding(); null/zero until loading succeeds + private String loadedBuildingId = null; + private LatLng mapOrigin = null; + private int numFloors = 0; + + // displayName ("GF", "F1", "LG" etc.) -> floorShapes list index. + // Built at load time. Used by getAdjacentFloorIndex() for reverse-lookup. + private Map displayNameToIndex = null; + + // Arrays indexed [0..numFloors-1], one list per floor. + // All three are null until tryLoadBuilding() completes. + @SuppressWarnings("unchecked") + private List[] wallsByFloor = null; + + // Centroids of staircase and lift features on each floor, in local metres. + // Used by SensorFusion.isNearTransition() to gate barometer floor changes. + @SuppressWarnings("unchecked") + private List[] stairCentersByFloor = null; + @SuppressWarnings("unchecked") + private List[] liftCentersByFloor = null; + + // When SensorFusion accepts a barometer floor change, it calls setCurrentFloor() to + // store the confirmed API index here. -1 means no override, fall back to WiFi. + private int currentFloorOverride = -1; + + // Physical floor number (LG=-1, GF=0, F1=1 etc...) -> API floorShapes list index. + // Built at load time. Needed because some buildings store floors out of physical order + // (e.g. Nucleus: (LG, F1, F2, F3, GF) so GF is at index 4, not 0). + private Map physicalToApiIndex = null; + + + + /** + * Creates a MapMatcher. No geometry is loaded until tryLoadBuilding() is called. + */ + MapMatcher(SensorFusion sf) { + this.sensorFusion = sf; + } + + + /** + * Loads wall, stair, and lift features from the floorplan cache for the current building. + * + * Building is selected in order: preferred name first (if provided), then + * point-in-polygon check against all cached building outlines, then closest center + * as fallback. Returns early if origin is null or no building is found in the cache. + * + * On success, builds the displayName and physicalFloor index lookup maps, converts + * all floor geometry to local metres, and runs MapGeometry.selfTest(). + * + */ + @SuppressWarnings("unchecked") + void tryLoadBuilding(String preferredBuildingId, LatLng origin) { + if (origin == null) { + Log.w(TAG_LOAD, "tryLoadBuilding: origin is null — aborting"); + return; + } + + // Preferred building by name + FloorplanApiClient.BuildingInfo building = null; + if (preferredBuildingId != null && !preferredBuildingId.isEmpty()) { + building = sensorFusion.getFloorplanBuilding(preferredBuildingId); + if (building != null) { + Log.d(TAG_LOAD, "Found preferred building: " + preferredBuildingId); + } + } + + // Polygon detection against all cached buildings + if (building == null) { + for (FloorplanApiClient.BuildingInfo b : sensorFusion.getFloorplanBuildings()) { + List outline = b.getOutlinePolygon(); + if (outline != null && outline.size() >= 3 + && BuildingPolygon.pointInPolygon(origin, outline)) { + building = b; + Log.d(TAG_LOAD, "Building detected via polygon: " + b.getName()); + break; + } + } + } + + // Closest center fallback + if (building == null) { + building = closestBuilding(origin); + if (building != null) { + Log.d(TAG_LOAD, "Building selected by closest center: " + building.getName()); + } + } + + if (building == null) { + Log.w(TAG_LOAD, "tryLoadBuilding: no building found in cache — map matching disabled"); + return; + } + + List floorShapes = building.getFloorShapesList(); + if (floorShapes == null || floorShapes.isEmpty()) { + Log.w(TAG_LOAD, "tryLoadBuilding: building has no floor shapes"); + return; + } + + // Allocate per-floor lists + numFloors = floorShapes.size(); + mapOrigin = origin; + loadedBuildingId = building.getName(); + + // Build both lookup maps: displayName to index (for adjacent-floor arithmetic) + // and physicalFloor to index (for WiFi floor conversion) + displayNameToIndex = new HashMap<>(); + physicalToApiIndex = new HashMap<>(); + for (int f = 0; f < numFloors; f++) { + String name = floorShapes.get(f).getDisplayName(); + if (name != null) { + displayNameToIndex.put(name, f); + physicalToApiIndex.put(displayNameToPhysical(name), f); + } + } + + wallsByFloor = new List[numFloors]; + stairCentersByFloor = new List[numFloors]; + liftCentersByFloor = new List[numFloors]; + currentFloorOverride = -1; + + for (int f = 0; f < numFloors; f++) { + wallsByFloor[f] = new ArrayList<>(); + stairCentersByFloor[f] = new ArrayList<>(); + liftCentersByFloor[f] = new ArrayList<>(); + + FloorplanApiClient.FloorShapes floor = floorShapes.get(f); + // Sort features into walls (for crossing checks) and + // stairs/lifts (centroid stored for proximity gating). + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + + if ("wall".equals(type)) { + String geoType = feature.getGeometryType(); + boolean isPoly = "MultiPolygon".equals(geoType) || "Polygon".equals(geoType); + + for (List part : feature.getParts()) { + List localPts = new ArrayList<>(part.size()); + for (LatLng ll : part) { + localPts.add(latLngToLocal(ll, origin)); + } + if (localPts.size() < 2) continue; + wallsByFloor[f].add(new WallFeature(localPts, isPoly)); + } + + } else if ("stairs".equals(type) || "staircase".equals(type)) { + for (List part : feature.getParts()) { + float[] centroid = computeCentroidLocal(part, origin); + if (centroid != null) stairCentersByFloor[f].add(centroid); + } + + } else if ("lift".equals(type) || "elevator".equals(type)) { + for (List part : feature.getParts()) { + float[] centroid = computeCentroidLocal(part, origin); + if (centroid != null) liftCentersByFloor[f].add(centroid); + } + } + } + + Log.d(TAG_LOAD, String.format("Floor %d (%s): walls=%d stairs=%d lifts=%d", + f, floor.getDisplayName(), + wallsByFloor[f].size(), + stairCentersByFloor[f].size(), + liftCentersByFloor[f].size())); + } + + Log.d(TAG_LOAD, "Loaded: building=" + loadedBuildingId + + " floors=" + numFloors); + + // Step 2: run geometry self-test + MapGeometry.selfTest(); + } + + /** + * Returns true once tryLoadBuilding() has successfully parsed at least one floor. + */ + boolean isInitialised() { + return loadedBuildingId != null && mapOrigin != null && numFloors > 0; + } + + + // Returns the wall features for the given API floor index, or an empty list if out of range. + List getWallsForFloor(int floorIndex) { + if (wallsByFloor == null || floorIndex < 0 || floorIndex >= numFloors) { + return new ArrayList<>(); + } + return wallsByFloor[floorIndex]; + } + + + + // Returns the building name that was loaded, or null if tryLoadBuilding() has not succeeded yet. + String getLoadedBuildingId() { + return loadedBuildingId; + } + + /** + * Returns the current API floorShapes index. Index 0 = floorShapesList[0], etc. + * + * A barometer-confirmed override set by SensorFusion takes priority over the WiFi + * reading. When no override is set, the WiFi floor integer is converted to an API + * index via physicalToApiIndex. Falls back to the bias formula if the lookup map + * is not ready yet (Nucleus/Murchison: WiFi 0 maps to index 1; others: index 0). + */ + int getLikelyFloorIndex() { + // Barometer override beats WiFi when SensorFusion has confirmed a floor change + if (currentFloorOverride >= 0 && currentFloorOverride < numFloors) { + Log.d(TAG_LOAD, "getLikelyFloorIndex: using barometer override=" + currentFloorOverride); + return currentFloorOverride; + } + int wifiFloor = sensorFusion.getWifiFloor(); + // physicalToApiIndex handles buildings where the API list is not in physical order + // (e.g. Nucleus: (LG, F1, F2, F3, GF) - WiFi 0 = GF = API index 4, not index 1). + // Falls back to the bias formula when the map has not loaded yet. + Integer apiIdx = (physicalToApiIndex != null) ? physicalToApiIndex.get(wifiFloor) : null; + int result = (apiIdx != null) + ? apiIdx + : Math.max(0, Math.min(numFloors - 1, wifiFloor + getAutoFloorBias())); + Log.d(TAG_LOAD, "getLikelyFloorIndex: wifiFloor=" + wifiFloor + + " bias=" + getAutoFloorBias() + " index=" + result); + return result; + } + + /** + * Returns the index offset between WiFi floor 0 (GF) and its position in the API floor list. + * Nucleus and Murchison store LG at index 0, so GF is at index 1 (bias = 1). + * All other buildings place GF at index 0 (bias = 0). + */ + int getAutoFloorBias() { + if ("nucleus_building".equals(loadedBuildingId) + || "murchison_house".equals(loadedBuildingId)) { + return 1; + } + return 0; + } + + /** + * Returns the floor-to-floor height in metres for the loaded building. + * SensorFusion divides the accumulated barometer elevation change by this value + * to determine how many floors the user has moved. + */ + float getFloorHeight() { + if ("nucleus_building".equals(loadedBuildingId)) return 5.0f; + if ("murchison_house".equals(loadedBuildingId)) return 4.0f; + if ("library".equals(loadedBuildingId)) return 3.6f; + return 4.0f; // generic fallback + } + + /** + * Records a barometer-confirmed floor index so getLikelyFloorIndex() returns it + * instead of the WiFi estimate. Called by SensorFusion.checkAndApplyFloorChange(). + * Pass -1 to clear the override and let WiFi drive floor selection again. + */ + void setCurrentFloor(int floor) { + currentFloorOverride = floor; + } + + // Returns the staircase centroids in local metres for floor f, or an empty list. + List getStairCentersForFloor(int f) { + if (stairCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); + return stairCentersByFloor[f]; + } + + // Returns the lift centroids in local metres for floor f, or an empty list. + List getLiftCentersForFloor(int f) { + if (liftCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); + return liftCentersByFloor[f]; + } + + /** + * Converts a physical floor number (LG=-1, GF=0, F1=1, F2=2, etc ...) to its + * position in the API floorShapes list. Returns -1 if not found. + * + * SensorFusion calls this when WiFi changes floor rather than going through + * getLikelyFloorIndex(), because getLikelyFloorIndex() already reflects the + * new WiFi value and combining it with getAdjacentFloorIndex() would shift + * one floor too far. + */ + int physicalFloorToApiIndex(int physicalFloor) { + if (physicalToApiIndex == null) return -1; + Integer idx = physicalToApiIndex.get(physicalFloor); + return (idx != null) ? idx : -1; + } + + /** + * Converts a WiFi floor integer to the display name used in the floorplan API. + * + * For Nucleus and Murchison, the API stores floors in this order: + * index 0=LG, 1=F1, 2=F2, 3=F3, 4=GF + * GF is at index 4, so this cannot simply call String.valueOf(wifiFloor). + * For other buildings, the WiFi integer is used as a plain string ("0", "1", etc.). + */ + private String wifiFloorToDisplayName(int wifiFloor) { + if ("nucleus_building".equals(loadedBuildingId) + || "murchison_house".equals(loadedBuildingId)) { + switch (wifiFloor) { + case 0: return "LG"; + case 1: return "F1"; + case 2: return "F2"; + case 3: return "F3"; + case 4: return "GF"; + default: + // Unknown floor — infer from barometer elevation + return (sensorFusion.getElevation() < -1.5f) ? "LG" : "GF"; + } + } + return String.valueOf(wifiFloor); + } + + + /** + * Returns the API index that is physicalDelta floors above/below currentApiIndex. + * Can't just do +/-1 on the index for buildings with non-physical API ordering + * (e.g. Nucleus GF is at index 4, not 0), so we convert via display name instead. + * Fixes the GF/F1 off-by-one seen in the auto-floor feature. + * Returns currentApiIndex unchanged if the target floor doesn't exist. + */ + int getAdjacentFloorIndex(int currentApiIndex, int physicalDelta) { + if (physicalToApiIndex == null || displayNameToIndex == null) return currentApiIndex; + // Reverse-lookup: find the display name that maps to this API index + for (Map.Entry e : displayNameToIndex.entrySet()) { + if (e.getValue() == currentApiIndex) { + int currentPhysical = displayNameToPhysical(e.getKey()); + int targetPhysical = currentPhysical + physicalDelta; + Integer targetApi = physicalToApiIndex.get(targetPhysical); + return (targetApi != null) ? targetApi : currentApiIndex; + } + } + return currentApiIndex; + } + + /** + * Converts a floor display name to a physical floor number. + * LG = -1, GF = 0, F1 = 1, F2 = 2, F3 = 3 + * Unknown names default to 0. + */ + private static int displayNameToPhysical(String name) { + if (name == null) return 0; + switch (name) { + case "LG": return -1; + case "GF": return 0; + default: + if (name.length() > 1 && name.charAt(0) == 'F') { + try { return Integer.parseInt(name.substring(1)); } + catch (NumberFormatException ignored) {} + } + try { return Integer.parseInt(name); } + catch (NumberFormatException ignored) { return 0; } + } + } + + /** + * Computes the centroid of a set of LatLng points in local metres. + * Used to get a single representative point for stairs/lift polygons. + * Returns null if pts is null or empty. + */ + private static float[] computeCentroidLocal(List pts, LatLng origin) { + if (pts == null || pts.isEmpty()) return null; + float sumE = 0f, sumN = 0f; + for (LatLng ll : pts) { + float[] local = latLngToLocal(ll, origin); + sumE += local[0]; + sumN += local[1]; + } + return new float[]{sumE / pts.size(), sumN / pts.size()}; + } + + /** + * Converts a WGS84 LatLng to a local easting/northing offset in metres + * relative to the particle filter origin. Uses the same equirectangular + * approximation as ParticleFilter.latLngToLocal() to keep coordinate frames + * consistent between the two classes. + */ + private static float[] latLngToLocal(LatLng point, LatLng origin) { + double latDiff = point.latitude - origin.latitude; + double lonDiff = point.longitude - origin.longitude; + float northing = (float) (latDiff * METERS_PER_DEGREE); + float easting = (float) (lonDiff * METERS_PER_DEGREE + * Math.cos(Math.toRadians(origin.latitude))); + return new float[]{easting, northing}; + } + + /** + * Returns the cached building whose centre is closest to origin. + * Used as a last resort when no building polygon contains the origin point. + * Distance is compared as squared lat/lon degrees (no sqrt needed for ordering). + */ + private FloorplanApiClient.BuildingInfo closestBuilding(LatLng origin) { + FloorplanApiClient.BuildingInfo best = null; + double bestDist = Double.MAX_VALUE; + for (FloorplanApiClient.BuildingInfo b : sensorFusion.getFloorplanBuildings()) { + LatLng center = b.getCenter(); + double dl = center.latitude - origin.latitude; + double dn = center.longitude - origin.longitude; + double dist = dl * dl + dn * dn; + if (dist < bestDist) { + bestDist = dist; + best = b; + } + } + return best; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java new file mode 100644 index 00000000..705d0e6c --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -0,0 +1,412 @@ +package com.openpositioning.PositionMe.sensors; + +import com.google.android.gms.maps.model.LatLng; + +import android.util.Log; + +import java.util.List; +import java.util.Random; + +/** + * The Particle Filter class upholds a sensor fusion algorithm. + * + * Fuses PDR motion estimates with WiFi and GNSS position fixes through + * a predict, update and resample cycle to produce a fused position estimate + * of user location. + */ + +public class ParticleFilter { + + // Number of particles maintained by the filter + private static final int Num_Particles = 200; + + // Meters conversion constant for WGS84 to easting, northing space + private static final double Meters_Per_Degree = 111111.0; + + // Approximated standard deviation spread from GNSS/WiFi fix. + private static final float GNSS_Init_STD = 15.0f; + private static final float WiFi_Init_STD = 8.0f; + + // PDR process noise standard deviation approximation (adjustable) + public static final float PDR_Process_Noise_STD = 0.4f; + + // Min and max GNSS measurement accuracy for weight update (define rejected measurements) + private static final float GNSS_Min_Accuracy = 5.0f; + private static final float GNSS_Max_Accuracy = 50.0f; + + // The defined particle set + private final Particle[] particles; + + // WGS84 reference point used as the local coordinate frame origin + private LatLng originLatLng; + + // Weighted estimate easting and northing of the current position estimate + private float estimatedX; + private float estimatedY; + + // True once initial position estimate has been established + private boolean initialised; + + private final Random random; + private final SensorFusion sensorFusion; + + // MapMatcher used to check wall crossings during prediction. Null until set via setMapMatcher(). + private MapMatcher mapMatcher; + + // Defines a single hypothesis about the user's position + private static class Particle{ + // Easting offset from local origin + float x; + + // Northing offset from local origin + float y; + + // Normalised importance weight + float weight; + + Particle(float x, float y, float weight) { + this.x = x; + this.y = y; + this.weight = weight; + } + } + + /** + * Constructor for the Particle Filter class. + * + * Attempts to initialise position and particles estimate from available WiFi and GNSS data. + * If neither are available at construction, tryInitialise() attempts again on later call. + */ + public ParticleFilter(){ + this.sensorFusion = SensorFusion.getInstance(); + this.particles = new Particle[Num_Particles]; + this.random = new Random(); + this.initialised = false; + + initialisePosition(); + } + + /** + * Automatically estimates the first available position from WiFi or GNSS + * and defines the particle set around this WGS84 point as an isotropic 2D + * Gaussian distribution. + * + * WiFi estimation prioritised for its greater indoor positioning accuracy. + * If unavailable, GNSS is used for the initial estimation instead. + * If neither is available, tryIinitialise() is called later. + */ + private void initialisePosition() { + float[] gnssLatLon = sensorFusion.getGNSSLatitude(false); + LatLng wifiLatLng = sensorFusion.getLatLngWifiPositioning(); + + // GNSS positioning returns null if no reading is available + boolean hasGNSS = gnssLatLon != null + && (gnssLatLon[0] != 0f || gnssLatLon[1] != 0f); + + // WiFi positioning returns null if no reading is available + boolean hasWifi = wifiLatLng != null; + + // Defer initialisation if neither GNSS or WiFi have a valid reading + if (!hasGNSS && !hasWifi) { + return; + } + + // Initialisation source and particle spread based on priority + LatLng initLatLng; + float spreadStd; + + // Uses WiFi positioning if available, otherwise GNSS as fallback + if (hasWifi) { + initLatLng = wifiLatLng; + spreadStd = WiFi_Init_STD; + } else { + initLatLng = new LatLng(gnssLatLon[0], gnssLatLon[1]); + spreadStd = GNSS_Init_STD; + } + + // Set local frame origin at initial estimated position + originLatLng = initLatLng; + Log.d("ParticleFilter", "Origin set to: " + + originLatLng.latitude + ", " + + originLatLng.longitude); + + // Distribute particles around (0 , 0) in local frame + spreadParticles(0.0f, 0.0f, spreadStd); + initialised = true; + } + + /** + * Sets the MapMatcher used for wall-crossing checks in predict(). + * Called once from SensorFusion after both objects are created. + */ + public void setMapMatcher(MapMatcher mm) { + this.mapMatcher = mm; + } + + /** + * Function to retry initial position estimation if it was deferred at construction + */ + public void tryInitialise() { + if (!initialised) { + initialisePosition(); + } + } + + /** + * Distributes particles as an isotropic 2D Gaussian distribution around + * (centerX, centerY) and assigns equal weights to each particle (1/Num_Particles). + */ + private void spreadParticles(float centerX, float centerY, float stdM) { + float initWeight = 1.0f / Num_Particles; + for (int i = 0; i < Num_Particles; i++) { + float x = centerX + (float) (random.nextGaussian() * stdM); + float y = centerY + (float) (random.nextGaussian() * stdM); + particles[i] = new Particle(x, y, initWeight); + } + + estimatedX = centerX; + estimatedY = centerY; + } + + /** + * PDR motion model prediction + * + * Moves all defined particles according to PDR displacement with + * added Gaussian process noise to account for uncertainty in step length + * and heading. + */ + public void predict(float deltaEast, float deltaNorth, float PDRstd) { + // Early return if filter not initialised + if (!initialised) { + return; + } + + // Save positions before the move so we can snap back any particle that crosses a wall + float[] oldX = new float[Num_Particles]; + float[] oldY = new float[Num_Particles]; + for (int i = 0; i < Num_Particles; i++) { + oldX[i] = particles[i].x; + oldY[i] = particles[i].y; + } + + // Shift each particle with Gaussian noise considered + for (int i = 0; i < Num_Particles; i++) { + float noiseX = (float) (random.nextGaussian() * PDRstd); + float noiseY = (float) (random.nextGaussian() * PDRstd); + particles[i].x += deltaEast + noiseX; + particles[i].y += deltaNorth + noiseY; + } + + // If MapMatcher is loaded, snap back any particle whose move crosses a wall on the current floor. + if (mapMatcher != null && mapMatcher.isInitialised()) { + int floorIndex = mapMatcher.getLikelyFloorIndex(); + List walls = mapMatcher.getWallsForFloor(floorIndex); + int snapped = 0; + for (int i = 0; i < Num_Particles; i++) { + for (MapMatcher.WallFeature wall : walls) { + if (MapGeometry.doesSegmentCrossPolygon( + oldX[i], oldY[i], particles[i].x, particles[i].y, + wall.localPoints)) { + particles[i].x = oldX[i]; + particles[i].y = oldY[i]; + snapped++; + break; + } + } + } + + // Debug and terminal verification + Log.d("ParticleFilter", "Wall rejection (floor " + floorIndex + "): " + + snapped + "/" + Num_Particles + " particles snapped this step"); + } + + // Update weighted mean estimate + estimatedX = 0f; + estimatedY = 0f; + for (int i = 0; i < Num_Particles; i++) { + estimatedX += particles[i].x * particles[i].weight; + estimatedY += particles[i].y * particles[i].weight; + } + + //Debug test and terminal verification + Log.d("ParticleFilter", "Delta: (" + deltaEast + ", " + deltaNorth + + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); + } + + /** + * + */ + public void updateWeights(LatLng measurementLatLng, float accuracy) { + // Early return if not initialised + if (!initialised || measurementLatLng == null) { + return; + } + + // Early return if out of accuracy range (poor measurement) + if (accuracy <= 0 || accuracy >= GNSS_Max_Accuracy) { + return; + } + float sigma = Math.max(accuracy, GNSS_Min_Accuracy); + + // Convert received measurement to local frame (meters) + float[] Measurement_Local = latLngToLocal(measurementLatLng); + float mx = Measurement_Local[0]; + float my = Measurement_Local[1]; + + // Calculate each particle weighting + float twoSigmaSquared = 2.0f * sigma * sigma; + for (int i = 0; i < Num_Particles; i++) { + // Calculate distance in meters from particle to measurement + float dx = particles[i].x - mx; + float dy = particles[i].y - my; + + // Gaussian likelihood equation + float d = (float) Math.sqrt(dx * dx + dy * dy); + float dSquared = d * d; + float likelihood = (float) Math.exp(-dSquared / twoSigmaSquared); + particles[i].weight *= likelihood; + } + + // Normalise weights and to sum = 1.0 + float weightSum = 0f; + for (int i = 0; i < Num_Particles; i++) { + weightSum += particles[i].weight; + } + for (int i = 0; i < Num_Particles; i++) { + particles[i].weight /= weightSum; + } + + // Recalculate weighted mean estimate of position + estimatedX = 0f; + estimatedY = 0f; + for (int i = 0; i < Num_Particles; i++) { + estimatedX += particles[i].x * particles[i].weight; + estimatedY += particles[i].y * particles[i].weight; + } + + // Debugging code for terminal output and observation + float maxWeight = 0f; + int maxIndex = 0; + for (int i = 0; i < Num_Particles; i++) { + if (particles[i].weight > maxWeight) { + maxWeight = particles[i].weight; + maxIndex = i; + } + } + + Log.d("ParticleFilter", "Update called - " + + "Measurement: (" + mx + ", " + my + ") " + + "Estimate: (" + estimatedX + ", " + estimatedY + ") " + + "Best particle: (" + particles[maxIndex].x + ", " + particles[maxIndex].y + ") " + + "Max weight: " + maxWeight); + + resample(); + } + + /** + * Systematic resampling of particles for weight degeneration solution. + * + * Eliminates low weight particles and duplicates high weight particles when + * N_eff < Num_Particles / 2. + */ + private void resample() { + // Calculate effective sample size as 1 / sum of particle weights squared + float sumWeightsSquared = 0f; + for (int i = 0; i < Num_Particles; i++) { + sumWeightsSquared += particles[i].weight * particles[i].weight; + } + float N_eff = 1.0f / sumWeightsSquared; + + // Early return if effective sample size >= set threshold + float threshold = Num_Particles / 2.0f; + if (N_eff >= threshold) { + Log.d("ParticleFilter", "Resampling skipped"); + return; + } + + // Build cumulative sum array from normalised weights + float[] cumulativeSum = new float[Num_Particles]; + cumulativeSum[0] = particles[0].weight; + for (int i = 1; i < Num_Particles; i++) { + cumulativeSum[i] = cumulativeSum[i-1] + particles[i].weight; + } + + // Set uniform starting point + float overN = 1.0f / Num_Particles; + float u1 = random.nextFloat() * overN; + + Particle[] resampledParticles = new Particle[Num_Particles]; + float uniformWeighting = overN; + int j = 0; + + for (int i = 0; i < Num_Particles; i++) { + float u = u1 + i * overN; + + while (j < Num_Particles - 1 && u > cumulativeSum[j]) { + j++; + } + + resampledParticles[i] = new Particle( + particles[j].x, particles[j].y, uniformWeighting); + } + + // Replace particle array with resampled particles + System.arraycopy(resampledParticles, 0, particles, 0, Num_Particles); + + // Debugging terminal output line + Log.d("ParticleFilter", "Resampling complete: all weights reset to " + uniformWeighting); + } + + /** + * Converts a WGS84 LatLng to a local easting/northing offset in meters + * relative to the local frame origin (originLatLng). + */ + public float[] latLngToLocal(LatLng point) { + if (originLatLng == null) return new float[]{0f, 0f}; + + double latDiff = point.latitude - originLatLng.latitude; + double lonDiff = point.longitude - originLatLng.longitude; + + float northing = (float) (latDiff * Meters_Per_Degree); + float easting = (float) (lonDiff * Meters_Per_Degree + * Math.cos(Math.toRadians(originLatLng.latitude))); + + return new float[]{easting, northing}; + } + + /** + * Converts a local easting/northing offset in meters back to WGS84. + */ + public LatLng localToLatLng(float eastingM, float northingM) { + if (originLatLng == null) return null; + + double lat = originLatLng.latitude + + northingM / Meters_Per_Degree; + double lon = originLatLng.longitude + + eastingM / (Meters_Per_Degree + * Math.cos(Math.toRadians(originLatLng.latitude))); + + return new LatLng(lat, lon); + } + + /** + * State accessors: + */ + + public boolean isInitialised() { + return initialised; + } + + public LatLng getEstimatedPosition() { + if (!initialised) return null; + return localToLatLng(estimatedX, estimatedY); + } + + public float[] getEstimatedLocalPosition() { + return new float[]{estimatedX, estimatedY}; + } + + public LatLng getOrigin() { + return originLatLng; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 639fc5c2..bbe65bb7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -29,6 +29,7 @@ public class SensorEventHandler { private final PdrProcessing pdrProcessing; private final PathView pathView; private final TrajectoryRecorder recorder; + private final ParticleFilter particleFilter; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); @@ -39,6 +40,10 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + // Previous PDR position for displacement deltas computation + private float previousPDRX = 0f; + private float previousPDRY = 0f; + /** * Creates a new SensorEventHandler. * @@ -46,15 +51,17 @@ public class SensorEventHandler { * @param pdrProcessing PDR processor for step-length and position calculation * @param pathView path drawing view for trajectory visualisation * @param recorder trajectory recorder for checking recording state and writing PDR data + * @param particleFilter particle filter sensor fusion algorithm * @param bootTime initial boot time offset */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + ParticleFilter particleFilter, long bootTime) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; + this.particleFilter = particleFilter; this.bootTime = bootTime; } @@ -174,6 +181,15 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude.clear(); if (recorder.isRecording()) { + // Compute PDR displacement and shift particles + float deltaX = newCords[0] - previousPDRX; + float deltaY = newCords[1] - previousPDRY; + particleFilter.predict(deltaX, deltaY, ParticleFilter.PDR_Process_Noise_STD); + + // Update previous PDR position for next step + previousPDRX = newCords[0]; + previousPDRY = newCords[1]; + this.pathView.drawTrajectory(newCords); state.stepCounter++; recorder.addPdrData( diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index aeb6386a..2acbc092 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -11,6 +11,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; import androidx.annotation.NonNull; @@ -33,19 +34,11 @@ import java.util.stream.Stream; /** - * The SensorFusion class is the main data gathering and processing class of the application. + * Main data gathering and processing class for the application. Singleton - all fragments + * share the same instance via SensorFusion.getInstance(). * - *

It follows the singleton design pattern to ensure that every fragment and process has access - * to the same data and sensor instances. Internally it delegates to specialised modules:

- *
    - *
  • {@link SensorState} – shared sensor data holder
    • - *
  • {@link SensorEventHandler} – sensor event dispatch (switch logic)
    • - *
  • {@link TrajectoryRecorder} – recording lifecycle & protobuf construction
    • - *
  • {@link WifiPositionManager} – WiFi scan processing & positioning
    • - *
- * - *

The public API is unchanged – all external callers continue to use - * {@code SensorFusion.getInstance().method()}.

+ * Delegates to: SensorState (data holder), SensorEventHandler (sensor dispatch), + * TrajectoryRecorder (recording and protobuf), WifiPositionManager (WiFi positioning). */ public class SensorFusion implements SensorEventListener { @@ -63,6 +56,8 @@ public class SensorFusion implements SensorEventListener { private SensorEventHandler eventHandler; private TrajectoryRecorder recorder; private WifiPositionManager wifiPositionManager; + private ParticleFilter particleFilter; + private MapMatcher mapMatcher; // Movement sensor instances (lifecycle managed here) private MovementSensor accelerometerSensor; @@ -94,6 +89,14 @@ public class SensorFusion implements SensorEventListener { // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); + + // Max distances from a staircase / lift for a barometer floor change to be accepted + private static final float STAIR_PROXIMITY_THRESHOLD_M = 5.0f; + private static final float LIFT_PROXIMITY_THRESHOLD_M = 50.0f; + // Elevation recorded when the current floor was last confirmed; NaN before first barometer reading + private float lastFloorElevation = Float.NaN; + // WiFi floor number at the last confirmed floor level; MIN_VALUE until first WiFi fix + private int lastAcceptedWifiFloor = Integer.MIN_VALUE; //endregion //region Initialisation @@ -115,17 +118,10 @@ public static SensorFusion getInstance() { } /** - * Initialisation function for the SensorFusion instance. - * - *

Initialises all movement sensor instances, creates internal modules, and prepares - * the system for data collection.

+ * Sets up all sensors, internal modules, and data processors. + * Must be called once with an application context before anything else. * - * @param context application context for permissions and device access. - * - * @see MovementSensor handling all SensorManager based data collection devices. - * @see ServerCommunications handling communication with the server. - * @see GNSSDataProcessor for location data processing. - * @see WifiDataProcessor for network data processing. + * @param context application context */ public void setContext(Context context) { this.appContext = context.getApplicationContext(); @@ -150,7 +146,7 @@ public void setContext(Context context) { SharedPreferences settings = PreferenceManager.getDefaultSharedPreferences(context); this.pdrProcessing = new PdrProcessing(context); this.pathView = new PathView(context, null); - WiFiPositioning wiFiPositioning = new WiFiPositioning(context); + WiFiPositioning wiFiPositioning = new WiFiPositioning(context, null); // Create internal modules this.recorder = new TrajectoryRecorder(appContext, state, serverCommunications, settings); @@ -160,9 +156,19 @@ public void setContext(Context context) { this.wifiPositionManager = new WifiPositionManager(wiFiPositioning, recorder); + // Initialise particle filter + this.particleFilter = new ParticleFilter(); + + // Create the map matcher and give the particle filter a reference to it. + // MapMatcher handles floor detection and stair/lift lookups to help determine whether near stairs or lift. + this.mapMatcher = new MapMatcher(this); + particleFilter.setMapMatcher(mapMatcher); + + wiFiPositioning.setParticleFilter(particleFilter); + long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, particleFilter, bootTime); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -200,16 +206,19 @@ public void update(Object[] objList) { //region SensorEventListener /** - * {@inheritDoc} - * - *

Delegates to {@link SensorEventHandler#handleSensorEvent(SensorEvent)}.

+ * Forwards sensor events to SensorEventHandler for processing. + * On each barometer reading while recording, also checks whether enough elevation + * change has accumulated to trigger a floor step. */ @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); + // Check for floor changes on every pressure update + if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { + checkAndApplyFloorChange(state.elevation); + } } - /** {@inheritDoc} */ @Override public void onAccuracyChanged(Sensor sensor, int i) {} @@ -218,9 +227,7 @@ public void onAccuracyChanged(Sensor sensor, int i) {} //region Start/Stop listening /** - * Registers all device listeners and enables updates with the specified sampling rate. - * - *

Should be called from {@link MainActivity} when resuming the application.

+ * Registers all device listeners. Called from MainActivity on resume. */ public void resumeListening() { accelerometerSensor.sensorManager.registerListener(this, @@ -251,9 +258,8 @@ public void resumeListening() { } /** - * Un-registers all device listeners and pauses data collection. - * - *

Should be called from {@link MainActivity} when pausing the application.

+ * Unregisters all device listeners. Called from MainActivity on pause. + * Does nothing while a recording is in progress. */ public void stopListening() { if (!recorder.isRecording()) { @@ -328,6 +334,13 @@ private void stopWirelessCollectors() { public void startRecording() { recorder.startRecording(pdrProcessing); eventHandler.resetBootTime(recorder.getBootTime()); + particleFilter.tryInitialise(); + // Reset the floor-change baselines for the new recording session + lastFloorElevation = Float.NaN; + lastAcceptedWifiFloor = Integer.MIN_VALUE; + + // Attempt to load the building map now that the particle filter is ready + tryTriggerMapMatcher(getSelectedBuildingId()); // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -422,6 +435,9 @@ public void setFloorplanBuildings(List building } floorplanBuildingCache.put(building.getName(), building); } + + // Cache is now populated - try to load the building map + tryTriggerMapMatcher(getSelectedBuildingId()); } /** @@ -446,6 +462,22 @@ public List getFloorplanBuildings() { return new ArrayList<>(floorplanBuildingCache.values()); } + /** + * Loads the building map into MapMatcher once the particle filter is ready, + * the origin is set, and the floorplan cache is populated. Called from both + * startRecording() and setFloorplanBuildings() since either can arrive first. + * + * @param preferredBuildingId building name hint, or null to auto-detect + */ + private void tryTriggerMapMatcher(String preferredBuildingId) { + if (mapMatcher == null) return; + if (!particleFilter.isInitialised()) return; + if (particleFilter.getOrigin() == null) return; + if (floorplanBuildingCache.isEmpty()) return; + mapMatcher.tryLoadBuilding(preferredBuildingId, particleFilter.getOrigin()); + Log.d("Debug", "MapMatcher ready: " + mapMatcher.isInitialised()); + } + /** * Writes the initial position and heading into the trajectory protobuf. * Should be called after startRecording() and setStartGNSSLatitude(). @@ -614,6 +646,33 @@ public LatLng getLatLngWifiPositioning() { return wifiPositionManager.getLatLngWifiPositioning(); } + /** + * Returns the best estimated position of the user from the particle filter. + * Returns {@code null} if the filter has not yet initialised, in which case + * the caller should fall back to the PDR-derived absolute position. + * + * @return particle filter position estimate, or {@code null} if not yet initialised. + */ + public LatLng getFusedPosition() { + if (particleFilter != null && particleFilter.isInitialised()) { + return particleFilter.getEstimatedPosition(); + } + return null; + } + + /** + * Feeds a WiFi position fix into the particle filter as a correction measurement. + * WiFi is more reliable indoors than GNSS, so this prevents PDR drift accumulating + * in environments where GNSS accuracy is too poor to pass the filter's threshold. + * + * @param wifiLatLng WiFi-derived position from the OpenPositioning API. + */ + public void correctWithWifiPosition(LatLng wifiLatLng) { + if (particleFilter != null && particleFilter.isInitialised() && wifiLatLng != null) { + particleFilter.updateWeights(wifiLatLng, 8.0f); + } + } + /** * Returns the current floor the user is on, obtained using WiFi positioning. * @@ -630,6 +689,137 @@ public void logSensorFrequencies() { eventHandler.logSensorFrequencies(); } + /** + * Returns the current floor as a WiFi-scale integer (0=GF, 1=F1, -1=LG etc.) + * for use by the floor switch in TrajectoryMapFragment. + * + * When MapMatcher is active, getLikelyFloorIndex() returns an API index that + * already has the building bias baked in. We subtract it here so the caller's + * setCurrentFloor(x, autoFloor=true) can add it back without double-counting. + * Falls back to the raw WiFi floor if MapMatcher is not loaded yet. + */ + public int getMapMatcherFloor() { + if (mapMatcher != null && mapMatcher.isInitialised()) { + return mapMatcher.getLikelyFloorIndex() - mapMatcher.getAutoFloorBias(); + } + return getWifiFloor(); + } + + //endregion + + //region Floor change gating + + /** + * Called on every barometer reading while recording. + * + * Rule 1: if WiFi reports a new floor since last check, snap the map to match immediately if near stairs or lift. + * Rule 2: if accumulated elevation change exceeds one floor height (per building), step + * the floor up or down. Only fires when near a staircase or lift. Uses + * state.elevator to classify the transition as lift or stairs. + */ + private void checkAndApplyFloorChange(float elevation) { + if (mapMatcher == null || !mapMatcher.isInitialised()) return; + + // Seed reference on first call + if (Float.isNaN(lastFloorElevation)) { + lastFloorElevation = elevation; + lastAcceptedWifiFloor = getWifiFloor(); + return; + } + + int currentWifiFloor = getWifiFloor(); + + // Rule 1: WiFi floor changed - snap the map to match and reset the elevation baseline + if (lastAcceptedWifiFloor != Integer.MIN_VALUE + && currentWifiFloor != lastAcceptedWifiFloor) { + int wifiApiFloor = mapMatcher.physicalFloorToApiIndex(currentWifiFloor); + if (wifiApiFloor >= 0) { + mapMatcher.setCurrentFloor(wifiApiFloor); + Log.i("SensorFusion", "Floor snapped to WiFi floor " + wifiApiFloor + + " (physical=" + currentWifiFloor + ")"); + } else { + mapMatcher.setCurrentFloor(-1); // unknown floor - clear override and let WiFi drive + } + lastFloorElevation = elevation; + lastAcceptedWifiFloor = currentWifiFloor; + return; + } + + // Rule 2: Barometer - checks accumulated elevation change against the building's floor + // height. Only accepts a floor change if the user is physically near a staircase or + // lift from the map. Floor changes in the middle of a room are ignored. + float floorHeight = mapMatcher.getFloorHeight(); + float elevationDelta = elevation - lastFloorElevation; + int floorsToMove = (int) (Math.abs(elevationDelta) / floorHeight); + if (floorsToMove == 0) return; + + // Only proceed if the user is physically near a staircase or lift + if (particleFilter == null || !particleFilter.isInitialised()) return; + int currentApiFloor = mapMatcher.getLikelyFloorIndex(); + float[] pos = particleFilter.getEstimatedLocalPosition(); + boolean nearLift = isNearAny(pos, + mapMatcher.getLiftCentersForFloor(currentApiFloor), LIFT_PROXIMITY_THRESHOLD_M); + boolean nearStairs = isNearAny(pos, + mapMatcher.getStairCentersForFloor(currentApiFloor), STAIR_PROXIMITY_THRESHOLD_M); + if (!nearLift && !nearStairs) { + Log.d("SensorFusion", "Barometer floor change skipped: not near a lift or stairs"); + return; + } + // Uses state.elevator (the PDR movement model) together with proximity to decide + // whether the user is in a lift or on stairs. state.elevator is true when vertical + // acceleration dominates and step activity is low - the lift pattern. + boolean movementModelSaysLift = state.elevator; + String transitionType; + if (nearLift && movementModelSaysLift) { + transitionType = "lift"; + } else if (nearStairs && !movementModelSaysLift) { + transitionType = "stairs"; + } else { + // Fallback: proximity wins when movement model and geometry disagree + transitionType = nearLift ? "lift" : "stairs"; + } + + int direction = elevationDelta > 0 ? 1 : -1; + int newApiFloor = currentApiFloor; + for (int i = 0; i < floorsToMove; i++) { + int next = mapMatcher.getAdjacentFloorIndex(newApiFloor, direction); + if (next == newApiFloor) break; // already at the top or bottom - stop + newApiFloor = next; + } + + if (newApiFloor != currentApiFloor) { + mapMatcher.setCurrentFloor(newApiFloor); + // Advance reference by the consumed distance, preserving the remainder + lastFloorElevation += floorsToMove * floorHeight * direction; + Log.i("SensorFusion", "Floor changed to " + newApiFloor + + " via " + transitionType + " (barometer, " + floorsToMove + + " floor(s), delta=" + elevationDelta + "m)"); + } + } + + /** + * Returns true if the estimated position is near a staircase or lift on the current floor. + * Called by TrajectoryMapFragment before applying an auto floor change. + */ + public boolean isNearTransition() { + if (mapMatcher == null || !mapMatcher.isInitialised()) return false; + if (particleFilter == null || !particleFilter.isInitialised()) return false; + int floor = mapMatcher.getLikelyFloorIndex(); + float[] pos = particleFilter.getEstimatedLocalPosition(); + return isNearAny(pos, mapMatcher.getStairCentersForFloor(floor), STAIR_PROXIMITY_THRESHOLD_M) + || isNearAny(pos, mapMatcher.getLiftCentersForFloor(floor), LIFT_PROXIMITY_THRESHOLD_M); + } + + /** Returns true if pos is within threshold meters of any centre in the list. */ + private boolean isNearAny(float[] pos, List centers, float threshold) { + if (centers == null || pos == null) return false; + for (float[] c : centers) { + float dx = pos[0] - c[0], dy = pos[1] - c[1]; + if (Math.sqrt(dx * dx + dy * dy) <= threshold) return true; + } + return false; + } + //endregion //region Location listener @@ -645,8 +835,15 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + + // Update particle weights with GNSS measurement + if (particleFilter.isInitialised()) { + LatLng gnssLatLng = new LatLng(location.getLatitude(), location.getLongitude()); + float accuracy = location.getAccuracy(); + particleFilter.updateWeights(gnssLatLng, accuracy); + } } } //endregion -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index dbf809dd..baceea8d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -54,6 +54,7 @@ public int getFloor() { // Store current floor of user, default value 0 (ground floor) private int floor=0; + private ParticleFilter particleFilter; /** * Constructor to create the WiFi positioning object @@ -62,9 +63,14 @@ public int getFloor() { * * @param context Context of object calling */ - public WiFiPositioning(Context context){ + public WiFiPositioning(Context context, ParticleFilter particleFilter){ // Initialising the Request queue this.requestQueue = Volley.newRequestQueue(context.getApplicationContext()); + this.particleFilter = particleFilter; + } + + public void setParticleFilter(ParticleFilter particleFilter) { + this.particleFilter = particleFilter; } /** @@ -89,6 +95,14 @@ public void request(JSONObject jsonWifiFeatures) { try { wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + + //Update particle filter weights with WiFi + if (particleFilter != null && particleFilter.isInitialised()) { + // Update particle filter weights with WiFi measurement + Log.d("ParticleFilter", "WiFi weight update: lat=" + wifiLocation.latitude + + ", lon=" + wifiLocation.longitude); + particleFilter.updateWeights(wifiLocation, 8.0f); + } } catch (JSONException e) { // Error log to keep record of errors (for secure programming and maintainability) Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); @@ -140,6 +154,15 @@ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) Log.d("jsonObject",response.toString()); wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + + //Update particle filter weights with WiFi + if (particleFilter != null && particleFilter.isInitialised()) { + // Update particle filter weights with WiFi measurement + Log.d("ParticleFilter", "WiFi weight update: lat=" + wifiLocation.latitude + + ", lon=" + wifiLocation.longitude); + particleFilter.updateWeights(wifiLocation, 8.0f); + } + callback.onSuccess(wifiLocation,floor); } catch (JSONException e) { Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 9765b044..0784b9ce 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -142,7 +142,7 @@ public PdrProcessing(Context context) { public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { return new float[]{this.positionX, this.positionY}; // Return current position without update - // - TODO - temporary solution of the empty list issue + // - TODO - temporary solution of the empty list issue } // Change angle so zero rad is east @@ -153,7 +153,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim // return current position, do not update return new float[]{this.positionX, this.positionY}; } - + // Calculate step length if(!useManualStep) { //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); @@ -308,12 +308,12 @@ public boolean estimateElevator(float[] gravity, float[] acc) { // get horizontal and vertical acceleration magnitude float verticalAcc = (float) Math.sqrt( Math.pow((acc[0] * gravity[0]/g),2) + - Math.pow((acc[1] * gravity[1]/g), 2) + - Math.pow((acc[2] * gravity[2]/g), 2)); + Math.pow((acc[1] * gravity[1]/g), 2) + + Math.pow((acc[2] * gravity[2]/g), 2)); float horizontalAcc = (float) Math.sqrt( Math.pow((acc[0] * (1 - gravity[0]/g)), 2) + - Math.pow((acc[1] * (1 - gravity[1]/g)), 2) + - Math.pow((acc[2] * (1 - gravity[2]/g)), 2)); + Math.pow((acc[1] * (1 - gravity[1]/g)), 2) + + Math.pow((acc[2] * (1 - gravity[2]/g)), 2)); // Save into buffer to compare with past values this.verticalAccel.putNewest(verticalAcc); this.horizontalAccel.putNewest(horizontalAcc); diff --git a/app/src/main/res/drawable/legend_dot.xml b/app/src/main/res/drawable/legend_dot.xml new file mode 100644 index 00000000..bb6f7dec --- /dev/null +++ b/app/src/main/res/drawable/legend_dot.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..78625d59 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -31,6 +31,22 @@ android:padding="8dp" android:gravity="center"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Floor Up button Choose Map ❇️ Auto Floor + Smooth + PDR Path + GNSS fix + WiFi fix + PDR step + Fused path GNSS error: Satellite Normal diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/secrets.properties b/secrets.properties deleted file mode 100644 index f0dc54fd..00000000 --- a/secrets.properties +++ /dev/null @@ -1,6 +0,0 @@ -# -# Modify the variables to set your keys -# -MAPS_API_KEY= -OPENPOSITIONING_API_KEY= -OPENPOSITIONING_MASTER_KEY=