diff --git a/.gitignore b/.gitignore index d4c3a57e..134bb5cd 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,4 @@ .cxx local.properties /.idea/ +.claude/settings.json diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java index 2d2b1cbf..22112992 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java @@ -84,7 +84,6 @@ public ReplayPoint(LatLng pdrLocation, LatLng gnssLocation, float orientation, f } } - /** Represents an IMU (Inertial Measurement Unit) data record used for orientation calculations. */ private static class ImuRecord { public long relativeTimestamp; public float accX, accY, accZ; // Accelerometer values @@ -92,13 +91,11 @@ private static class ImuRecord { public float rotationVectorX, rotationVectorY, rotationVectorZ, rotationVectorW; // Rotation quaternion } - /** Represents a Pedestrian Dead Reckoning (PDR) data record storing position shifts over time. */ private static class PdrRecord { public long relativeTimestamp; public float x, y; // Position relative to the starting point } - /** Represents a GNSS (Global Navigation Satellite System) data record with latitude/longitude. */ private static class GnssRecord { public long relativeTimestamp; public double latitude, longitude; // GNSS coordinates @@ -199,7 +196,6 @@ public static List parseTrajectoryData(String filePath, Context con return result; } -/** Parses IMU data from JSON. */ private static List parseImuData(JsonArray imuArray) { List imuList = new ArrayList<>(); if (imuArray == null) return imuList; @@ -209,7 +205,7 @@ private static List parseImuData(JsonArray imuArray) { imuList.add(record); } return imuList; -}/** Parses PDR data from JSON. */ +} private static List parsePdrData(JsonArray pdrArray) { List pdrList = new ArrayList<>(); if (pdrArray == null) return pdrList; @@ -219,7 +215,7 @@ private static List parsePdrData(JsonArray pdrArray) { pdrList.add(record); } return pdrList; -}/** Parses GNSS data from JSON. */ +} private static List parseGnssData(JsonArray gnssArray) { List gnssList = new ArrayList<>(); if (gnssArray == null) return gnssList; @@ -229,17 +225,17 @@ private static List parseGnssData(JsonArray gnssArray) { gnssList.add(record); } return gnssList; -}/** Finds the closest IMU record to the given timestamp. */ +} private static ImuRecord findClosestImuRecord(List imuList, long targetTimestamp) { return imuList.stream().min(Comparator.comparingLong(imu -> Math.abs(imu.relativeTimestamp - targetTimestamp))) .orElse(null); -}/** Finds the closest GNSS record to the given timestamp. */ +} private static GnssRecord findClosestGnssRecord(List gnssList, long targetTimestamp) { return gnssList.stream().min(Comparator.comparingLong(gnss -> Math.abs(gnss.relativeTimestamp - targetTimestamp))) .orElse(null); -}/** Computes the orientation from a rotation vector. */ +} private static float computeOrientationFromRotationVector(float rx, float ry, float rz, float rw, Context context) { float[] rotationVector = new float[]{rx, ry, rz, rw}; float[] rotationMatrix = new float[9]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java index cadb6037..9b6c27de 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java @@ -51,6 +51,9 @@ public static class MapShapeFeature { private final String indoorType; private final String geometryType; private final List> parts; + private final String strokeColor; // hex string from API, e.g. "#666666", or null + private final String fillColor; // hex string from API, or null + private final float strokeWidth; // 0 means not specified by API /** * Constructs a MapShapeFeature. @@ -58,12 +61,19 @@ public static class MapShapeFeature { * @param indoorType feature type from properties (e.g. "wall", "room") * @param geometryType GeoJSON geometry type (e.g. "MultiLineString", "MultiPolygon") * @param parts coordinate lists: each inner list is a line or polygon ring + * @param strokeColor stroke colour from API properties, or null + * @param fillColor fill colour from API properties, or null + * @param strokeWidth stroke width from API properties, 0 if not set */ public MapShapeFeature(String indoorType, String geometryType, - List> parts) { + List> parts, + String strokeColor, String fillColor, float strokeWidth) { this.indoorType = indoorType; this.geometryType = geometryType; this.parts = parts; + this.strokeColor = strokeColor; + this.fillColor = fillColor; + this.strokeWidth = strokeWidth; } /** Returns the indoor feature type (e.g. "wall", "room"). */ @@ -74,6 +84,15 @@ public MapShapeFeature(String indoorType, String geometryType, /** Returns coordinate parts: lines for MultiLineString, rings for MultiPolygon. */ public List> getParts() { return parts; } + + /** Returns the API-provided stroke colour string, or null if not set. */ + public String getStrokeColor() { return strokeColor; } + + /** Returns the API-provided fill colour string, or null if not set. */ + public String getFillColor() { return fillColor; } + + /** Returns the API-provided stroke width, or 0 if not set. */ + public float getStrokeWidth() { return strokeWidth; } } /** @@ -444,7 +463,27 @@ private MapShapeFeature parseMapShapeFeature(JSONObject feature) { return null; } - return new MapShapeFeature(indoorType, geoType, parts); + // Parse visual properties — try the same field names the private repo uses + String strokeColor = null; + String fillColor = null; + float strokeWidth = 0f; + if (properties != null) { + // stroke colour: stroke_color → line_color → color + if (properties.has("stroke_color")) strokeColor = properties.optString("stroke_color", null); + else if (properties.has("line_color")) strokeColor = properties.optString("line_color", null); + else if (properties.has("color")) strokeColor = properties.optString("color", null); + + // fill colour: fill_color → fill + if (properties.has("fill_color")) fillColor = properties.optString("fill_color", null); + else if (properties.has("fill")) fillColor = properties.optString("fill", null); + + // stroke width: stroke_width → line_width → width + if (properties.has("stroke_width")) strokeWidth = (float) properties.optDouble("stroke_width", 0.0); + else if (properties.has("line_width")) strokeWidth = (float) properties.optDouble("line_width", 0.0); + else if (properties.has("width")) strokeWidth = (float) properties.optDouble("width", 0.0); + } + + return new MapShapeFeature(indoorType, geoType, parts, strokeColor, fillColor, strokeWidth); } catch (JSONException e) { Log.e(TAG, "Failed to parse map_shapes feature", e); return null; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java index 9497848e..0ca473bf 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java @@ -13,36 +13,20 @@ import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.service.SensorCollectionService; -import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; import com.openpositioning.PositionMe.presentation.fragment.RecordingFragment; import com.openpositioning.PositionMe.presentation.fragment.CorrectionFragment; /** - * The RecordingActivity manages the recording flow of the application, guiding the user through a sequence - * of screens for location selection, recording, and correction before finalizing the process. - *

- * This activity follows a structured workflow: - *

    - *
  1. StartLocationFragment - Allows users to select their starting location.
    2. - *
  2. RecordingFragment - Handles the recording process and contains a TrajectoryMapFragment.
    3. - *
  3. CorrectionFragment - Enables users to review and correct recorded data before completion.
    4. - *
- *

- * The activity ensures that the screen remains on during the recording process to prevent interruptions. - * It also provides fragment transactions for seamless navigation between different stages of the workflow. - *

- * This class is referenced in various fragments such as HomeFragment, StartLocationFragment, - * RecordingFragment, and CorrectionFragment to control navigation through the recording flow. + * Manages the recording flow: name dialog → live recording → correction. + * The start location is anchored automatically from the first GPS fix; + * no manual pin-drop screen is required. * - * @see StartLocationFragment The first step in the recording process where users select their starting location. - * @see RecordingFragment Handles data recording and map visualization. - * @see CorrectionFragment Allows users to review and make corrections before finalizing the process. - * @see com.openpositioning.PositionMe.R.layout#activity_recording The associated layout for this activity. + * @see RecordingFragment live map and sensor data during walking. + * @see CorrectionFragment review/correct before upload. * * @author ShuGu */ - public class RecordingActivity extends AppCompatActivity { @Override @@ -51,32 +35,18 @@ protected void onCreate(@Nullable Bundle savedInstanceState) { setContentView(R.layout.activity_recording); if (savedInstanceState == null) { - // Show trajectory name input dialog before proceeding to start location showTrajectoryNameDialog(); } - // Keep screen on getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); } - /** - * {@inheritDoc} - * Re-registers sensor listeners so that IMU, step detection, barometer and other - * movement sensors remain active while this activity is in the foreground. - * Without this, sensors are unregistered when {@link MainActivity#onPause()} fires - * during the activity transition, leaving PDR and elevation updates dead. - */ @Override protected void onResume() { super.onResume(); SensorFusion.getInstance().resumeListening(); } - /** - * {@inheritDoc} - * Stops sensor listeners when this activity is no longer visible, unless - * the foreground {@link SensorCollectionService} is running (recording in progress). - */ @Override protected void onPause() { super.onPause(); @@ -86,9 +56,8 @@ protected void onPause() { } /** - * Shows an AlertDialog prompting the user to enter a trajectory name. - * The name is stored in SensorFusion as trajectory_id and later written to the protobuf. - * After input, proceeds to StartLocationFragment. + * Prompts for a trajectory name, then starts recording immediately. + * The GPS anchor is written automatically on the first location fix. */ private void showTrajectoryNameDialog() { EditText input = new EditText(this); @@ -101,36 +70,24 @@ private void showTrajectoryNameDialog() { .setMessage("Enter a name for this recording session:") .setView(input) .setCancelable(false) - .setPositiveButton("Save", (dialog, which) -> { + .setPositiveButton("Start", (dialog, which) -> { String name = input.getText().toString().trim(); - if (name.isEmpty()) { - // Default name based on timestamp - name = "traj_" + System.currentTimeMillis(); - } - SensorFusion.getInstance().setTrajectoryId(name); - showStartLocationScreen(); + if (name.isEmpty()) name = "traj_" + System.currentTimeMillis(); + SensorFusion sf = SensorFusion.getInstance(); + sf.setTrajectoryId(name); + sf.startRecording(); + showRecordingScreen(); }) .setNegativeButton("Skip", (dialog, which) -> { - // Use default name - SensorFusion.getInstance().setTrajectoryId( - "traj_" + System.currentTimeMillis()); - showStartLocationScreen(); + SensorFusion sf = SensorFusion.getInstance(); + sf.setTrajectoryId("traj_" + System.currentTimeMillis()); + sf.startRecording(); + showRecordingScreen(); }) .show(); } - /** - * Show the StartLocationFragment (beginning of flow). - */ - public void showStartLocationScreen() { - FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); - ft.replace(R.id.mainFragmentContainer, new StartLocationFragment()); - ft.commit(); - } - - /** - * Show the RecordingFragment, which contains the TrajectoryMapFragment internally. - */ + /** Show the RecordingFragment (live map + sensors). */ public void showRecordingScreen() { FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); ft.replace(R.id.mainFragmentContainer, new RecordingFragment()); @@ -138,9 +95,7 @@ public void showRecordingScreen() { ft.commit(); } - /** - * Show the CorrectionFragment after the user stops recording. - */ + /** Show the CorrectionFragment after the user stops recording. */ public void showCorrectionScreen() { FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); ft.replace(R.id.mainFragmentContainer, new CorrectionFragment()); @@ -148,9 +103,7 @@ public void showCorrectionScreen() { ft.commit(); } - /** - * Finish the Activity (or do any final steps) once corrections are done. - */ + /** Finish the Activity once corrections are done. */ public void finishFlow() { getWindow().clearFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); finish(); 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..37f3d373 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 @@ -7,6 +7,7 @@ import android.os.Bundle; import android.os.CountDownTimer; import android.os.Handler; +import android.util.Log; import android.view.LayoutInflater; import android.view.View; import android.view.ViewGroup; @@ -87,12 +88,31 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; + // Minimum displacement before the displayed point is moved. + // Keeps the marker locked while stationary; particle-filter noise is ~0.1-0.15 m + // per resample so 0.5 m gives comfortable headroom above the noise floor. + private static final double MOVEMENT_THRESHOLD_METERS = 0.5; + + // Maximum single-tick displacement accepted from the fused estimate. + // Jumps larger than this are treated as filter teleports: the PDR dead-reckoning + // fallback is used instead so the trajectory stays physically continuous. + private static final double MAX_JUMP_METERS = 8.0; + + // Last fused point that was actually rendered + private LatLng lastSentFusedPosition = null; + + // PDR {x, y} coordinates (meters from recording origin) at the last accepted render. + // Used to compute the PDR delta when the fused estimate teleports. + private float lastAcceptedPdrX = 0f; + private float lastAcceptedPdrY = 0f; + + private LatLng lastSentWifiPosition = null; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { updateUIandPosition(); - // Loop again - refreshDataHandler.postDelayed(refreshDataTask, 200); + refreshDataHandler.postDelayed(refreshDataTask, 16); //loop faster } }; @@ -228,7 +248,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; } @@ -252,6 +272,10 @@ private void onAddTestPoint() { * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. */ private void updateUIandPosition() { + // Elevation comes from the barometer so is available immediately, no PDR needed should work FINE + float elevationVal = sensorFusion.getElevation(); + elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); + float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); if (pdrValues == null) return; @@ -260,30 +284,86 @@ private void updateUIandPosition() { + Math.pow(pdrValues[1] - previousPosY, 2)); distanceTravelled.setText(getString(R.string.meter, String.format("%.2f", distance))); - // Elevation - float elevationVal = sensorFusion.getElevation(); - elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); + // // Current location + // // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon + // // Or simply pass relative data for the TrajectoryMapFragment to handle + // // For example: + // float[] latLngArray = sensorFusion.getGNSSLatitude(true); + // if (latLngArray != null) { + // LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally + // LatLng newLocation = UtilFunctions.calculateNewPos( + // oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, + // new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } + // ); + + // // Pass the location + orientation to the map + // if (trajectoryMapFragment != null) { + // trajectoryMapFragment.updateUserLocation(newLocation, + // (float) Math.toDegrees(sensorFusion.passOrientation())); + // } + // } + + // Get the latest fused position from SensorFusion (best estimate of user location) + LatLng fusedPosition = sensorFusion.getFusedPosition(); + + if (fusedPosition != null && trajectoryMapFragment != null) { + + boolean isFirstPoint = (lastSentFusedPosition == null); + double movedDistance = 0.0; + + // Determine if the fused position has moved enough to warrant a map update. + boolean movementDetected = false; + + if (lastSentFusedPosition != null) { + movedDistance = UtilFunctions.distanceBetweenPoints( + lastSentFusedPosition, fusedPosition); + movementDetected = movedDistance >= MOVEMENT_THRESHOLD_METERS; + } + + if (isFirstPoint || movementDetected) { // Only update the map if it's the first point or if the fused position has moved enough + LatLng positionToRender; + + if (!isFirstPoint && movedDistance > MAX_JUMP_METERS) { + // ---- TELEPORT DETECTED ---- + // The fused estimate jumped implausibly (bad GNSS/filter divergence). + // Fall back to PDR dead-reckoning: apply the PDR step delta to the + // last confirmed anchor so the trajectory stays physically continuous. + //i think this should work + float pdrDeltaX = pdrValues[0] - lastAcceptedPdrX; // Compute PDR step delta since last accepted point + float pdrDeltaY = pdrValues[1] - lastAcceptedPdrY; + double pdrStep = Math.sqrt(pdrDeltaX * pdrDeltaX + pdrDeltaY * pdrDeltaY); + + if (pdrStep >= MOVEMENT_THRESHOLD_METERS && pdrStep < MAX_JUMP_METERS) { + positionToRender = UtilFunctions.convertENUToWGS84( + lastSentFusedPosition, + new float[]{pdrDeltaX, pdrDeltaY, 0f}); + + } else { + // PDR also hasn't moved enough so stay put this tick + positionToRender = null; + Log.d("FUSED_TEST", "Teleport " + (int) movedDistance + + "m rejected, no PDR movement yet"); + } + } else { + // Normal update hence fused estimate is within plausible range + positionToRender = fusedPosition; + } - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally - LatLng newLocation = UtilFunctions.calculateNewPos( - oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); - - // Pass the location + orientation to the map - if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + if (positionToRender != null) { + boolean rendered = trajectoryMapFragment.updateUserLocation( + positionToRender, + (float) Math.toDegrees(sensorFusion.passOrientation())); + if (rendered) { + lastSentFusedPosition = positionToRender; + lastAcceptedPdrX = pdrValues[0]; + lastAcceptedPdrY = pdrValues[1]; + } + } } } - // GNSS logic if you want to show GNSS error, etc. + + float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { // If user toggles showing GNSS in the map, call e.g. @@ -302,6 +382,17 @@ private void updateUIandPosition() { } } + // WiFi observation logic for colour-coded last N updates + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + // Only add to history when the location has actually changed (new API response) + if (wifiLocation != null && !wifiLocation.equals(lastSentWifiPosition)) { + Log.d("WiFiDebug", "New WiFi fix: " + wifiLocation); + trajectoryMapFragment.updateWiFiObservation(wifiLocation); + lastSentWifiPosition = wifiLocation; + } + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index 0951e85a..06a480f9 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java @@ -149,24 +149,9 @@ private void setupMap() { startMarker = mMap.addMarker(new MarkerOptions() .position(position) .title("Start Position") - .draggable(true)); + .draggable(false)); mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(position, zoom)); - // Marker drag listener to update the start position when dragged - mMap.setOnMarkerDragListener(new GoogleMap.OnMarkerDragListener() { - @Override - public void onMarkerDragStart(Marker marker) {} - - @Override - public void onMarkerDragEnd(Marker marker) { - startPosition[0] = (float) marker.getPosition().latitude; - startPosition[1] = (float) marker.getPosition().longitude; - } - - @Override - public void onMarkerDrag(Marker marker) {} - }); - // Polygon click listener for building selection mMap.setOnPolygonClickListener(polygon -> { String buildingName = (String) polygon.getTag(); @@ -468,11 +453,12 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat } if (requireActivity() instanceof RecordingActivity) { - // Start sensor recording + set the start location + // Start sensor recording and seed initial position autonomously (GNSS/WiFi). sensorFusion.startRecording(); - sensorFusion.setStartGNSSLatitude(startPosition); - // Write trajectory_id, initial_position and initial heading to protobuf - sensorFusion.writeInitialMetadata(); + // If no fix is available yet, listeners will populate initial metadata later. + if (sensorFusion.initializeStartPositionAutonomously()) { + sensorFusion.writeInitialMetadata(); + } // Switch to the recording screen ((RecordingActivity) requireActivity()).showRecordingScreen(); 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..f5e4b831 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 @@ -22,16 +22,21 @@ import com.google.android.gms.maps.OnMapReadyCallback; import com.openpositioning.PositionMe.R; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.sensors.SensorFusion; +import com.openpositioning.PositionMe.utils.MapMatchingConfig; import com.openpositioning.PositionMe.utils.IndoorMapManager; +import com.openpositioning.PositionMe.utils.CrossFloorClassifier; +import com.openpositioning.PositionMe.utils.WallGeometryBuilder; import com.openpositioning.PositionMe.utils.UtilFunctions; +import android.graphics.PointF; +import java.util.List; import com.google.android.gms.maps.CameraUpdateFactory; import com.google.android.gms.maps.GoogleMap; import com.google.android.gms.maps.SupportMapFragment; import com.google.android.gms.maps.model.*; import java.util.ArrayList; -import java.util.List; /** @@ -66,7 +71,10 @@ public class TrajectoryMapFragment extends Fragment { private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red - private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private boolean isGnssOn = true; // Tracks if GNSS tracking is enabled (matches default android:checked="true") + + // Accuracy circle drawn around the position marker (radius = particle-cloud RMS spread) + private Circle accuracyCircle; private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location @@ -77,26 +85,115 @@ public class TrajectoryMapFragment extends Fragment { private IndoorMapManager indoorMapManager; // Manages indoor mapping private SensorFusion sensorFusion; + // Floorplan live-fetch state + private FloorplanApiClient floorplanApiClient = new FloorplanApiClient(); + private boolean floorplanFetchAttempted = false; + private boolean wasIndoorMapSet = false; + // Auto-floor state private static final String TAG = "TrajectoryMapFragment"; private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000; private static final long AUTO_FLOOR_CHECK_INTERVAL_MS = 1000; + private static final int FLOOR_TRANSITION_ZONE = Integer.MIN_VALUE; + private static final double MIN_POLYLINE_DISTANCE_M = 0.5; private Handler autoFloorHandler; private Runnable autoFloorTask; private int lastCandidateFloor = Integer.MIN_VALUE; private long lastCandidateTime = 0; + private final MapMatchingConfig mapMatchingConfig = new MapMatchingConfig(); + private LatLng wallOrigin; + // Tracks the particle-filter origin that was used for the last wall build. + // Walls must be rebuilt whenever the PF origin changes so both share the same ENU frame. + private LatLng lastWallBuildOrigin = null; + + // The last position that was ACTUALLY added to the polyline. + // Used as the "from" point in the wall-crossing check so that the anchor never + // advances through a wall: if a segment is rejected, the anchor stays at the last + // good point and the next segment is checked from there instead. + private LatLng lastPolylinePoint = null; + // UI private Spinner switchMapSpinner; private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothingSwitch; + private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + private Polygon nkmlPolygon; + private Polygon fjbPolygon; + private Polygon faradayPolygon; + + // --- Last N observation display state --- + private static final int MAX_OBSERVATIONS = 20; + + // Rolling histories of absolute position updates + private final List gnssHistory = new ArrayList<>(); + private final List wifiHistory = new ArrayList<>(); + private final List pdrHistory = new ArrayList<>(); + + // Rendered map circles for each source, so they can be removed/redrawn cleanly + private final List gnssCircles = new ArrayList<>(); + private final List wifiCircles = new ArrayList<>(); + private final List pdrCircles = new ArrayList<>(); + + // Optional UI switches for visibility control + private SwitchMaterial wifiSwitch; + private SwitchMaterial pdrSwitch; + + // --- Display smoothing state --- + + // Types of smoothing filters available for display + private enum SmoothingType { + RAW, // No smoothing + MOVING_AVERAGE, // Average over last N points + EXPONENTIAL // Exponential smoothing + } + private Spinner smoothingSpinner; + + // Current selected smoothing mode (default = RAW) + private SmoothingType smoothingType = SmoothingType.RAW; + + // For exponential smoothing + private LatLng smoothedDisplayLocation = null; + + // For moving average + private static final int SMOOTHING_WINDOW = 5; + private final List smoothingBuffer = new ArrayList<>(); + + // Exponential smoothing strength + private static final double ALPHA = 0.25; + + private com.google.android.material.button.MaterialButton settingsButton; + private View settingsContainer; + + + /** + * Adds a new position observation to a rolling history list. + * Maintains only the most recent MAX_OBSERVATIONS points. + * + * @param history The list storing past observations (GNSS, WiFi, or PDR) + * @param point The new LatLng position to add + */ + private void addObservation(List history, LatLng point) { + + // Ignore null points (e.g., when a sensor has no valid reading) + if (point == null) return; + // Add the new observation to the history + history.add(point); + + // If we exceed the maximum allowed observations, + // remove the oldest point (FIFO behaviour) + if (history.size() > MAX_OBSERVATIONS) { + history.remove(0); + } + } public TrajectoryMapFragment() { // Required empty public constructor @@ -119,12 +216,18 @@ public void onViewCreated(@NonNull View view, // Grab references to UI controls switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); + wifiSwitch = view.findViewById(R.id.wifiSwitch); + pdrSwitch = view.findViewById(R.id.pdrSwitch); + smoothingSpinner = view.findViewById(R.id.smoothingSpinner); autoFloorSwitch = view.findViewById(R.id.autoFloor); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); switchColorButton = view.findViewById(R.id.lineColorButton); + settingsButton = view.findViewById(R.id.settingsButton); + settingsContainer = view.findViewById(R.id.settingsContainer); + // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -149,13 +252,19 @@ public void onMapReady(@NonNull GoogleMap googleMap) { drawBuildingPolygon(); + // Draw green outlines around buildings that have indoor map data + if (indoorMapManager != null) { + indoorMapManager.setIndicationOfIndoorMap(); + } + Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!"); } }); } - + // Smoothing type spinner setup + initSmoothingSpinner(); // Map type spinner setup initMapTypeSpinner(); @@ -166,8 +275,18 @@ public void onMapReady(@NonNull GoogleMap googleMap) { gnssMarker.remove(); gnssMarker = null; } + redrawObservationOverlays(); + }); + + wifiSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + redrawObservationOverlays(); + }); + + pdrSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + redrawObservationOverlays(); }); + // Color switch switchColorButton.setOnClickListener(v -> { if (polyline != null) { @@ -180,6 +299,22 @@ public void onMapReady(@NonNull GoogleMap googleMap) { polyline.setColor(Color.RED); isRed = true; } + // Keep floorplan lines in sync with trajectory color + if (indoorMapManager != null) { + indoorMapManager.setFloorPlanColor(isRed ? Color.RED : Color.BLACK); + indoorMapManager.redrawCurrentFloor(); + } + } + }); + + // Setting Button + settingsButton.setOnClickListener(v -> { + if (settingsContainer.getVisibility() == View.VISIBLE) { + settingsContainer.setVisibility(View.GONE); + settingsButton.setText("Settings"); + } else { + settingsContainer.setVisibility(View.VISIBLE); + settingsButton.setText("Hide Settings"); } }); @@ -211,6 +346,77 @@ public void onMapReady(@NonNull GoogleMap googleMap) { }); } + /** + * Redraws the colour-coded observation circles for GNSS, WiFi, and PDR. + * Only sources enabled by their switches are displayed. + */ + private void redrawObservationOverlays() { + if (gMap == null) return; + + clearObservationCircles(); + + if (gnssSwitch != null && gnssSwitch.isChecked()) { + drawHistory(gnssHistory, gnssCircles, Color.BLUE); + } + + if (wifiSwitch != null && wifiSwitch.isChecked()) { + drawHistory(wifiHistory, wifiCircles, Color.GREEN); + } + + if (pdrSwitch != null && pdrSwitch.isChecked()) { + drawHistory(pdrHistory, pdrCircles, Color.RED); + } + } + + /** + * Draws one rolling history of observations on the map as circles. + * Older observations are faded, newer ones are more visible. + * + * @param history The observation points to render + * @param rendered The list of Circle references currently on the map + * @param color The base colour for this data source + */ + private void drawHistory(List history, List rendered, int color) { + for (int i = 0; i < history.size(); i++) { + LatLng point = history.get(i); + + // Fade oldest→newest but keep alpha high (150–255) so dots are always visible + int alpha = 150 + (int) (105f * (i + 1) / history.size()); + int fadedColor = Color.argb(alpha, Color.red(color), Color.green(color), Color.blue(color)); + + Circle circle = gMap.addCircle(new CircleOptions() + .center(point) + .radius(0.6) + .strokeWidth(0f) + .fillColor(fadedColor)); + + rendered.add(circle); + } + } + + /** + * Removes all currently displayed observation circles from the map. + */ + private void clearObservationCircles() { + removeAll(gnssCircles); + removeAll(wifiCircles); + removeAll(pdrCircles); + } + + /** + * Removes every circle in the provided list from the map and clears the list. + * + * @param circles The rendered circles to remove + */ + private void removeAll(List circles) { + for (Circle circle : circles) { + if (circle != null) { + circle.remove(); + } + } + circles.clear(); + } + /** * Initialize the map settings with the provided GoogleMap instance. *

@@ -266,40 +472,195 @@ private void initMapSettings(GoogleMap map) { */ private void initMapTypeSpinner() { if (switchMapSpinner == null) return; + String[] maps = new String[]{ - getString(R.string.hybrid), - getString(R.string.normal), - getString(R.string.satellite) + "Select Map Type", + "Hybrid", + "Normal", + "Satellite" }; - ArrayAdapter adapter = new ArrayAdapter<>( + + ArrayAdapter adapter = new ArrayAdapter( requireContext(), android.R.layout.simple_spinner_dropdown_item, maps - ); + ) { + @Override + public boolean isEnabled(int position) { + return position != 0; // disable hint item + } + }; + switchMapSpinner.setAdapter(adapter); + // Show hint initially + switchMapSpinner.setSelection(0); + switchMapSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { @Override public void onItemSelected(AdapterView parent, View view, int position, long id) { + if (gMap == null) return; - switch (position){ + + switch (position) { case 0: + return; // ignore hint + + case 1: gMap.setMapType(GoogleMap.MAP_TYPE_HYBRID); break; - case 1: + + case 2: gMap.setMapType(GoogleMap.MAP_TYPE_NORMAL); break; - case 2: + + case 3: gMap.setMapType(GoogleMap.MAP_TYPE_SATELLITE); break; } } + + @Override + public void onNothingSelected(AdapterView parent) {} + }); + } + + /** + * Initializes the smoothing filter spinner. + * Allows user to select how trajectory is smoothed. + */ + private void initSmoothingSpinner() { + + String[] options = new String[]{ + "Select Display Smoothing", + "Raw", + "Moving Average", + "Exponential" + }; + + ArrayAdapter adapter = new ArrayAdapter<>( + requireContext(), + android.R.layout.simple_spinner_dropdown_item, + options + ); + + smoothingSpinner.setAdapter(adapter); + + // Show hint item initially + smoothingSpinner.setSelection(0); + + smoothingSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { + @Override + public void onItemSelected(AdapterView parent, View view, int position, long id) { + + switch (position) { + case 0: + // Hint row selected, do nothing + return; + case 1: + smoothingType = SmoothingType.RAW; + break; + case 2: + smoothingType = SmoothingType.MOVING_AVERAGE; + break; + case 3: + smoothingType = SmoothingType.EXPONENTIAL; + break; + } + + // Reset smoothing state when switching modes + smoothedDisplayLocation = null; + smoothingBuffer.clear(); + } + @Override public void onNothingSelected(AdapterView parent) {} }); } + + /** + * Returns the display location after applying the selected smoothing filter. + */ + private LatLng applySmoothing(@NonNull LatLng newLocation) { + + switch (smoothingType) { + + case RAW: + return newLocation; + + case MOVING_AVERAGE: + smoothingBuffer.add(newLocation); + + if (smoothingBuffer.size() > SMOOTHING_WINDOW) { + smoothingBuffer.remove(0); + } + + double sumLat = 0; + double sumLng = 0; + + for (LatLng p : smoothingBuffer) { + sumLat += p.latitude; + sumLng += p.longitude; + } + + return new LatLng( + sumLat / smoothingBuffer.size(), + sumLng / smoothingBuffer.size() + ); + + case EXPONENTIAL: + + if (smoothedDisplayLocation == null) { + smoothedDisplayLocation = newLocation; + return newLocation; + } + + double lat = ALPHA * newLocation.latitude + + (1 - ALPHA) * smoothedDisplayLocation.latitude; + + double lng = ALPHA * newLocation.longitude + + (1 - ALPHA) * smoothedDisplayLocation.longitude; + + smoothedDisplayLocation = new LatLng(lat, lng); + return smoothedDisplayLocation; + } + + return newLocation; + } + + /** + * Adds a GNSS observation to the rolling history and redraws the overlays. + * + * @param point New GNSS position + */ + public void addGnssObservation(@NonNull LatLng point) { + addObservation(gnssHistory, point); + redrawObservationOverlays(); + } + + /** + * Adds a WiFi observation to the rolling history and redraws the overlays. + * + * @param point New WiFi-derived position + */ + public void addWifiObservation(@NonNull LatLng point) { + addObservation(wifiHistory, point); + redrawObservationOverlays(); + } + + /** + * Adds a PDR observation to the rolling history and redraws the overlays. + * + * @param point New PDR-derived position + */ + public void addPdrObservation(@NonNull LatLng point) { + addObservation(pdrHistory, point); + redrawObservationOverlays(); + } + + /** * Update the user's current location on the map, create or move orientation marker, * and append to polyline if the user actually moved. @@ -307,30 +668,74 @@ public void onNothingSelected(AdapterView parent) {} * @param newLocation The new location to plot. * @param orientation The user’s heading (e.g. from sensor fusion). */ - public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { - if (gMap == null) return; + /** + * Update the user's current location on the map. + * + * @return true if the map was ready and the update was rendered; + * false if {@code gMap} was not yet initialised (caller should retry). + */ + public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation) { + if (gMap == null) return false; - // Keep track of current location + // Apply selected smoothing filter before rendering + LatLng displayLocation = applySmoothing(newLocation); + + addObservation(pdrHistory, displayLocation); + redrawObservationOverlays(); + + // Keep track of current location using the displayed point LatLng oldLocation = this.currentLocation; - this.currentLocation = newLocation; + this.currentLocation = displayLocation; + + // Determine visual state from sensor fusion + boolean stationary = (sensorFusion != null) && sensorFusion.isStationary(); + float uncertainty = (sensorFusion != null) + ? sensorFusion.getPositionUncertaintyMeters() : 2f; + // Cap displayed radius: show at least 1 m, at most 15 m + double circleRadius = Math.min(Math.max(uncertainty, 1f), 15f); // If no marker, create it if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) + .position(displayLocation) .flat(true) .title("Current Position") .icon(BitmapDescriptorFactory.fromBitmap( UtilFunctions.getBitmapFromVector(requireContext(), R.drawable.ic_baseline_navigation_24))) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); + gMap.animateCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); } else { - // Update marker position + orientation - orientationMarker.setPosition(newLocation); + orientationMarker.setPosition(displayLocation); orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); + // Smooth pan — animateCamera avoids the jarring jump of moveCamera + gMap.animateCamera(CameraUpdateFactory.newLatLng(displayLocation)); + } + + // Fade the marker when stationary so the user can clearly see the lock state + if (orientationMarker != null) { + orientationMarker.setAlpha(stationary ? 0.55f : 1.0f); + } + + // Draw / update the accuracy circle around the position marker + if (accuracyCircle == null) { + accuracyCircle = gMap.addCircle(new CircleOptions() + .center(displayLocation) + .radius(circleRadius) + .strokeWidth(1.5f) + .strokeColor(Color.argb(180, 33, 150, 243)) // blue outline + .fillColor(Color.argb(40, 33, 150, 243))); // faint blue fill + } else { + accuracyCircle.setCenter(displayLocation); + accuracyCircle.setRadius(circleRadius); + // Tint the circle green when stationary to give instant visual feedback + if (stationary) { + accuracyCircle.setStrokeColor(Color.argb(180, 76, 175, 80)); + accuracyCircle.setFillColor(Color.argb(40, 76, 175, 80)); + } else { + accuracyCircle.setStrokeColor(Color.argb(180, 33, 150, 243)); + accuracyCircle.setFillColor(Color.argb(40, 33, 150, 243)); + } } // Extend polyline if movement occurred @@ -339,27 +744,74 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { points.add(newLocation); polyline.setPoints(points); }*/ - // Extend polyline + // Rebuild wall geometry if the particle-filter origin has changed since the last build. + // Both the wall segments and wouldCrossWall() must use the same ENU origin; if the + // filter was reset (WiFi anchor, GNSS fix, setStartGNSSLatitude) the origin shifts and + // the old walls are misaligned, causing segments to pass through them undetected. + if (sensorFusion != null && indoorMapManager != null && indoorMapManager.getIsIndoorMapSet()) { + LatLng pfOrigin = sensorFusion.getParticleFilter().getOrigin(); + if (pfOrigin != null && !pfOrigin.equals(lastWallBuildOrigin)) { + updateWallsForPdr(); + } + } + + // Extend polyline. + // IMPORTANT: use lastPolylinePoint (not currentLocation / oldLocation) as the + // "from" anchor for the wall check. currentLocation always advances to displayLocation + // (so the marker and camera stay correct), but lastPolylinePoint only advances when a + // point is actually drawn. This prevents the anchor from moving to the wrong side of + // a wall: once a segment is rejected, the next check still starts from the last valid + // drawn point, so the trajectory can resume as soon as the fused position re-enters + // valid territory — it never gets permanently stuck. if (polyline != null) { List points = new ArrayList<>(polyline.getPoints()); + uncertainty = (sensorFusion != null) ? sensorFusion.getPositionUncertaintyMeters() : 2f; - // 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); + if (lastPolylinePoint == null) { + points.add(displayLocation); polyline.setPoints(points); + lastPolylinePoint = displayLocation; + } else { + double dist = UtilFunctions.distanceBetweenPoints(lastPolylinePoint, displayLocation); + if (dist >= MIN_POLYLINE_DISTANCE_M) { + boolean crossesWall = sensorFusion != null + && sensorFusion.getParticleFilter() + .wouldCrossWall(lastPolylinePoint, displayLocation); + if (!crossesWall) { + points.add(displayLocation); + polyline.setPoints(points); + lastPolylinePoint = displayLocation; + } + } } } - // Update indoor map overlay if (indoorMapManager != null) { - indoorMapManager.setCurrentLocation(newLocation); - setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + indoorMapManager.setCurrentLocation(displayLocation); + boolean nowIndoorMapSet = indoorMapManager.getIsIndoorMapSet(); + setFloorControlsVisibility(nowIndoorMapSet ? View.VISIBLE : View.GONE); + + if (!wasIndoorMapSet && nowIndoorMapSet) { + int building = indoorMapManager.getCurrentBuilding(); + String apiName = buildingConstantToApiName(building); + if (sensorFusion != null && apiName != null + && (sensorFusion.getSelectedBuildingId() == null + || sensorFusion.getSelectedBuildingId().isEmpty())) { + sensorFusion.setSelectedBuildingId(apiName); + } + FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) + ? sensorFusion.getFloorplanBuilding(apiName) : null; + if (info != null) { + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); + } + if (autoFloorSwitch != null && !autoFloorSwitch.isChecked()) { + autoFloorSwitch.setChecked(true); + } + } + wasIndoorMapSet = nowIndoorMapSet; } + return true; } @@ -418,6 +870,36 @@ public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng posi */ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; + addObservation(gnssHistory, gnssLocation); + redrawObservationOverlays(); + fetchFloorplanIfNeeded(gnssLocation); + + // Drive building detection from raw GNSS so the floorplan appears as soon + // as GPS is available, even before the particle filter produces a fused position. + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(gnssLocation); + boolean nowIndoorMapSet = indoorMapManager.getIsIndoorMapSet(); + setFloorControlsVisibility(nowIndoorMapSet ? View.VISIBLE : View.GONE); + if (!wasIndoorMapSet && nowIndoorMapSet) { + int building = indoorMapManager.getCurrentBuilding(); + String apiName = buildingConstantToApiName(building); + if (sensorFusion != null && apiName != null + && (sensorFusion.getSelectedBuildingId() == null + || sensorFusion.getSelectedBuildingId().isEmpty())) { + sensorFusion.setSelectedBuildingId(apiName); + } + FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) + ? sensorFusion.getFloorplanBuilding(apiName) : null; + if (info != null) { + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); + } + if (autoFloorSwitch != null && !autoFloorSwitch.isChecked()) { + autoFloorSwitch.setChecked(true); + } + wasIndoorMapSet = true; + } + } + if (!isGnssOn) return; if (gnssMarker == null) { @@ -442,6 +924,14 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } } + /** + * Updates the WiFi history with a new location and refreshes the map display. + * @param wifiLocation The new coordinates to add to the observation history. + */ + public void updateWiFiObservation(@NonNull LatLng wifiLocation) { + addObservation(wifiHistory, wifiLocation); + redrawObservationOverlays(); + } /** * Remove GNSS marker if user toggles it off @@ -500,8 +990,16 @@ public void clearMapAndReset() { gnssMarker.remove(); gnssMarker = null; } + if (accuracyCircle != null) { + accuracyCircle.remove(); + accuracyCircle = null; + } lastGnssLocation = null; currentLocation = null; + lastPolylinePoint = null; + lastWallBuildOrigin = null; + floorplanFetchAttempted = false; + wasIndoorMapSet = false; // Clear test point markers for (Marker m : testPointMarkers) { @@ -509,6 +1007,13 @@ public void clearMapAndReset() { } testPointMarkers.clear(); + // remove coloured observation circles from map + gnssHistory.clear(); + wifiHistory.clear(); + pdrHistory.clear(); + clearObservationCircles(); + + smoothedDisplayLocation = null; // Re-create empty polylines with your chosen colors if (gMap != null) { @@ -574,54 +1079,95 @@ private void drawBuildingPolygon() { PolygonOptions buildingPolygonOptions = new PolygonOptions() .add(nucleus1, nucleus2, nucleus3, nucleus4, nucleus5) - .strokeColor(Color.RED) // Red border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 255, 0, 0)) // Semi-transparent red fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.RED) + .strokeWidth(10f) + .fillColor(Color.argb(70, 255, 0, 0)) + .clickable(true) + .zIndex(1); - // Options for the new polygon PolygonOptions buildingPolygonOptions2 = new PolygonOptions() .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 - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.BLUE) + .strokeWidth(10f) + .fillColor(Color.argb(50, 0, 0, 255)) + .clickable(true) + .zIndex(1); PolygonOptions buildingPolygonOptions3 = new PolygonOptions() .add(fjb1, fjb2, fjb3, fjb4, fjb1) - .strokeColor(Color.GREEN) // Green border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 0, 255, 0)) // Semi-transparent green fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.GREEN) + .strokeWidth(10f) + .fillColor(Color.argb(70, 0, 255, 0)) + .clickable(true) + .zIndex(1); PolygonOptions buildingPolygonOptions4 = new PolygonOptions() .add(faraday1, faraday2, faraday3, faraday4, faraday1) - .strokeColor(Color.YELLOW) // Yellow border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 255, 255, 0)) // Semi-transparent yellow fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays - - - // Remove the old polygon if it exists - if (buildingPolygon != null) { - buildingPolygon.remove(); - } + .strokeColor(Color.YELLOW) + .strokeWidth(10f) + .fillColor(Color.argb(70, 255, 255, 0)) + .clickable(true) + .zIndex(1); + + // Remove old polygons if they exist (e.g. after map reset) + if (buildingPolygon != null) buildingPolygon.remove(); + if (nkmlPolygon != null) nkmlPolygon.remove(); + if (fjbPolygon != null) fjbPolygon.remove(); + if (faradayPolygon != null) faradayPolygon.remove(); - // Add the polygon to the map buildingPolygon = gMap.addPolygon(buildingPolygonOptions); - gMap.addPolygon(buildingPolygonOptions2); - gMap.addPolygon(buildingPolygonOptions3); - gMap.addPolygon(buildingPolygonOptions4); + nkmlPolygon = gMap.addPolygon(buildingPolygonOptions2); + fjbPolygon = gMap.addPolygon(buildingPolygonOptions3); + faradayPolygon = gMap.addPolygon(buildingPolygonOptions4); + + // Tag each polygon so the click listener can identify the building by name + buildingPolygon.setTag("Nucleus Building"); + nkmlPolygon.setTag("Noreen & Kenneth Murray Library"); + fjbPolygon.setTag("Frank Jarvis Building"); + faradayPolygon.setTag("Faraday Building"); + + // Show the building name in a Toast when the user taps a polygon + gMap.setOnPolygonClickListener(polygon -> { + Object tag = polygon.getTag(); + if (tag != null && getContext() != null) { + android.widget.Toast.makeText( + getContext(), (String) tag, android.widget.Toast.LENGTH_SHORT).show(); + } + }); + Log.d(TAG, "Building polygon added, vertex count: " + buildingPolygon.getPoints().size()); } //region Auto-floor logic + // Uses WiFi floor when available; otherwise barometric elevation divided by floor height (meters). /** * Starts the periodic auto-floor evaluation task. Checks every second * and applies floor changes only after the debounce window (3 seconds * of consistent readings). */ + //endregion + + /** + * Maps barometer elevation to floor level based on height ranges. + * - 9-11m: Floor 3 + * - 3-6m: Floor 2 + * - -1 to 2m: Floor 1 + * - -4 to -7: Ground floor + * - -8 and below: Lower ground + * + * @param elevation Barometer elevation in meters + * @return Floor level (0 = ground, 1 = first floor, -1 = lower ground, etc.) + */ + private int getFloorFromBarometerHeight(float elevation) { + if (elevation >= 9f && elevation <= 11f) return 3; + if (elevation >= 3f && elevation <= 6f) return 2; + if (elevation >= -1f && elevation <= 2f) return 1; + if (elevation >= -7f && elevation <= -4f) return 0; + if (elevation <= -8f) return -1; + return FLOOR_TRANSITION_ZONE; + } + private void startAutoFloor() { if (autoFloorHandler == null) { autoFloorHandler = new Handler(Looper.getMainLooper()); @@ -644,27 +1190,23 @@ public void run() { } /** - * Applies the best-guess floor immediately without debounce. - * Called once when auto-floor is first toggled on, so the user - * sees an instant correction after manually browsing wrong floors. + * Applies the barometer-based floor immediately without debounce. + * Maps barometer elevation to floor level using the configured height ranges. */ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; - int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); - } else { - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - } + updateWallsForPdr(); + + float elevation = sensorFusion.getElevation(); + int candidateFloor = getFloorFromBarometerHeight(elevation); + if (candidateFloor == FLOOR_TRANSITION_ZONE) return; indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); - // Seed the debounce state so subsequent checks don't re-trigger immediately + Log.d(TAG, "applyImmediateFloor -> elevation=" + elevation + " floor=" + candidateFloor); + lastCandidateFloor = candidateFloor; lastCandidateTime = SystemClock.elapsedRealtime(); } @@ -682,28 +1224,20 @@ 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. + * Evaluates the current floor using barometric elevation with debounce. + * Maps barometer height directly to floor level based on configured ranges. */ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; - int candidateFloor; + updateWallsForPdr(); - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); - } else { - // Fallback: barometric elevation estimate - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - } + float elevation = sensorFusion.getElevation(); + int candidateFloor = getFloorFromBarometerHeight(elevation); + + if (candidateFloor == FLOOR_TRANSITION_ZONE) return; - // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { lastCandidateFloor = candidateFloor; @@ -714,10 +1248,88 @@ 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 lastCandidateTime = now; + Log.d(TAG, "Auto-floor applied: elevation=" + elevation + " floor=" + candidateFloor); } } //endregion + + /** + * Fetches floorplan data from the server once per recording session. + * On success, caches the buildings in SensorFusion and refreshes indoor map overlays. + */ + private void fetchFloorplanIfNeeded(LatLng gnssLocation) { + if (floorplanFetchAttempted) return; + if (sensorFusion == null) return; + floorplanFetchAttempted = true; + + floorplanApiClient.requestFloorplan( + gnssLocation.latitude, + gnssLocation.longitude, + new ArrayList<>(), + new FloorplanApiClient.FloorplanCallback() { + @Override + public void onSuccess(List buildings) { + if (sensorFusion == null) return; + sensorFusion.setFloorplanBuildings(buildings); + if (indoorMapManager != null) { + indoorMapManager.setIndicationOfIndoorMap(); + if (currentLocation != null) { + indoorMapManager.setCurrentLocation(currentLocation); + } + // Load wall geometry immediately so the particle filter has wall + // constraints from the moment the floorplan arrives, even if + // auto-floor is not enabled. + updateWallsForPdr(); + } + Log.d(TAG, "Floorplan fetched: " + buildings.size() + " buildings"); + } + + @Override + public void onFailure(String error) { + Log.w(TAG, "Floorplan fetch failed: " + error); + floorplanFetchAttempted = false; // allow retry on next update + } + } + ); + } + + /** Maps an IndoorMapManager building constant to the API building name. */ + private String buildingConstantToApiName(int building) { + switch (building) { + case IndoorMapManager.BUILDING_NUCLEUS: return "nucleus_building"; + case IndoorMapManager.BUILDING_LIBRARY: return "library"; + case IndoorMapManager.BUILDING_MURCHISON: return "murchison_house"; + default: return null; + } + } + + private void updateWallsForPdr() { + if (sensorFusion == null || indoorMapManager == null) return; + if (!indoorMapManager.getIsIndoorMapSet()) return; + LatLng current = indoorMapManager.getLastLocation(); + if (current == null) return; + + // CRITICAL: build wall geometry in the SAME ENU frame as the particle positions. + // The particle filter's origin is the anchor for all (easting, northing) coordinates. + // If wallOrigin differs from particleFilter.origin, wall segments are offset and + // particles appear to phase through them even though the intersection check is correct. + LatLng pfOrigin = sensorFusion.getParticleFilter().getOrigin(); + if (pfOrigin != null) { + // Particle filter is initialized — use its origin so both frames are aligned. + wallOrigin = pfOrigin; + } else if (wallOrigin == null) { + // Filter not yet initialized; use current location as a temporary origin. + // It will be recomputed once the filter has an origin. + wallOrigin = current; + } + + FloorplanApiClient.FloorShapes floor = indoorMapManager.getCurrentFloorShape(); + if (floor == null) return; + + List> walls = WallGeometryBuilder.buildWalls(floor, wallOrigin); + sensorFusion.setPdrWalls(walls); + lastWallBuildOrigin = wallOrigin; + } } 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..b96fabbe --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -0,0 +1,491 @@ + +package com.openpositioning.PositionMe.sensors; +import android.util.Log; +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.utils.UtilFunctions; +import java.util.ArrayList; +import java.util.List; + + +import java.util.Random; +/** + * Sequential Monte Carlo (particle filter) for fused indoor positioning. + * + *

Maintains a cloud of NUM_PARTICLES weighted hypothesis particles, each + * representing a candidate position (easting, northing) and heading-bias offset in a + * local ENU frame centred on the first fix. Three update sources drive the filter:

+ *
    + *
  • PDR propagates particles using the pedestrian dead-reckoning step displacement + * with per-particle heading-bias correction and Gaussian noise, then slides any + * particle that would cross a wall along the wall tangent so it remains inside + * the building.
    • + *
  • GNSS weights particles with a Student-t likelihood (v=4) that is robust to + * the multipath outliers common indoors. Accuracy is inflated 10× when wall + * segments are loaded to prevent bad GPS fixes from dragging the cloud outside + * the building.
    • + *
  • WiFi applies the same Student-t likelihood using positions from the + * openpositioning fingerprinting API, which are the primary indoor anchor. The + * filter is automatically reset to a WiFi fix whenever the estimated position + * has diverged by more than 2.5× the particle-cloud spread.
    • + *
+ * + *

Systematic resampling with Gaussian roughening (Thrun, Burgard & Fox 2005, + * Probabilistic Robotics) is applied whenever the effective sample size N_eff drops + * below N/2 to prevent sample impoverishment.

+ */ +public class ParticleFilter { + private static final String TAG = "ParticleFilter"; + private static final int NUM_PARTICLES = 200; // Number of particles + private float[][] particles; + + // [num particles][3] = {easting (m), northing (m), headingBias (rad)} + // headingBias is the estimated error between the phone's compass heading and the + // true walking direction. It starts near zero and is implicitly corrected during + // movement as particles with the right bias land closer to GNSS/WiFi observations + // and therefore accumulate higher weights. + private float[] weights; // Weights for each particle + private LatLng origin; // Origin point for ENU conversion + + private boolean initialized = false; + private final Random random = new Random(); + + // Wall segments: {x1, y1, x2, y2} in ENU + private List walls = new ArrayList<>(); + + // ~17° initial heading-bias uncertainty, matching Woodman & Harle (UbiComp 2008). + private static final float INITIAL_BIAS_STD = 0.3f; + // Slow random-walk drift on heading bias per step (rad). + private static final float BIAS_DRIFT_STD = 0.01f; + + /** + * Initialises the filter at the given geographic position. + * Particles are spread in a Gaussian cloud of radius {@code accuracyMeters} around + * {@code firstFix}, which becomes the ENU origin for all subsequent computations. + * Each particle is also assigned a random initial heading-bias drawn from + * {@link #INITIAL_BIAS_STD}. Subsequent calls are ignored once the filter is + * initialised; call {@link #reset()} first to re-seed at a new location. + * It's best to initialise with a GNSS fix when outdoors, but the filter can also be + * seeded with a WiFi position or manual anchor when GNSS is unavailable or unreliable indoors.. + * + * -- Japjot + * + * @param firstFix WGS-84 position used as the ENU origin and initial mean + * @param accuracyMeters 1-sigma spread of the initial particle cloud in metres; + * use a larger value when the fix is uncertain (e.g. GNSS indoors) + */ + public void initialise(LatLng firstFix, float accuracyMeters) { + if (initialized) return; // ignore subsequent calls + this.origin = firstFix; + this.particles = new float[NUM_PARTICLES][3]; + this.weights = new float[NUM_PARTICLES]; + float spread = accuracyMeters; + + for (int i = 0; i < NUM_PARTICLES; i++) { + particles[i][0] = (float) (random.nextGaussian() * spread); // east + particles[i][1] = (float) (random.nextGaussian() * spread); // north + particles[i][2] = (float) (random.nextGaussian() * INITIAL_BIAS_STD); // heading bias + weights[i] = 1.0f / NUM_PARTICLES; + } + initialized = true; + Log.d(TAG, "Initialized at " + firstFix + " accuracy=" + accuracyMeters + "m"); + } + + public boolean isInitialized() { + return initialized; + } + + + public void reset() { + initialized = false; + origin = null; + particles = null; + weights = null; + } + + + public LatLng getOrigin() { + return origin; + } + + /** + * Loads wall segments into the filter for collision detection. + * Each element is a float array {@code {x1, y1, x2, y2}} in ENU metres relative to + * {@link #getOrigin()}. These segments are used in {@link #predict} to prevent + * particles from passing through building walls, and in {@link #wouldCrossWall} for + * trajectory display validation. + * + * @param wallSegments list of wall line segments; may be empty to disable collision checks + */ + public void setWalls(List wallSegments) { + this.walls = wallSegments; + } + + /** + * Returns {@code true} if at least one wall segment has been loaded via {@link #setWalls}. + * Used to distinguish indoor operation (walls present) from outdoor/unloaded state so + * that GNSS noise inflation and outlier thresholds can be tightened appropriately. + * + * @return {@code true} when the wall list is non-null and non-empty + */ + public boolean hasWalls() { + return walls != null && !walls.isEmpty(); + } + + + public static final float HEADING_NOISE_STD = 0.05f; + public static final float STRIDE_LENGTH_NOISE_STD = 0.05f; + + // Small positional noise added to each particle after resampling. + // Prevents sample impoverishment: without roughening, particles copied from the + // same parent are identical and provide no extra information on the next step. + private static final float ROUGHENING_STD = 0.08f; // metres — reduced to limit stationary drift + + /** + * Propagates each particle forward by one PDR step using a noisy motion model. + * The step displacement is derived from the PDR easting/northing deltas. Per-particle + * heading-bias (see {@link #INITIAL_BIAS_STD}) is applied before adding Gaussian noise + * ({@link #HEADING_NOISE_STD}, {@link #STRIDE_LENGTH_NOISE_STD}) to the heading and stride. + * Particles that would cross a wall segment are instead slid along the wall tangent so they + * remain inside the building without clustering at the boundary. + *

+ * Weights are intentionally unchanged here; weight updates occur only in + * {@link #updateGNSS} and {@link #updateWiFi} when an observation arrives. + * + * @param deltaEasting eastward PDR displacement since the last step, in metres + * @param deltaNorthing northward PDR displacement since the last step, in metres + * + * written by -- Japjot + * @JapjotS + */ + public void predict(float deltaEasting, float deltaNorthing) { + if (!initialized) return; // ignore if not initialized + + + float stride = (float) Math.sqrt(deltaEasting * deltaEasting + deltaNorthing * deltaNorthing); + float heading = (float) Math.atan2(deltaNorthing, deltaEasting); // calculate heading from deltas + + for (int i = 0; i < NUM_PARTICLES; i++) { + // Apply this particle's estimated heading bias before adding step noise. + // Based on: Woodman, O.J. & Harle, R. (2008). "Pedestrian localisation for + // indoor environments." UbiComp 2008, pp. 114-123. + float correctedHeading = heading + particles[i][2]; + float noisyHeading = correctedHeading + (float) (random.nextGaussian() * HEADING_NOISE_STD); + float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); + + float oldX = particles[i][0]; + float oldY = particles[i][1]; + float newX = oldX + noisyStride * (float) Math.cos(noisyHeading); + float newY = oldY + noisyStride * (float) Math.sin(noisyHeading); + + if (intersectsWall(oldX, oldY, newX, newY)) { + // Wall hit: slide along the first blocking wall's tangent instead of freezing. + // Frozen particles cluster at wall boundaries and pull the weighted mean (fused + // position) towards walls even as the user walks forward. Sliding lets particles + // continue moving in the corridor direction, keeping the cloud well-distributed. + boolean moved = false; + for (float[] wall : walls) { + if (!doIntersect(oldX, oldY, newX, newY, + wall[0], wall[1], wall[2], wall[3])) continue; + // Wall tangent unit vector + float wx = wall[2] - wall[0]; + float wy = wall[3] - wall[1]; + float wLen = (float) Math.sqrt(wx * wx + wy * wy); + if (wLen < 1e-6f) break; + wx /= wLen; + wy /= wLen; + // Project desired step onto the wall tangent + float dx = newX - oldX; + float dy = newY - oldY; + float proj = dx * wx + dy * wy; + float slideX = oldX + proj * wx; + float slideY = oldY + proj * wy; + // Accept only if the slide itself is wall-free + if (!intersectsWall(oldX, oldY, slideX, slideY)) { + particles[i][0] = slideX; + particles[i][1] = slideY; + moved = true; + } + break; // handle only the first blocking wall + } + // If no valid slide found, keep at old position (fallback) + } else { + particles[i][0] = newX; + particles[i][1] = newY; + } + + // Heading bias random walk: bias drifts slowly each step. + particles[i][2] += (float) (random.nextGaussian() * BIAS_DRIFT_STD); + } + // Weights are intentionally NOT updated or resampled here. + // predict() only propagates particle positions; weight updates happen + // exclusively in updateGNSS() and updateWiFi() when a new observation arrives. + } + + /** + * Returns true if the straight line between two geographic positions would + * cross any loaded wall segment. Used by the map layer to prevent the + * trajectory polyline from being drawn through floorplan walls. + * + * @param from start of the candidate segment (WGS-84) + * @param to end of the candidate segment (WGS-84) + */ + public boolean wouldCrossWall(LatLng from, LatLng to) { + if (origin == null || walls == null || walls.isEmpty()) return false; + float[] fromENU = UtilFunctions.convertWGS84ToENU(origin, from); + float[] toENU = UtilFunctions.convertWGS84ToENU(origin, to); + return intersectsWall(fromENU[0], fromENU[1], toENU[0], toENU[1]); + } + + private boolean intersectsWall(float x1, float y1, float x2, float y2) { + // Check if the line segment from (x1, y1) to (x2, y2) intersects any wall segment + if (walls == null || walls.isEmpty()) return false; + for (float[] wall : walls) { + if (doIntersect(x1, y1, x2, y2, wall[0], wall[1], wall[2], wall[3])) { + return true; + } + } + return false; + } + + + + + private boolean doIntersect(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) { + float den = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); //calculate the denominator of the intersection formula + if (den == 0) return false; + // Calculate the intersection point using the parametric form of the line segments + float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / den; + float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / den; + return ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1; + } + + /** + * Computes the effective sample size: N_eff = 1 / sum(w_i²). + * + *

N_eff equals NUM_PARTICLES when all weights are equal (maximum diversity) + * and approaches 1 when a single particle carries all the weight (collapsed). + * Resampling is triggered when N_eff falls below NUM_PARTICLES / 2.

+ */ + private float computeEffectiveSampleSize() { + float sumSquared = 0f; + for (float w : weights) { + sumSquared += w * w; + } + return sumSquared < 1e-10f ? NUM_PARTICLES : 1.0f / sumSquared; + } + + /** + * Systematic resampling followed by roughening. + * + *

Systematic resampling draws NUM_PARTICLES new particles proportional to + * their weights using a single random offset, which has lower variance than + * multinomial resampling.

+ * + *

Roughening adds small Gaussian noise after resampling. Without it, + * particles copied from the same parent are identical and degenerate into + * a point mass on the next predict step.

+ */ + private void resample() { + float[][] newParticles = new float[NUM_PARTICLES][3]; + float step = 1.0f / NUM_PARTICLES; + float cumulativeWeight = weights[0]; + float randomStart = random.nextFloat() * step; + int j = 0; + for (int i = 0; i < NUM_PARTICLES; i++) { + float threshold = randomStart + i * step; + while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { + j++; + cumulativeWeight += weights[j]; + } + newParticles[i][0] = particles[j][0]; + newParticles[i][1] = particles[j][1]; + newParticles[i][2] = particles[j][2]; + } + particles = newParticles; + + // Roughening: add small noise so copied particles diverge on the next step. + // Wall-check each jitter so roughening doesn't land particles inside walls. + for (int i = 0; i < NUM_PARTICLES; i++) { + float rx = (float) (random.nextGaussian() * ROUGHENING_STD); + float ry = (float) (random.nextGaussian() * ROUGHENING_STD); + float roughenedX = particles[i][0] + rx; + float roughenedY = particles[i][1] + ry; + if (!intersectsWall(particles[i][0], particles[i][1], roughenedX, roughenedY)) { + particles[i][0] = roughenedX; + particles[i][1] = roughenedY; + } + } + + // Reset to uniform weights after resampling + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } + } + + /** + * Updates particle weights from a GNSS observation using a Student-t likelihood function. + * The Student-t distribution (v=4) has heavier tails than a Gaussian, making it robust to + * the multipath and NLOS outlier fixes common indoors. When wall segments are loaded the + * effective accuracy is inflated to {@code max(10 × gnssAccuracy, 50 m)} to prevent indoor + * GPS noise from dragging particles through walls. Systematic resampling is triggered when + * the effective sample size N_eff drops below N/2. + * + * @param gnssPosition WGS-84 position reported by the GNSS subsystem + * @param gnssAccuracy 1-sigma horizontal accuracy reported by the GNSS fix, in metres + */ + public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { + if (!initialized) return; // ignore if not initialized + + // Convert GNSS position to ENU coordinates + float[] mesurementENU = UtilFunctions.convertWGS84ToENU(origin, gnssPosition); + + float mx = mesurementENU[0]; //easting value of the measurement + float my = mesurementENU[1]; //northing value of the measurement + + // Indoors, GPS signals reflect off ceilings and walls, giving fixes several + // metres off and often on the wrong side of a wall. Inflate the uncertainty + // so the GNSS observation cannot drag particles through wall segments. + float effectiveAccuracy = walls.isEmpty() ? gnssAccuracy : Math.max(gnssAccuracy * 10f, 50f); + float variance = effectiveAccuracy * effectiveAccuracy; // Convert accuracy to variance sigma^2 + + + // Student-t likelihood (v=4) — heavier tails than Gaussian, robust to outlier GNSS fixes. + // Based on: Nurminen, H. et al. (2013). "Particle Filter and Smoother for Indoor + // Localization." IPIN 2013. w_i isproportional to (1 + d²/(v·sigma²))^(-(v+2)/2), v=4 leasd to exponent=-3. + final float nu = 4.0f; + float weightSum = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dx = particles[i][0] - mx; + float dy = particles[i][1] - my; + float distanceSquared = dx * dx + dy * dy; + weights[i] *= (float) Math.pow(1.0f + distanceSquared / (nu * variance), -(nu + 2f) / 2f); + weightSum += weights[i]; + } + + if (weightSum < 1e-10f) { + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } + } else { + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] /= weightSum; + } + } + + // Resample only when particle diversity is low (N_eff < N/2). + // Resampling on every observation wastes diversity; N_eff-gating preserves it. + float nEff = computeEffectiveSampleSize(); + if (nEff < NUM_PARTICLES / 2.0f) { + resample(); + Log.d(TAG, "GNSS resampled: Neff=" + nEff); + } + Log.d(TAG, "GNSS update: pos=" + gnssPosition + " accuracy=" + gnssAccuracy + "m Neff=" + nEff); + + + } + + /** + * Updates particle weights from a WiFi fingerprinting observation using a Student-t + * likelihood function identical in form to {@link #updateGNSS}. WiFi fixes are the + * preferred indoor anchor because they are unaffected by multipath; the filter is + * initialised here if no prior fix exists, and reset to this position if the current + * estimate has diverged by more than 2.5× the particle-cloud spread. + * + * @param wifiPosition WGS-84 position returned by the openpositioning WiFi API + * @param wifiAccuracy assumed 1-sigma accuracy of the WiFi fix, in metres + */ + public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { + if (!initialized) return; // ignore if not initialized + float[] mesurementENU = UtilFunctions.convertWGS84ToENU(origin, wifiPosition); + float mx = mesurementENU[0]; //easting value of the measurement + float my = mesurementENU[1]; //northing value of the measurement + float variance = wifiAccuracy * wifiAccuracy; // Convert accuracy to variance sigma^2 + + + // Student-t likelihood (v=4) — same robust formulation as updateGNSS(). + final float nu = 4.0f; + float weightSum = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dx = particles[i][0] - mx; + float dy = particles[i][1] - my; + float distanceSquared = dx * dx + dy * dy; + weights[i] *= (float) Math.pow(1.0f + distanceSquared / (nu * variance), -(nu + 2f) / 2f); + weightSum += weights[i]; + } + + if (weightSum < 1e-10f) { + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } + } else { + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] /= weightSum; + } + } + + // Resample only when particle diversity is low (N_eff < N/2) + float nEff = computeEffectiveSampleSize(); + if (nEff < NUM_PARTICLES / 2.0f) { + resample(); + Log.d(TAG, "WiFi resampled: Neff=" + nEff); + } + Log.d(TAG, "WiFi update: pos=" + wifiPosition + " accuracy=" + wifiAccuracy + "m Neff=" + nEff); + + + } + + /** + * Computes and returns the fused position estimate as the weighted mean of all particles. + * The mean is computed in ENU space and then converted back to WGS-84. This is the + * primary output of the filter, representing the best current estimate of the user's + * location given all PDR, GNSS, and WiFi observations received so far. + * + * @return fused WGS-84 position, or {@code null} if the filter has not been initialised + */ + public LatLng getFusedPosition() { + if (!initialized) return null; // Return null if not initialized + + // Calculate weighted average of particles + float meanEasting = 0f; + float meanNorthing = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + meanEasting += particles[i][0] * weights[i]; + meanNorthing += particles[i][1] * weights[i]; + } + + // Convert mean ENU back to WGS84 coordinates + return UtilFunctions.convertENUToWGS84(origin, new float[]{meanEasting, meanNorthing, 0f}); + } + + /** + * Returns the RMS spread of particles around their weighted mean, in metres. + * Provides a live estimate of position uncertainty — large when particles are + * spread out, small when they are tightly clustered. + */ + /** + * Returns the RMS spread of particles around their weighted mean, in metres. + * Computed as {@code sqrt(weightedVariance_E + weightedVariance_N)}, this provides a + * live estimate of positional uncertainty: large when the cloud is spread out (poor + * convergence or divergence), small when particles are tightly clustered (high confidence). + * Used by the map display to scale the accuracy circle and by the GNSS outlier gate. + * + * @return position uncertainty in metres, or {@link Float#MAX_VALUE} if not initialised + */ + public float getPositionUncertaintyMeters() { + if (!initialized) return Float.MAX_VALUE; + float meanE = 0f, meanN = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + meanE += particles[i][0] * weights[i]; + meanN += particles[i][1] * weights[i]; + } + float varE = 0f, varN = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dE = particles[i][0] - meanE; + float dN = particles[i][1] - meanN; + varE += weights[i] * dE * dE; + varN += weights[i] * dN * dN; + } + return (float) Math.sqrt(varE + varN); + } + +} 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..8ff5da47 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -22,6 +22,60 @@ */ public class SensorEventHandler { + // FOR PARTICLE FILTER + /** + * Callback invoked by {@link SensorEventHandler} every time a valid step is detected. + * The deltas are in ENU metres relative to the session origin, computed from the + * accumulated PDR position at the time of the step event. + */ + public interface StepListener { + /** + * Called once per accepted step. + * + * @param deltaEasting eastward displacement of this step in metres + * @param deltaNorthing northward displacement of this step in metres + */ + void onStep(float deltaEasting, float deltaNorthing); + } + + private StepListener stepListener; + private float lastEasting = 0f; + private float lastNorthing = 0f; + private boolean orientationInitialized = false; + + private float lastAcceptedHeading = Float.NaN; + private static final float MAX_HEADING_JUMP_RAD = (float) (Math.PI / 6.0); + private static final long MIN_STEP_INTERVAL_MS = 300; + + /** + * Registers a listener to receive per-step ENU displacement notifications. + * The listener is invoked from the sensor thread each time a step passes all quality + * gates (minimum interval, peak acceleration, heading outlier check). Pass {@code null} + * to remove a previously registered listener. + * + * @param listener the {@link StepListener} to notify on each accepted step, or {@code null} + */ + public void setStepListener(StepListener listener) { + this.stepListener = listener; + } + + /** + * Resets the PDR step-delta baseline to zero and clears the heading history. + * Must be called at the start of each recording session so that the first step of the + * new session does not fire a large spurious delta derived from the previous session's + * accumulated PDR position. + */ + public void resetStepOrigin() { + lastEasting = 0f; + lastNorthing = 0f; + orientationInitialized = false; + lastAcceptedHeading = Float.NaN; + } + + // END OF PARTICLE FILTER + + + private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; @@ -92,12 +146,11 @@ public void handleSensorEvent(SensorEvent sensorEvent) { } break; - // NOTE: intentional fall-through from GYROSCOPE to LINEAR_ACCELERATION - // (existing behavior preserved during refactoring) case Sensor.TYPE_GYROSCOPE: state.angularVelocity[0] = sensorEvent.values[0]; state.angularVelocity[1] = sensorEvent.values[1]; state.angularVelocity[2] = sensorEvent.values[2]; + break; case Sensor.TYPE_LINEAR_ACCELERATION: state.filteredAcc[0] = sensorEvent.values[0]; @@ -143,45 +196,62 @@ public void handleSensorEvent(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation); SensorManager.getOrientation(rotationVectorDCM, state.orientation); + orientationInitialized = true; break; case Sensor.TYPE_STEP_DETECTOR: + if (!orientationInitialized) break; long stepTime = SystemClock.uptimeMillis() - bootTime; - if (currentTime - lastStepTime < 20) { - Log.e("SensorFusion", "Ignoring step event, too soon after last step event:" - + (currentTime - lastStepTime) + " ms"); - break; - } else { - lastStepTime = currentTime; - - if (accelMagnitude.isEmpty()) { - Log.e("SensorFusion", - "stepDetection triggered, but accelMagnitude is empty! " + - "This can cause updatePdr(...) to fail or return bad results."); + long timeSinceLastStep = currentTime - lastStepTime; + if (timeSinceLastStep < MIN_STEP_INTERVAL_MS) break; + lastStepTime = currentTime; + + float rawHeading = state.orientation[0]; + float headingForStep; + if (!Float.isNaN(lastAcceptedHeading)) { + float delta = rawHeading - lastAcceptedHeading; + while (delta > (float) Math.PI) delta -= (float) (2 * Math.PI); + while (delta < -(float) Math.PI) delta += (float) (2 * Math.PI); + if (Math.abs(delta) > MAX_HEADING_JUMP_RAD && timeSinceLastStep >= 2000L) { + headingForStep = lastAcceptedHeading; } else { - Log.d("SensorFusion", - "stepDetection triggered, accelMagnitude size = " - + accelMagnitude.size()); + lastAcceptedHeading = rawHeading; + headingForStep = rawHeading; } + } else { + lastAcceptedHeading = rawHeading; + headingForStep = rawHeading; + } - float[] newCords = this.pdrProcessing.updatePdr( - stepTime, - this.accelMagnitude, - state.orientation[0] - ); + if (accelMagnitude.isEmpty()) { + break; + } + double peakAccel = 0.0; + for (double v : accelMagnitude) if (v > peakAccel) peakAccel = v; + if (peakAccel < 0.5) { + accelMagnitude.clear(); + break; + } - this.accelMagnitude.clear(); + float[] newCords = pdrProcessing.updatePdr(stepTime, accelMagnitude, headingForStep); + accelMagnitude.clear(); - if (recorder.isRecording()) { - this.pathView.drawTrajectory(newCords); - state.stepCounter++; - recorder.addPdrData( - SystemClock.uptimeMillis() - bootTime, - newCords[0], newCords[1]); - } - break; + if (recorder.isRecording()) { + pathView.drawTrajectory(newCords); + state.stepCounter++; + recorder.addPdrData(SystemClock.uptimeMillis() - bootTime, newCords[0], newCords[1]); } + + if (stepListener != null) { + float deltaEasting = newCords[0] - lastEasting; + float deltaNorthing = newCords[1] - lastNorthing; + stepListener.onStep(deltaEasting, deltaNorthing); + } + + lastEasting = newCords[0]; + lastNorthing = newCords[1]; + break; } } 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..2b57465a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -2,6 +2,7 @@ import android.content.Context; import android.content.SharedPreferences; +import android.graphics.PointF; import android.hardware.Sensor; import android.hardware.SensorEvent; import android.hardware.SensorEventListener; @@ -11,7 +12,11 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; +// import android.graphics.PointF; +//import com.openpositioning.PositionMe.utils.WiFiPositioning; +import com.openpositioning.PositionMe.utils.WallGeometryBuilder; import androidx.annotation.NonNull; import androidx.preference.PreferenceManager; @@ -24,6 +29,8 @@ import com.openpositioning.PositionMe.utils.PdrProcessing; import com.openpositioning.PositionMe.utils.TrajectoryValidator; import com.openpositioning.PositionMe.data.remote.ServerCommunications; +import com.openpositioning.PositionMe.utils.UtilFunctions; +import com.openpositioning.PositionMe.utils.BuildingPolygon; import java.util.ArrayList; import java.util.HashMap; @@ -91,6 +98,35 @@ public class SensorFusion implements SensorEventListener { // Sensor registration latency setting long maxReportLatencyNs = 0; + //particle filter + private final ParticleFilter particleFilter = new ParticleFilter(); + + // Last GNSS position actually sent to the particle filter. + // Used to suppress stationary noise: indoors, GPS bounces 10-30 m even when + // the device is still, which drags particles out of the building. + private LatLng lastGnssForFilter = null; + + private static final int STATIONARY_WINDOW = 20; + private static final float STATIONARY_THRESHOLD = 0.015f; + private final List linearAccelWindow = new ArrayList<>(); + private boolean isStationary = false; + + private final List headingCalibSamples = new ArrayList<>(); + private float headingBias = 0f; + private boolean headingCalibDone = false; + private long headingCalibStartMs = 0L; + private static final long HEADING_CALIB_DURATION_MS = 10_000L; + + // Timestamp (elapsedRealtime ms) of the last setStartGNSSLatitude() call. + // GNSS updates are suppressed for 10 s so a bad indoor GPS fix cannot drag + // particles away from the user-confirmed anchor immediately after anchoring. + private long filterAnchorTimeMs = 0L; + + // True once the first GNSS fix during this recording session has been used to + // anchor startLocation and write initial protobuf metadata (auto-init path). + private boolean initialMetadataWritten = false; + + // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); @@ -164,6 +200,16 @@ public void setContext(Context context) { this.eventHandler = new SensorEventHandler( state, pdrProcessing, pathView, recorder, bootTime); + + //particle filter + eventHandler.setStepListener((deltaEasting, deltaNorthing) -> { + if (particleFilter.isInitialized() && !isStationary) { + particleFilter.predict(deltaEasting, deltaNorthing); + } + }); + + + // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); wifiProcessor.registerObserver(wifiPositionManager); @@ -187,12 +233,49 @@ public void update(Object[] objList) { this.bleRttManager = new BleRttManager(recorder); bleProcessor.registerObserver(bleRttManager); + if (!rttManager.isRttSupported()) { new Handler(Looper.getMainLooper()).post(() -> Toast.makeText(appContext, "WiFi RTT is not supported on this device", Toast.LENGTH_LONG).show()); } + + wifiProcessor.registerObserver(objects -> { + new Handler(Looper.getMainLooper()).postDelayed(() -> { + LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning(); + if (wifiPosition != null) { + if (!particleFilter.isInitialized()) { + // WiFi is the preferred initial anchor indoors — more accurate than GNSS. + particleFilter.initialise(wifiPosition, 15f); + } else if (!isStationary) { + // Divergence recovery (Thrun, Burgard & Fox 2005, "Probabilistic Robotics", + // If WiFi contradicts the current estimate by more than 2.5× the particle + // spread, the filter has likely converged to a wrong location (e.g. from a + // bad indoor GNSS fix). Reset at the WiFi anchor so re-convergence can happen. + LatLng fused = particleFilter.getFusedPosition(); + float uncertainty = particleFilter.getPositionUncertaintyMeters(); + if (fused != null) { + double wifiDist = UtilFunctions.distanceBetweenPoints(fused, wifiPosition); + float divergenceThreshold = Math.max(uncertainty * 2.5f, 20f); + if (wifiDist > divergenceThreshold) { + Log.d("SensorFusion", "WiFi-reset: filter diverged " + + (int) wifiDist + " m (threshold " + (int) divergenceThreshold + " m)"); + particleFilter.reset(); + particleFilter.initialise(wifiPosition, 15f); + lastGnssForFilter = wifiPosition; + filterAnchorTimeMs = SystemClock.elapsedRealtime(); + } else { + particleFilter.updateWiFi(wifiPosition, 20f); + } + } else { + particleFilter.updateWiFi(wifiPosition, 20f); + } + } + } + autoSeedStartAndMetadataIfNeeded(); + }, 1000); + }); } //endregion @@ -204,9 +287,132 @@ public void update(Object[] objList) { * *

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

*/ + @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); + if (sensorEvent.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) { + updateStationaryState(sensorEvent.values[0], sensorEvent.values[1], sensorEvent.values[2]); + } + if (sensorEvent.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR && recorder.isRecording()) { + updateHeadingCalibration(); + } + if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { + updateFloorLogic(); + } + } + + private void updateHeadingCalibration() { + if (headingCalibDone || !isStationary) return; + long now = android.os.SystemClock.elapsedRealtime(); + if (headingCalibStartMs == 0L) headingCalibStartMs = now; + headingCalibSamples.add(state.orientation[0]); + if (now - headingCalibStartMs >= HEADING_CALIB_DURATION_MS && headingCalibSamples.size() >= 10) { + float sum = 0f; + for (float h : headingCalibSamples) sum += h; + headingBias = sum / headingCalibSamples.size(); + headingCalibDone = true; + Log.d("SensorFusion", "Heading bias calibrated: " + (float) Math.toDegrees(headingBias) + "degrees"); + } + } + + public float getHeadingBias() { + return headingCalibDone ? headingBias : 0f; + } + + /** + * Maintains a rolling variance of linear-acceleration magnitude. + * Sets {@code isStationary = true} when variance drops below the threshold, + * meaning the phone has been essentially still for the last STATIONARY_WINDOW samples. + */ + private void updateStationaryState(float x, float y, float z) { + float mag = (float) Math.sqrt(x * x + y * y + z * z); + linearAccelWindow.add(mag); + if (linearAccelWindow.size() > STATIONARY_WINDOW) { + linearAccelWindow.remove(0); + } + if (linearAccelWindow.size() < STATIONARY_WINDOW) return; + + float mean = 0f; + for (float v : linearAccelWindow) mean += v; + mean /= STATIONARY_WINDOW; + + float variance = 0f; + for (float v : linearAccelWindow) variance += (v - mean) * (v - mean); + variance /= STATIONARY_WINDOW; + + isStationary = variance < STATIONARY_THRESHOLD; + } + + // Nucleus floor-to-floor height in metres (GF=0, F1=5.5, F2=11, F3=16.5). + // Used for absolute floor index computation from barometric elevation. + private static final float NUCLEUS_FLOOR_HEIGHT_M = 5.5f; + // Lift snap tolerance: only commit a lift floor change when the barometer elevation + // is within this distance of the exact target level (prevents mid-ascent false triggers). + private static final float LIFT_SNAP_TOLERANCE_M = 0.8f; + + private void updateFloorLogic() { + if (!pdrProcessing.getElevationList().isFull()) return; + + float finishAvg = (float) pdrProcessing.getElevationList().getListCopy().stream() + .mapToDouble(f -> f).average().orElse(0.0); + float diff = finishAvg - pdrProcessing.getStartElevation(); + float floorHeight = NUCLEUS_FLOOR_HEIGHT_M; + + // Require at least 40% of floor height before evaluating a change. + if (Math.abs(diff) < floorHeight * 0.4f) return; + + // Absolute floor index from recording origin (not relative to current floor). + // Math.round avoids cumulative integer-truncation errors. + int targetFloor = Math.round(diff / floorHeight); + if (targetFloor == pdrProcessing.getCurrentFloor()) return; + + LatLng pos = getFusedPosition(); + if (pos == null) return; + + FloorplanApiClient.BuildingInfo building = getFloorplanBuilding(getSelectedBuildingId()); + if (building == null) return; + + // Use current floor + bias to index into the building's floor list. + int currentFloorIdx = pdrProcessing.getCurrentFloor() + 1; // +1 bias for Nucleus/Murchison (GF at index 1) + if (currentFloorIdx < 0 || currentFloorIdx >= building.getFloorShapesList().size()) return; + + FloorplanApiClient.FloorShapes floorShapes = building.getFloorShapesList().get(currentFloorIdx); + boolean nearTransition = false; + String type = "unknown"; + + for (FloorplanApiClient.MapShapeFeature feature : floorShapes.getFeatures()) { + String fType = feature.getIndoorType(); + if ("lift".equals(fType) || "stairs".equals(fType)) { + for (List part : feature.getParts()) { + if (BuildingPolygon.pointInPolygon(pos, part)) { + nearTransition = true; + type = fType; + break; + } + } + } + if (nearTransition) break; + } + + if (!nearTransition) return; + + float horizontalMovement = (float) Math.sqrt( + Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2)); + boolean isLift = horizontalMovement < 0.2f; + + if ("lift".equals(type) && isLift) { + // Snap-to-floor guard: only commit once the barometer reads within + // LIFT_SNAP_TOLERANCE_M of the exact target level (GF=0, F1=5.5, F2=11 …). + float targetElev = targetFloor * floorHeight; + if (Math.abs(diff - targetElev) > LIFT_SNAP_TOLERANCE_M) return; + pdrProcessing.setCurrentFloor(targetFloor); + String TAG = "FLOOR CHANGE LIFT"; + } else if ("stairs".equals(type) && !isLift) { + //pdrProcessing.setCurrentFloor(targetFloor); + String TAG = "FLOOR CHANGE STAIRS"; + + } } /** {@inheritDoc} */ @@ -242,7 +448,7 @@ public void resumeListening() { stepDetectionSensor.sensorManager.registerListener(this, stepDetectionSensor.sensor, SensorManager.SENSOR_DELAY_NORMAL); rotationSensor.sensorManager.registerListener(this, - rotationSensor.sensor, (int) 1e6); + rotationSensor.sensor, 10000); // 100 Hz — heading must update faster than step rate // Foreground service owns WiFi/BLE scanning during recording. if (!recorder.isRecording()) { startWirelessCollectors(); @@ -326,8 +532,21 @@ private void stopWirelessCollectors() { * @see SensorCollectionService */ public void startRecording() { - recorder.startRecording(pdrProcessing); + recorder.startRecording(pdrProcessing); // calls pdrProcessing.resetPDR() internally eventHandler.resetBootTime(recorder.getBootTime()); + // Sync step-delta baseline with the freshly-zeroed PDR so the first step of this + // session doesn't fire a massive spurious delta from the previous session's position. + eventHandler.resetStepOrigin(); + + initialMetadataWritten = false; + state.startLocation[0] = 0f; + state.startLocation[1] = 0f; + filterAnchorTimeMs = 0L; + lastGnssForFilter = null; + particleFilter.reset(); + headingCalibDone = false; + headingCalibSamples.clear(); + headingCalibStartMs = 0L; // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -337,6 +556,15 @@ public void startRecording() { } } + // /** + // * Inject wall polylines (meters) into PDR for collision correction. + // */ + // public void setPdrWalls(List> walls) { + // if (pdrProcessing != null) { + // pdrProcessing.setWalls(walls); + // } + // } + /** * Disables saving sensor values to the trajectory object. * Also stops the foreground service since background collection is no longer needed. @@ -482,6 +710,32 @@ public float[] getGNSSLatitude(boolean start) { return latLong; } + /** + * Returns the best available position estimate. + * Priority: (1) particle-filter fused estimate; (2) PDR dead-reckoning from + * the GNSS-anchored start location when WiFi/GNSS are unavailable (deep indoors). + * This keeps the trajectory live and fluid even when external signals fail. + * + * @return current position estimate, or null if no fix is available yet. + */ + public LatLng getFusedPosition() { + LatLng filtered = particleFilter.getFusedPosition(); + if (filtered != null) return filtered; + + // PDR fallback: startLocation + accumulated step displacement. + float startLat = state.startLocation[0]; + float startLon = state.startLocation[1]; + if (startLat == 0f && startLon == 0f) { + startLat = state.latitude; // fall back to latest raw GNSS if not anchored yet + startLon = state.longitude; + } + if (startLat == 0f && startLon == 0f) return null; + float[] pdr = pdrProcessing.getPDRMovement(); + LatLng origin = new LatLng(startLat, startLon); + if (pdr[0] == 0f && pdr[1] == 0f) return origin; + return UtilFunctions.convertENUToWGS84(origin, new float[]{pdr[0], pdr[1], 0f}); + } + /** * Setter function for core location data. * @@ -490,6 +744,82 @@ public float[] getGNSSLatitude(boolean start) { public void setStartGNSSLatitude(float[] startPosition) { state.startLocation[0] = startPosition[0]; state.startLocation[1] = startPosition[1]; + + // Reset and re-seed the particle filter at the chosen start location. + // The filter may have been initialized earlier from a noisy indoor GPS fix; + // re-seeding here ensures the fused position starts at the user-confirmed location. + particleFilter.reset(); + LatLng chosenStart = new LatLng(startPosition[0], startPosition[1]); + particleFilter.initialise(chosenStart, 5f); // 5 m spread — user just pinpointed their location + filterAnchorTimeMs = SystemClock.elapsedRealtime(); // start 10 s GNSS grace period + lastGnssForFilter = chosenStart; + } + + private boolean hasValidStartLocation() { + return isValidCoordinate(state.startLocation[0], state.startLocation[1]); + } + + private boolean isValidCoordinate(double lat, double lon) { + if (Double.isNaN(lat) || Double.isNaN(lon) + || Double.isInfinite(lat) || Double.isInfinite(lon)) { + return false; + } + if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) { + return false; + } + // Treat (0,0) as unset in this app. + return !(lat == 0.0 && lon == 0.0); + } + + /** + * Seeds initial position from sensors without user interaction. + * Priority: GNSS -> WiFi -> fused particle estimate. + * + * @return true if a valid initial position is available + */ + public boolean initializeStartPositionAutonomously() { + if (hasValidStartLocation()) { + return true; + } + + if (isValidCoordinate(state.latitude, state.longitude)) { + setStartGNSSLatitude(new float[]{state.latitude, state.longitude}); + return true; + } + + LatLng wifiPosition = (wifiPositionManager != null) + ? wifiPositionManager.getLatLngWifiPositioning() + : null; + if (wifiPosition != null + && isValidCoordinate(wifiPosition.latitude, wifiPosition.longitude)) { + setStartGNSSLatitude(new float[]{ + (float) wifiPosition.latitude, + (float) wifiPosition.longitude + }); + return true; + } + + LatLng fusedPosition = particleFilter.getFusedPosition(); + if (fusedPosition != null + && isValidCoordinate(fusedPosition.latitude, fusedPosition.longitude)) { + setStartGNSSLatitude(new float[]{ + (float) fusedPosition.latitude, + (float) fusedPosition.longitude + }); + return true; + } + + return false; + } + + private void autoSeedStartAndMetadataIfNeeded() { + if (!recorder.isRecording() || hasValidStartLocation()) { + return; + } + + if (initializeStartPositionAutonomously()) { + writeInitialMetadata(); + } } /** @@ -623,6 +953,30 @@ public int getWifiFloor() { return wifiPositionManager.getWifiFloor(); } + /** + * Returns true if the most recent WiFi position fix arrived within the last 30 seconds. + * Used by auto-floor logic to avoid permanently bypassing barometric guards once WiFi + * has fired even once during a recording session. + * + * @return true if WiFi position data is fresh enough to trust for floor detection + */ + public boolean isWifiPositionFresh() { + return wifiPositionManager.isWifiPositionFresh(30_000L); + } + + /** + * Returns the magnitude of horizontal filtered acceleration (m/s squared). + * Used by CrossFloorClassifier to distinguish lift (low horizontal movement) + * from stairs (higher horizontal movement) during floor transitions. + * + * @return horizontal acceleration magnitude in m/ss quared + */ + public float getHorizontalAccelMagnitude() { + return (float) Math.sqrt( + Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2) + ); + } + /** * Utility function to log the event frequency of each sensor. */ @@ -630,6 +984,43 @@ public void logSensorFrequencies() { eventHandler.logSensorFrequencies(); } + public ParticleFilter getParticleFilter() { + return particleFilter; + } + + /** Returns true when the device has been stationary for the last sensor window. */ + public boolean isStationary() { + return isStationary; + } + + /** + * Returns the floor number currently tracked by the PDR/barometric pipeline. + * This is the sensor-authoritative floor — updated continuously by barometric + * transitions in {@link #updateFloorLogic()}, independent of what the map display shows. + */ + public int getPdrCurrentFloor() { + return pdrProcessing.getCurrentFloor(); + } + + /** Returns the RMS particle-cloud spread in metres (live position uncertainty). */ + public float getPositionUncertaintyMeters() { + return particleFilter.getPositionUncertaintyMeters(); + } + + // Inject wall polylines (meters) into PDR for collision correction. + + public void setPdrWalls(List> wallPolylines) { //JAPJOT -- i have added this function to convert wall polylines into segments and pass to particle + List segments = new ArrayList<>(); + for (List polyline : wallPolylines) { + for (int i = 0; i < polyline.size() - 1; i++) { + PointF p1 = polyline.get(i); + PointF p2 = polyline.get(i + 1); //convert each pair of consecutive points into a line segment represented as [x1, y1, x2, y2] + segments.add(new float[]{p1.x, p1.y, p2.x, p2.y}); + } + } + particleFilter.setWalls(segments); + } + //endregion //region Location listener @@ -645,6 +1036,80 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + + LatLng gnssPos = new LatLng(location.getLatitude(), location.getLongitude()); + float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; + + // Auto-initialise: first GNSS fix during recording anchors start location and + // writes initial metadata automatically — no manual StartLocationFragment needed. + if (recorder.isRecording() && !initialMetadataWritten) { + state.startLocation[0] = state.latitude; + state.startLocation[1] = state.longitude; + if (!particleFilter.isInitialized()) { + // WiFi hasn't provided a better anchor yet; seed with GNSS + generous spread. + float initSpread = Math.max(accuracy * 4f, 25f); + particleFilter.initialise(gnssPos, initSpread); + lastGnssForFilter = gnssPos; + } + writeInitialMetadata(); + initialMetadataWritten = true; + Log.d("SensorFusion", "Auto-anchored: " + state.latitude + "," + state.longitude); + return; + } + + // Grace period: for 10 s after setStartGNSSLatitude() suppress ALL GNSS so a bad + // indoor GPS fix cannot drag particles away from the confirmed anchor. + if (filterAnchorTimeMs > 0 + && SystemClock.elapsedRealtime() - filterAnchorTimeMs < 10_000L) { + return; + } + + if (!particleFilter.isInitialized()) { + // Inflate spread aggressively for indoor GNSS initialization. + // Reported accuracy is typically 5-15 m, but indoor multipath error is 20-100 m. + // A 25 m minimum spread ensures particles cover enough area for WiFi corrections. + // Ref: Davidson et al. (2010), "Application of particle filters to a map-aided + // indoor positioning system," IEEE/ION PLANS. + float initSpread = Math.max(accuracy * 4f, 25f); + particleFilter.initialise(gnssPos, initSpread); + lastGnssForFilter = gnssPos; + } else { + // Displacement gate: only update if the fix has actually moved enough. + // Suppresses the 10-30 m stationary bounce GPS produces indoors. + float minDisplacement = Math.max(accuracy * 0.5f, 3.0f); + boolean shouldUpdate = (lastGnssForFilter == null) + || (UtilFunctions.distanceBetweenPoints(lastGnssForFilter, gnssPos) + >= minDisplacement); + if (shouldUpdate) { + // Outlier gate: reject spurious GNSS jumps. + // Hard limit: >50 m is physically impossible from GPS; always reject. + // Soft limit: once filter has partially converged (uncertainty < 30 m), + // reject fixes that are >2.5× accuracy away from the fused estimate. + LatLng fused = particleFilter.getFusedPosition(); + boolean outlier = false; + if (fused != null) { + double offset = UtilFunctions.distanceBetweenPoints(fused, gnssPos); + float uncertainty = particleFilter.getPositionUncertaintyMeters(); + if (offset > 50f) { + outlier = true; + Log.d("SensorFusion", "GNSS hard-rejected: " + (int) offset + "m"); + } else if (particleFilter.hasWalls() && offset > 20f) { + outlier = true; + Log.d("SensorFusion", "GNSS indoor-rejected: " + (int) offset + "m"); + } else if (uncertainty < 30f && offset > Math.max(accuracy * 2.5f, 20f)) { + outlier = true; + Log.d("SensorFusion", "GNSS outlier: " + (int) offset + + "m offset, sigma=" + (int) uncertainty + "m"); + } + } + if (!outlier) { + particleFilter.updateGNSS(gnssPos, accuracy); + lastGnssForFilter = gnssPos; + } + } + } + + autoSeedStartAndMetadataIfNeeded(); } } 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..4fc80e7c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -43,6 +43,18 @@ public LatLng getWifiLocation() { // Store user's location obtained using WiFi positioning private LatLng wifiLocation; + + // Timestamp (System.currentTimeMillis) of the most recent successful WiFi fix + private long lastWifiFixTimeMs = 0; + + /** + * Returns the wall-clock time of the most recent WiFi position fix. + * Returns 0 if no fix has been received yet. + */ + public long getWifiLocationTimestampMs() { + return lastWifiFixTimeMs; + } + /** * Getter for the WiFi positioning floor obtained using openpositioning API * @return the user's location based on openpositioning API @@ -89,6 +101,7 @@ public void request(JSONObject jsonWifiFeatures) { try { wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + lastWifiFixTimeMs = System.currentTimeMillis(); } 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 +153,7 @@ 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"); + lastWifiFixTimeMs = System.currentTimeMillis(); 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/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 1edfd68a..887a1765 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -56,8 +56,13 @@ public void update(Object[] wifiList) { /** * Creates a request to obtain a WiFi location for the obtained WiFi fingerprint. + * Skipped if the scan returned no access points (would cause a 422 from the API). */ private void createWifiPositioningRequest() { + if (this.wifiList == null || this.wifiList.isEmpty()) { + Log.d("WiFiDebug", "No APs in scan, skipping API call"); + return; + } try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { @@ -65,6 +70,7 @@ private void createWifiPositioningRequest() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + Log.d("WiFiDebug", "Sending fingerprint with " + this.wifiList.size() + " APs: " + wifiFingerPrint.toString()); this.wiFiPositioning.request(wifiFingerPrint); } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); @@ -82,6 +88,7 @@ private void createWifiPositionRequestCallback() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { @@ -116,6 +123,19 @@ public int getWifiFloor() { return this.wiFiPositioning.getFloor(); } + /** + * Returns true if the most recent WiFi position fix is newer than {@code maxAgeMs} milliseconds. + * Used to prevent stale WiFi fixes from permanently overriding the barometric floor path. + * + * @param maxAgeMs maximum acceptable age of the WiFi fix in milliseconds + * @return true if a fresh fix is available + */ + public boolean isWifiPositionFresh(long maxAgeMs) { + long ts = wiFiPositioning.getWifiLocationTimestampMs(); + if (ts == 0) return false; + return (System.currentTimeMillis() - ts) <= maxAgeMs; + } + /** * Returns the most recent list of WiFi scan results. * diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java index 06f48a46..db6e7ee5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java @@ -14,15 +14,19 @@ */ public class BuildingPolygon { // Defining the coordinates of the building boundaries (rectangular boundaries based on floor map shape) - // North-East and South-West Coordinates for the Nucleus Building - public static final LatLng NUCLEUS_NE=new LatLng(55.92332001571212, -3.1738768212979593); - public static final LatLng NUCLEUS_SW=new LatLng(55.92282257022002, -3.1745956532857647); + // Detection-only shift — leave at 0.0. To move the PNG overlay, use OVERLAY_SHIFT_* in IndoorMapManager instead. + static final double SHIFT_LAT = 0.0; + static final double SHIFT_LNG = 0.0; + + public static final LatLng NUCLEUS_NE=new LatLng(55.92332001571212+SHIFT_LAT, -3.1738768212979593+SHIFT_LNG); + public static final LatLng NUCLEUS_SW=new LatLng(55.92282257022002+SHIFT_LAT, -3.1745956532857647+SHIFT_LNG); + // North-East and South-West Coordinates for the Kenneth and Murray Library Building - public static final LatLng LIBRARY_NE=new LatLng(55.92306692576906, -3.174771893078224); - public static final LatLng LIBRARY_SW=new LatLng(55.92281045664704, -3.175184089079065); + public static final LatLng LIBRARY_NE=new LatLng(55.92306692576906+SHIFT_LAT, -3.174771893078224+SHIFT_LNG); + public static final LatLng LIBRARY_SW=new LatLng(55.92281045664704+SHIFT_LAT, -3.175184089079065+SHIFT_LNG); // North-East and South-West Coordinates for Murchison House - public static final LatLng MURCHISON_NE=new LatLng(55.92240, -3.17150); - public static final LatLng MURCHISON_SW=new LatLng(55.92170, -3.17280); + public static final LatLng MURCHISON_NE=new LatLng(55.92240+SHIFT_LAT, -3.17150+SHIFT_LNG); + public static final LatLng MURCHISON_SW=new LatLng(55.92170+SHIFT_LAT, -3.17280+SHIFT_LNG); // Boundary coordinates of the Nucleus building (clockwise) public static final List NUCLEUS_POLYGON = new ArrayList() {{ diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java b/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java new file mode 100644 index 00000000..c1d40304 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java @@ -0,0 +1,51 @@ +package com.openpositioning.PositionMe.utils; + +/** + * Classifies cross-floor movements into lift or stairs based on displacement and height change. + * Pure function: no side effects, safe for unit testing and future integration. + */ +public final class CrossFloorClassifier { + + public enum Mode { + LIFT, + STAIRS, + UNKNOWN + } + + private CrossFloorClassifier() { + // Utility class + } + + /** + * Decide whether a cross-floor movement is a lift or stairs event. + * + * @param horizontalDelta horizontal displacement in meters. + * @param heightDelta vertical displacement in meters (absolute value is considered). + * @param featureDistance distance to the nearest cross-floor feature in meters (unused for now, + * reserved for tie-breaks). + * @param config thresholds used for classification. + * @return mode describing the movement. + */ + public static Mode classify(double horizontalDelta, + double heightDelta, + double featureDistance, + MapMatchingConfig config) { + double absHeight = Math.abs(heightDelta); + double absHorizontal = Math.abs(horizontalDelta); + + boolean heightSignificant = absHeight >= config.baroHeightThreshold; + if (!heightSignificant) { + return Mode.UNKNOWN; + } + + if (absHorizontal < config.liftHorizontalMax) { + return Mode.LIFT; + } + + if (absHorizontal >= config.stairsHorizontalMin) { + return Mode.STAIRS; + } + + return Mode.UNKNOWN; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..a3dc1c15 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -4,15 +4,21 @@ import android.util.Log; import com.google.android.gms.maps.GoogleMap; +import com.google.android.gms.maps.model.BitmapDescriptorFactory; +import com.google.android.gms.maps.model.GroundOverlay; +import com.google.android.gms.maps.model.GroundOverlayOptions; import com.google.android.gms.maps.model.LatLng; +import com.google.android.gms.maps.model.LatLngBounds; import com.google.android.gms.maps.model.Polygon; import com.google.android.gms.maps.model.PolygonOptions; import com.google.android.gms.maps.model.Polyline; import com.google.android.gms.maps.model.PolylineOptions; +import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.sensors.SensorFusion; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; /** @@ -35,13 +41,41 @@ public class IndoorMapManager { public static final int BUILDING_LIBRARY = 2; public static final int BUILDING_MURCHISON = 3; + // Pre-rendered PNG floor plan images for Nucleus and Library (same as PositionMe_Private) + private static final List NUCLEUS_MAPS = Arrays.asList( + R.drawable.nucleuslg, R.drawable.nucleusg, R.drawable.nucleus1, + R.drawable.nucleus2, R.drawable.nucleus3); + private static final List LIBRARY_MAPS = Arrays.asList( + R.drawable.libraryg, R.drawable.library1, R.drawable.library2, + R.drawable.library3); + + + + // Lat/lng bounds for positioning ground overlay images on the map + static final float OVERLAY_SHIFT_LAT = 0; + static final float OVERLAY_SHIFT_LNG = 0; + private static final LatLngBounds NUCLEUS_BOUNDS = new LatLngBounds( + new LatLng(BuildingPolygon.NUCLEUS_SW.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.NUCLEUS_SW.longitude + OVERLAY_SHIFT_LNG), + new LatLng(BuildingPolygon.NUCLEUS_NE.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.NUCLEUS_NE.longitude + OVERLAY_SHIFT_LNG)); + private static final LatLngBounds LIBRARY_BOUNDS = new LatLngBounds( + new LatLng(BuildingPolygon.LIBRARY_SW.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.LIBRARY_SW.longitude + OVERLAY_SHIFT_LNG), + new LatLng(BuildingPolygon.LIBRARY_NE.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.LIBRARY_NE.longitude + OVERLAY_SHIFT_LNG)); + private GoogleMap gMap; private LatLng currentLocation; private boolean isIndoorMapSet = false; private int currentFloor; private int currentBuilding = BUILDING_NONE; + // Floor height in meters derived from building metadata private float floorHeight; + // Ground overlay for Nucleus and Library (pre-rendered PNG floor plan images) + private GroundOverlay groundOverlay; + // Vector shapes currently drawn on the map (cleared on floor switch or exit) private final List drawnPolygons = new ArrayList<>(); private final List drawnPolylines = new ArrayList<>(); @@ -50,15 +84,21 @@ public class IndoorMapManager { private List currentFloorShapes; // Average floor heights per building (meters), used for barometric auto-floor - public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F; + public static final float NUCLEUS_FLOOR_HEIGHT = 5.5F; public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; - // Colours for different indoor feature types - private static final int WALL_STROKE = Color.argb(200, 80, 80, 80); - private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243); - private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); - private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + // Active trajectory color set by the user via the color toggle button. + // Floor plan shapes are drawn in this color so they stay visually consistent. + private int floorPlanColor = Color.RED; // default matches the default polyline color + + /** Call this whenever the user changes the trajectory polyline color. */ + public void setFloorPlanColor(int color) { + this.floorPlanColor = color; + } + + // Last known user location (lat/lng on map) + private LatLng lastLocation; /** * Constructor to set the map instance. @@ -77,9 +117,15 @@ public IndoorMapManager(GoogleMap map) { */ public void setCurrentLocation(LatLng currentLocation) { this.currentLocation = currentLocation; + this.lastLocation = currentLocation; setBuildingOverlay(); } + /** Returns last location stored via setCurrentLocation (may be null). */ + public LatLng getLastLocation() { + return lastLocation; + } + /** * Returns the current building's floor height. * @@ -117,6 +163,74 @@ public int getCurrentFloor() { return currentFloor; } + /** + * Returns the current floor shapes object, or null if unavailable. + */ + public FloorplanApiClient.FloorShapes getCurrentFloorShape() { + if (currentFloorShapes == null) return null; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return null; + return currentFloorShapes.get(currentFloor); + } + + /** + * Returns true if the last known location is within the given radius (meters) + * of a lift feature on the current floor. + * Used by auto-floor logic to restrict barometric floor changes to lift areas only. + * + * @param radiusMeters proximity threshold in meters + * @return true when near a lift; false otherwise or when data missing + */ + public boolean isNearLift(float radiusMeters) { + if (lastLocation == null || currentFloorShapes == null) return false; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false; + FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor); + if (floor == null || floor.getFeatures() == null) return false; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + if (!"lift".equals(feature.getIndoorType())) continue; + for (List part : feature.getParts()) { + for (LatLng point : part) { + if (UtilFunctions.distanceBetweenPoints(lastLocation, point) <= radiusMeters) { + return true; + } + } + } + } + return false; + } + + /** + * Returns true if the last known location is within the given radius (meters) + * of any stairs or lift feature on the current floor. + * + * @param radiusMeters proximity threshold in meters + * @return true when near stairs/lift; false otherwise or when data missing + */ + public boolean isNearCrossFloorFeature(float radiusMeters) { + if (lastLocation == null || currentFloorShapes == null) return false; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false; + + FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor); + if (floor == null || floor.getFeatures() == null) return false; + + double minDistance = Double.MAX_VALUE; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + if (!"stairs".equals(type) && !"lift".equals(type)) continue; + for (List part : feature.getParts()) { + for (LatLng point : part) { + double d = UtilFunctions.distanceBetweenPoints(lastLocation, point); + if (d < minDistance) { + minDistance = d; + } + if (minDistance <= radiusMeters) { + return true; + } + } + } + } + return false; + } + /** * Returns the display name for the current floor (e.g. "LG", "G", "1"). * Falls back to the numeric index if no display name is available. @@ -129,6 +243,14 @@ public String getCurrentFloorDisplayName() { && currentFloor < currentFloorShapes.size()) { return currentFloorShapes.get(currentFloor).getDisplayName(); } + // Fallback display names for PNG buildings when API data not yet loaded + if (currentBuilding == BUILDING_NUCLEUS) { + String[] names = {"LG", "G", "1", "2", "3"}; + if (currentFloor >= 0 && currentFloor < names.length) return names[currentFloor]; + } else if (currentBuilding == BUILDING_LIBRARY) { + String[] names = {"G", "1", "2", "3"}; + if (currentFloor >= 0 && currentFloor < names.length) return names[currentFloor]; + } return String.valueOf(currentFloor); } @@ -140,14 +262,7 @@ public String getCurrentFloorDisplayName() { * @return the floor index offset for auto-floor conversion */ public int getAutoFloorBias() { - switch (currentBuilding) { - case BUILDING_NUCLEUS: - case BUILDING_MURCHISON: - return 1; // LG at index 0, so G = index 1 - case BUILDING_LIBRARY: - default: - return 0; // G at index 0 - } + return 0; } /** @@ -159,16 +274,18 @@ public int getAutoFloorBias() { * @param autoFloor true if called by auto-floor feature */ public void setCurrentFloor(int newFloor, boolean autoFloor) { - if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; - if (autoFloor) { newFloor += getAutoFloorBias(); } - if (newFloor >= 0 && newFloor < currentFloorShapes.size() - && newFloor != this.currentFloor) { + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + int maxFloor = currentFloorShapes.size(); + if (newFloor >= 0 && newFloor < maxFloor && newFloor != this.currentFloor) { + this.currentFloor = newFloor; + drawFloorShapes(newFloor); + } + } else if (newFloor >= 0 && newFloor != this.currentFloor) { this.currentFloor = newFloor; - drawFloorShapes(newFloor); } } @@ -196,6 +313,13 @@ public void decreaseFloor() { *

Detection priority: floorplan API real polygon outlines first, * then legacy hard-coded rectangular boundaries as fallback.

*/ + /** Redraws the current floor with the latest color settings. */ + public void redrawCurrentFloor() { + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + drawFloorShapes(currentFloor); + } + } + private void setBuildingOverlay() { try { int detected = detectCurrentBuilding(); @@ -208,7 +332,7 @@ private void setBuildingOverlay() { switch (detected) { case BUILDING_NUCLEUS: apiName = "nucleus_building"; - currentFloor = 1; + currentFloor = 0; floorHeight = NUCLEUS_FLOOR_HEIGHT; break; case BUILDING_LIBRARY: @@ -218,27 +342,54 @@ private void setBuildingOverlay() { break; case BUILDING_MURCHISON: apiName = "murchison_house"; - currentFloor = 1; + currentFloor = 0; floorHeight = MURCHISON_FLOOR_HEIGHT; break; default: return; } - // Load floor shapes from cached API data + // Mark as indoors immediately so floor controls, auto-floor, and wall + // collision all activate without waiting for the API response. + isIndoorMapSet = true; + + // Load floor shapes from cached API data if already available FloorplanApiClient.BuildingInfo building = SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { currentFloorShapes = building.getFloorShapesList(); } + // Draw shapes now if available; otherwise they will be drawn when + // setCurrentLocation is called again after the floorplan API responds. if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); - isIndoorMapSet = true; + } + + } else if (inAnyBuilding && isIndoorMapSet + && (currentFloorShapes == null || currentFloorShapes.isEmpty())) { + // Already marked indoors but shapes weren't available yet — try again now + // that the API may have responded. + String apiName2; + switch (currentBuilding) { + case BUILDING_NUCLEUS: apiName2 = "nucleus_building"; break; + case BUILDING_LIBRARY: apiName2 = "library"; break; + case BUILDING_MURCHISON:apiName2 = "murchison_house"; break; + default: apiName2 = null; + } + if (apiName2 != null) { + FloorplanApiClient.BuildingInfo b = + SensorFusion.getInstance().getFloorplanBuilding(apiName2); + if (b != null) { + currentFloorShapes = b.getFloorShapesList(); + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + drawFloorShapes(currentFloor); + } + } } } else if (!inAnyBuilding && isIndoorMapSet) { - clearDrawnShapes(); + clearDrawnShapes(); // also removes groundOverlay isIndoorMapSet = false; currentBuilding = BUILDING_NONE; currentFloor = 0; @@ -249,6 +400,36 @@ private void setBuildingOverlay() { } } + /** + * Displays the pre-rendered PNG floor plan image for Nucleus or Library as a + * ground overlay. Replaces any existing ground overlay. + * + * @param building BUILDING_NUCLEUS or BUILDING_LIBRARY + * @param floorIndex floor index matching the NUCLEUS_MAPS / LIBRARY_MAPS list + */ + private void showGroundOverlay(int building, int floorIndex) { + if (groundOverlay != null) { + groundOverlay.remove(); + groundOverlay = null; + } + List maps; + LatLngBounds bounds; + if (building == BUILDING_NUCLEUS) { + maps = NUCLEUS_MAPS; + bounds = NUCLEUS_BOUNDS; + } else if (building == BUILDING_LIBRARY) { + maps = LIBRARY_MAPS; + bounds = LIBRARY_BOUNDS; + } else { + return; + } + if (floorIndex < 0 || floorIndex >= maps.size()) return; + groundOverlay = gMap.addGroundOverlay(new GroundOverlayOptions() + .image(BitmapDescriptorFactory.fromResource(maps.get(floorIndex))) + .positionFromBounds(bounds) + .transparency(0.1f)); // slight transparency so satellite map is visible underneath + } + /** * Draws all vector shapes for the given floor index on the Google Map. * Clears any previously drawn shapes before drawing the new floor. @@ -263,17 +444,23 @@ private void drawFloorShapes(int floorIndex) { FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex); for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { - String geoType = feature.getGeometryType(); + String geoType = feature.getGeometryType(); String indoorType = feature.getIndoorType(); + // Prefer API-provided colours; fall back to type-based defaults + int stroke = parseApiColor(feature.getStrokeColor(), getStrokeColor(indoorType)); + int fill = parseApiColor(feature.getFillColor(), getFillColor(indoorType)); + float width = feature.getStrokeWidth() > 0 + ? Math.max(feature.getStrokeWidth(), 4f) : 5f; + if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { for (List ring : feature.getParts()) { if (ring.size() < 3) continue; Polygon p = gMap.addPolygon(new PolygonOptions() .addAll(ring) - .strokeColor(getStrokeColor(indoorType)) - .strokeWidth(5f) - .fillColor(getFillColor(indoorType))); + .strokeColor(stroke) + .strokeWidth(width) + .fillColor(fill)); drawnPolygons.add(p); } } else if ("MultiLineString".equals(geoType) @@ -282,14 +469,27 @@ private void drawFloorShapes(int floorIndex) { if (line.size() < 2) continue; Polyline pl = gMap.addPolyline(new PolylineOptions() .addAll(line) - .color(getStrokeColor(indoorType)) - .width(6f)); + .color(stroke) + .width(width)); drawnPolylines.add(pl); } } } } + /** + * Parses an API colour string (e.g. "#666666") to an ARGB int. + * Returns {@code fallback} if the string is null, empty, or unparseable. + */ + private int parseApiColor(String colorStr, int fallback) { + if (colorStr == null || colorStr.trim().isEmpty()) return fallback; + try { + return Color.parseColor(colorStr.trim()); + } catch (IllegalArgumentException ex) { + return fallback; + } + } + /** * Removes all vector shapes currently drawn on the map. */ @@ -298,18 +498,29 @@ private void clearDrawnShapes() { for (Polyline p : drawnPolylines) p.remove(); drawnPolygons.clear(); drawnPolylines.clear(); + if (groundOverlay != null) { + groundOverlay.remove(); + groundOverlay = null; + } } /** * Returns the stroke colour for a given indoor feature type. + * Feature colours are fixed regardless of the user's trajectory colour choice: + * * @param indoorType the indoor_type property value * @return ARGB colour value */ private int getStrokeColor(String indoorType) { - if ("wall".equals(indoorType)) return WALL_STROKE; - if ("room".equals(indoorType)) return ROOM_STROKE; - return DEFAULT_STROKE; + if ("wall".equals(indoorType)) return Color.argb(220, 220, 30, 30); // red + if ("stairs".equals(indoorType)) return Color.argb(220, 220, 180, 0); // yellow + if ("lift".equals(indoorType)) return Color.argb(220, 30, 180, 30); // green + // room / other use the user-chosen trajectory colour + int r = Color.red(floorPlanColor); + int g = Color.green(floorPlanColor); + int b = Color.blue(floorPlanColor); + return Color.argb(160, r, g, b); } /** @@ -319,7 +530,15 @@ private int getStrokeColor(String indoorType) { * @return ARGB colour value */ private int getFillColor(String indoorType) { - if ("room".equals(indoorType)) return ROOM_FILL; + if ("wall".equals(indoorType)) return Color.argb( 60, 220, 30, 30); // faint red + if ("stairs".equals(indoorType)) return Color.argb( 60, 220, 180, 0); // faint yellow + if ("lift".equals(indoorType)) return Color.argb( 60, 30, 180, 30); // faint green + if ("room".equals(indoorType)) { + int r = Color.red(floorPlanColor); + int g = Color.green(floorPlanColor); + int b = Color.blue(floorPlanColor); + return Color.argb(40, r, g, b); + } return Color.TRANSPARENT; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java new file mode 100644 index 00000000..258ccb9a --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java @@ -0,0 +1,41 @@ +package com.openpositioning.PositionMe.utils; + +/** + * Centralized defaults for map-matching parameters. + * Values are in meters unless stated otherwise. + * No behavior change is made by instantiating this class; it is a holder for later tuning. + */ +public class MapMatchingConfig { + + /** Minimum vertical delta from barometer to consider a floor change (meters). */ + public final float baroHeightThreshold; + + /** Maximum horizontal displacement to still classify a cross-floor move as lift (meters). */ + public final float liftHorizontalMax; + + /** Minimum horizontal displacement to classify a cross-floor move as stairs (meters). */ + public final float stairsHorizontalMin; + + /** Padding distance to keep corrected paths away from walls (meters). */ + public final float wallPadding; + + /** Proximity radius to accept cross-floor features (meters). */ + public final float crossFeatureProximity; + + public MapMatchingConfig() { + // Defaults: Nucleus floor height 5.5 m, 3 m baro threshold, 10 m feature proximity + this(3.0f, 1.5f, 2.0f, 0.2f, 10.0f); + } + + public MapMatchingConfig(float baroHeightThreshold, + float liftHorizontalMax, + float stairsHorizontalMin, + float wallPadding, + float crossFeatureProximity) { + this.baroHeightThreshold = baroHeightThreshold; + this.liftHorizontalMax = liftHorizontalMax; + this.stairsHorizontalMin = stairsHorizontalMin; + this.wallPadding = wallPadding; + this.crossFeatureProximity = crossFeatureProximity; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java new file mode 100644 index 00000000..c101fe78 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java @@ -0,0 +1,171 @@ +package com.openpositioning.PositionMe.utils; + +import java.util.*; +import java.util.concurrent.atomic.AtomicReference; + +public class PathRouter { + + private static final float EPS = 0.05f; + + private static final class Graph { + final float[][] nodes; + final float[][] adj; + final List solid; + final List trans; + Graph(float[][] nodes, float[][] adj, List solid, List trans) { + this.nodes = nodes; this.adj = adj; this.solid = solid; this.trans = trans; + } + int n() { return nodes.length; } + } + + private final AtomicReference graphRef = new AtomicReference<>(); + + public void rebuild(List solid, List transitions) { + List s = solid != null ? new ArrayList<>(solid) : Collections.emptyList(); + List t = transitions != null ? new ArrayList<>(transitions) : Collections.emptyList(); + new Thread(() -> graphRef.set(buildGraph(s, t))).start(); + } + + public List route(float[] start, float[] end, boolean floorChanged) { + Graph g = graphRef.get(); + if (g == null) return Collections.singletonList(end); + List active = floorChanged ? g.solid : concat(g.solid, g.trans); + if (!crossed(start[0], start[1], end[0], end[1], active)) { + return Collections.singletonList(end); + } + return astar(g, start, end, active, !floorChanged && !g.trans.isEmpty()); + } + + private static Graph buildGraph(List solid, List trans) { + List verts = dedup(concat(solid, trans)); + int n = verts.size(); + float[][] nodes = verts.toArray(new float[0][]); + float[][] adj = new float[n][n]; + for (float[] r : adj) Arrays.fill(r, -1f); + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { + if (!crossed(nodes[i][0], nodes[i][1], nodes[j][0], nodes[j][1], solid)) { + float d = dst(nodes[i], nodes[j]); + adj[i][j] = adj[j][i] = d; + } + } + } + return new Graph(nodes, adj, solid, trans); + } + + private static List astar(Graph g, float[] start, float[] end, + List active, boolean checkTrans) { + int n = g.n(), S = n, E = n + 1, T = n + 2; + float[] gs = fill(T), fs = fill(T); + int[] prev = new int[T]; + boolean[] closed = new boolean[T]; + Arrays.fill(prev, -1); + gs[S] = 0f; + fs[S] = dst(start, end); + PriorityQueue open = new PriorityQueue<>(Comparator.comparingDouble(i -> fs[i])); + open.add(S); + while (!open.isEmpty()) { + int cur = open.poll(); + if (closed[cur]) continue; + closed[cur] = true; + if (cur == E) return buildPath(prev, S, E, n, g, start, end); + for (int nb = 0; nb < T; nb++) { + if (closed[nb]) continue; + float w = ew(g, cur, nb, S, E, start, end, active, checkTrans); + if (w < 0) continue; + float ng = gs[cur] + w; + if (ng < gs[nb]) { + gs[nb] = ng; + fs[nb] = ng + dst(pt(g, nb, S, E, start, end), end); + prev[nb] = cur; + open.add(nb); + } + } + } + return Collections.singletonList(end); + } + + private static float ew(Graph g, int a, int b, int S, int E, + float[] start, float[] end, List active, boolean checkTrans) { + float[] pa = pt(g, a, S, E, start, end); + float[] pb = pt(g, b, S, E, start, end); + if (a < g.n() && b < g.n()) { + if (g.adj[a][b] < 0) return -1f; + if (checkTrans && crossed(pa[0], pa[1], pb[0], pb[1], g.trans)) return -1f; + return g.adj[a][b]; + } + return crossed(pa[0], pa[1], pb[0], pb[1], active) ? -1f : dst(pa, pb); + } + + private static float[] pt(Graph g, int i, int S, int E, float[] s, float[] e) { + if (i == S) return s; + if (i == E) return e; + return g.nodes[i]; + } + + private static List buildPath(int[] prev, int S, int E, int n, + Graph g, float[] s, float[] e) { + List path = new ArrayList<>(); + for (int cur = E; cur != S && cur != -1; cur = prev[cur]) { + path.add(0, pt(g, cur, S, E, s, e)); + } + return path.isEmpty() ? Collections.singletonList(e) : path; + } + + private static boolean crossed(float ax, float ay, float bx, float by, List walls) { + for (float[] w : walls) { + if (coin(ax, ay, w[0], w[1]) || coin(ax, ay, w[2], w[3]) || + coin(bx, by, w[0], w[1]) || coin(bx, by, w[2], w[3])) continue; + if (seg(ax, ay, bx, by, w[0], w[1], w[2], w[3])) return true; + } + return false; + } + + private static boolean coin(float ax, float ay, float bx, float by) { + return Math.abs(ax - bx) < EPS && Math.abs(ay - by) < EPS; + } + + private static boolean seg(float ax, float ay, float bx, float by, + float cx, float cy, float dx, float dy) { + float d1 = ccw(cx, cy, dx, dy, ax, ay), d2 = ccw(cx, cy, dx, dy, bx, by); + float d3 = ccw(ax, ay, bx, by, cx, cy), d4 = ccw(ax, ay, bx, by, dx, dy); + return ((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) + && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0)); + } + + private static float ccw(float ox, float oy, float ax, float ay, float bx, float by) { + return (ax - ox) * (by - oy) - (ay - oy) * (bx - ox); + } + + private static float dst(float[] a, float[] b) { + float dx = b[0] - a[0], dy = b[1] - a[1]; + return (float) Math.sqrt(dx * dx + dy * dy); + } + + private static List dedup(List segs) { + List list = new ArrayList<>(); + Set seen = new HashSet<>(); + for (float[] seg : segs) { + add(seg[0], seg[1], list, seen); + add(seg[2], seg[3], list, seen); + } + return list; + } + + private static void add(float x, float y, List list, Set seen) { + long key = (long) Math.round(x / 0.05f) * 4_000_000L + (long) Math.round(y / 0.05f); + if (seen.add(key)) list.add(new float[]{x, y}); + } + + private static List concat(List a, List b) { + List r = new ArrayList<>(a); + r.addAll(b); + return r; + } + + private static float[] fill(int n) { + float[] a = new float[n]; + Arrays.fill(a, Float.MAX_VALUE); + return a; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java index 5a5efa8d..c9d2ea11 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java @@ -102,8 +102,8 @@ protected void onDraw(Canvas canvas) { // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -134,8 +134,8 @@ else if(reDraw){ // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -153,8 +153,8 @@ else if(reDraw){ // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -174,6 +174,8 @@ public void drawTrajectory(float[] newCords) { // Negate the y coordinate and add it to the yCoords list, since screen coordinates // start from top to bottom yCoords.add(-newCords[1]); + // postInvalidate() is thread-safe and schedules a redraw on the UI thread + postInvalidate(); } /** @@ -186,20 +188,27 @@ private void scaleTrajectory() { int centerX = getWidth() / 2; int centerY = getHeight() / 2; - // Calculate the scaling that would be required in each direction - float xRightRange = (getWidth() / 2) / (Math.abs(Collections.max(xCoords))); - float xLeftRange = (getWidth() / 2) / (Math.abs(Collections.min(xCoords))); - float yTopRange = (getHeight() / 2) / (Math.abs(Collections.max(yCoords))); - float yBottomRange = (getHeight() / 2) / (Math.abs(Collections.min(yCoords))); + // Compute the maximum absolute extent from the origin in each axis. + // Using max(|max|, |min|) avoids division-by-zero when the walk is entirely + // on one side of the origin (e.g. all-positive X during the first leg of a square). + float xExtent = Math.max(Math.abs(Collections.max(xCoords)), + Math.abs(Collections.min(xCoords))); + float yExtent = Math.max(Math.abs(Collections.max(yCoords)), + Math.abs(Collections.min(yCoords))); + + // Fallback to 1 if the trajectory has no movement in an axis (prevents /0) + if (xExtent == 0) xExtent = 1f; + if (yExtent == 0) yExtent = 1f; // Take the minimum scaling ratio to ensure all points fit within the view - float minRatio = Math.min(Math.min(xRightRange, xLeftRange), Math.min(yTopRange, yBottomRange)); + float minRatio = Math.min((getWidth() / 2f) / xExtent, + (getHeight() / 2f) / yExtent); // Add margins to the scaling ratio scalingRatio = 0.9f * minRatio; // Limit scaling ratio to an equivalent of zoom of 21 in google maps - if (scalingRatio >= 23.926) { + if (scalingRatio >= 23.926f) { scalingRatio = 23.926f; } System.out.println("Adjusted scaling ratio: " + scalingRatio); 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..6cece6ef 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -17,9 +17,8 @@ /** * Processes data recorded in the {@link SensorFusion} class and calculates live PDR estimates. - * It calculates the position from the steps and directions detected, using either estimated values - * (eg. stride length from the Weiberg algorithm) or provided constants, calculates the elevation - * and attempts to estimate the current floor as well as elevators. + * * Coordinates are relative to the session start, in meters (x east, y north). Elevation is + * relative to the initial barometer median. Attempts to estimate current floor and elevator use. * * @author Mate Stodulka * @author Michal Dvorak @@ -44,6 +43,10 @@ public class PdrProcessing { // Settings for accessing shared variables private SharedPreferences settings; + + // Centralized map-matching thresholds (currently informational only) + private final MapMatchingConfig mapMatchingConfig; + // Step length private float stepLength; // Using manually input constants instead of estimated values @@ -53,11 +56,16 @@ public class PdrProcessing { private float positionX; private float positionY; + // Optional wall geometry in meters (polylines) + //private List> walls; + // Vertical movement calculation private Float[] startElevationBuffer; private float startElevation; private int setupIndex = 0; private float elevation; + + // Floor-to-floor height in meters (manual setting) private int floorHeight; private int currentFloor; @@ -68,9 +76,15 @@ public class PdrProcessing { private CircularFloatBuffer verticalAccel; private CircularFloatBuffer horizontalAccel; - // Step sum and length aggregation variables private float sumStepLength = 0; private int stepCount = 0; + + private float kfStepLength = 0.7f; + private float kfVariance = 0.1f; + private static final float KF_PROCESS_NOISE = 0.005f; + private static final float KF_MEAS_NOISE = 0.04f; + private static final float MIN_STEP_LENGTH = 0.2f; + private static final float MAX_STEP_LENGTH = 1.5f; //endregion /** @@ -84,6 +98,10 @@ public PdrProcessing(Context context) { this.settings = PreferenceManager.getDefaultSharedPreferences(context); // Check if estimate or manual values should be used this.useManualStep = this.settings.getBoolean("manual_step_values", false); + + // Initialize map-matching configuration with defaults + this.mapMatchingConfig = new MapMatchingConfig(); + if(useManualStep) { try { // Retrieve manual step length @@ -123,11 +141,12 @@ public PdrProcessing(Context context) { } // Distance between floors is building dependent, use manual value - this.floorHeight = settings.getInt("floor_height", 4); + this.floorHeight = settings.getInt("floor_height", 4); // Array for holding initial values this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + } /** @@ -154,12 +173,14 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim return new float[]{this.positionX, this.positionY}; } - // Calculate step length - if(!useManualStep) { - //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); - // Estimate stride - this.stepLength = weibergMinMax(accelMagnitudeOvertime); - // System.err.println("Step Length" + stepLength); + if (!useManualStep) { + float rawStep = weibergMinMax(accelMagnitudeOvertime); + rawStep = Math.max(MIN_STEP_LENGTH, Math.min(MAX_STEP_LENGTH, rawStep)); + kfVariance += KF_PROCESS_NOISE; + float gain = kfVariance / (kfVariance + KF_MEAS_NOISE); + kfStepLength += gain * (rawStep - kfStepLength); + kfVariance = (1f - gain) * kfVariance; + this.stepLength = kfStepLength; } // Increment aggregate variables @@ -205,20 +226,6 @@ public float updateElevation(float absoluteElevation) { // Add to buffer this.elevationList.putNewest(absoluteElevation); - // Check if there was floor movement - // Check if there is enough data to evaluate - if(this.elevationList.isFull()) { - // Check average of elevation array - List elevationMemory = this.elevationList.getListCopy(); - OptionalDouble currentAvg = elevationMemory.stream().mapToDouble(f -> f).average(); - float finishAvg = currentAvg.isPresent() ? (float) currentAvg.getAsDouble() : 0; - - // Check if we moved floor by comparing with start position - if(Math.abs(finishAvg - startElevation) > this.floorHeight) { - // Change floors - 'floor' division - this.currentFloor += (finishAvg - startElevation)/this.floorHeight; - } - } // Return current elevation return elevation; } @@ -250,8 +257,7 @@ private float weibergMinMax(List accelMagnitude) { double maxAccel = Collections.max(validAccel); double minAccel = Collections.min(validAccel); - // calculate bounce - float bounce = (float) Math.pow((maxAccel - minAccel), 0.25); + float bounce = (float) Math.pow(Math.max(0.001, maxAccel - minAccel), 0.25); // determine which constant to use based on settings if (this.settings.getBoolean("overwrite_constants", false)) { @@ -282,6 +288,14 @@ public float getCurrentElevation() { return this.elevation; } + public float getStartElevation() { + return this.startElevation; + } + + public CircularFloatBuffer getElevationList() { + return this.elevationList; + } + /** * Get the current floor number as estimated by the PDR class. * @@ -291,6 +305,14 @@ public int getCurrentFloor() { return this.currentFloor; } + public void setCurrentFloor(int floor) { + this.currentFloor = floor; + } + + public int getFloorHeightValue() { + return this.floorHeight; + } + /** * Estimates if the user is currently taking an elevator. * From the gravity and gravity-removed acceleration values the magnitude of horizontal and @@ -395,6 +417,12 @@ public void resetPDR() { this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + this.setupIndex = 0; + this.startElevation = 0f; + this.kfStepLength = 0.7f; + this.kfVariance = 0.1f; + this.sumStepLength = 0f; + this.stepCount = 0; } /** @@ -403,14 +431,10 @@ public void resetPDR() { * @return average step length in meters. */ public float getAverageStepLength(){ - //Calculate average step length - float averageStepLength = sumStepLength/(float) stepCount; - - //Reset sum and number of steps + if (stepCount == 0) return 0.75f; + float averageStepLength = sumStepLength / (float) stepCount; stepCount = 0; sumStepLength = 0; - - //Return average step length return averageStepLength; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java index 56a88aa3..c7dfe576 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java @@ -16,7 +16,43 @@ */ public class UtilFunctions { // Constant 1degree of latitiude/longitude (in m) - private static final int DEGREE_IN_M=111111; + private static final double EARTH_RADIUS = 6371000.0; // in meters + + public static float[] convertWGS84ToENU(LatLng origin, LatLng point){ + //convert to radians + + double lat1 = Math.toRadians(origin.latitude); + double lon1 = Math.toRadians(origin.longitude); + double lat2 = Math.toRadians(point.latitude); + double lon2 = Math.toRadians(point.longitude); + + //calculate easting + double easting = EARTH_RADIUS * (lon2 - lon1) * Math.cos(lat1); + //calculate northing + double northing = EARTH_RADIUS * (lat2 - lat1); + //calculate up + double up = 0; // Assuming flat surface + + return new float[]{(float) easting, (float) northing, (float) up}; + } + + public static LatLng convertENUToWGS84(LatLng origin, float[] enu){ + //convert to radians + double lat1 = Math.toRadians(origin.latitude); + double lon1 = Math.toRadians(origin.longitude); + + //calculate latitude + double lat2 = lat1 + (enu[1] / EARTH_RADIUS); + //calculate longitude + double lon2 = lon1 + (enu[0] / (EARTH_RADIUS * Math.cos(lat1))); + //calculate up (not used in this case) + double up = 0; // Assuming flat surface + + return new LatLng(Math.toDegrees(lat2), Math.toDegrees(lon2)); + } + + + private static final int DEGREE_IN_M=111111; //NOT NEEDED FOR CONVERSION, JUST USED AS A CONSTANT TO CALCULATE NEW COORDINATES /** * Simple function to calculate the angle between two close points * @param pointA Starting point @@ -61,7 +97,7 @@ public static double degreesToMetersLat(double degreeVal) { * @return double corresponding to the value in meters. */ public static double degreesToMetersLng(double degreeVal, double latitude) { - return degreeVal*DEGREE_IN_M/Math.cos(Math.toRadians(latitude)); + return degreeVal * DEGREE_IN_M * Math.cos(Math.toRadians(latitude)); } /** diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java new file mode 100644 index 00000000..cd4662ae --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java @@ -0,0 +1,69 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import java.util.List; + +/** + * Adjusts proposed 2D steps to avoid crossing walls. + * Uses simple segment intersection: if the step crosses any wall segment, movement is cancelled. + */ +public final class WallCollisionCorrector { + + private WallCollisionCorrector() { + // Utility class + } + + /** + * Returns a corrected point that avoids crossing wall segments. + * + * @param previous previous position (meters). + * @param candidate proposed next position (meters). + * @param walls list of wall polylines; each polyline is a list of points in meters. + * @return corrected point (candidate or previous when blocked). + */ + public static PointF correct(PointF previous, + PointF candidate, + List> walls) { + if (walls == null || walls.isEmpty()) { + return candidate; + } + for (List wall : walls) { + if (wall == null || wall.size() < 2) continue; + for (int i = 0; i < wall.size() - 1; i++) { + PointF a = wall.get(i); + PointF b = wall.get(i + 1); + if (segmentsIntersect(previous, candidate, a, b)) { + // Block movement; return previous position + return previous; + } + } + } + return candidate; + } + + private static boolean segmentsIntersect(PointF p1, PointF p2, PointF q1, PointF q2) { + float d1 = direction(q1, q2, p1); + float d2 = direction(q1, q2, p2); + float d3 = direction(p1, p2, q1); + float d4 = direction(p1, p2, q2); + + if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) && + ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) { + return true; + } + return d1 == 0 && onSegment(q1, q2, p1) + || d2 == 0 && onSegment(q1, q2, p2) + || d3 == 0 && onSegment(p1, p2, q1) + || d4 == 0 && onSegment(p1, p2, q2); + } + + private static float direction(PointF a, PointF b, PointF c) { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); + } + + private static boolean onSegment(PointF a, PointF b, PointF c) { + return Math.min(a.x, b.x) <= c.x && c.x <= Math.max(a.x, b.x) + && Math.min(a.y, b.y) <= c.y && c.y <= Math.max(a.y, b.y); + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java new file mode 100644 index 00000000..5f8bf611 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java @@ -0,0 +1,58 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; + +import java.util.ArrayList; +import java.util.List; + +/** + * Builds meter-based wall polylines from floorplan features, relative to an origin. + */ +public final class WallGeometryBuilder { + + private WallGeometryBuilder() {} + + /** + * Converts wall features (LineString/MultiLineString/Polygon rings) to local meter polylines. + * + * @param floor floor data containing features. + * @param origin reference lat/lng (e.g., initial PDR position) for local frame. + * @return list of wall polylines; empty if none. + */ + public static List> buildWalls(FloorplanApiClient.FloorShapes floor, LatLng origin) { + return buildByType(floor, origin, "wall"); + } + + public static List> buildTransitions(FloorplanApiClient.FloorShapes floor, LatLng origin) { + List> result = new ArrayList<>(); + result.addAll(buildByType(floor, origin, "stairs")); + result.addAll(buildByType(floor, origin, "lift")); + return result; + } + + private static List> buildByType(FloorplanApiClient.FloorShapes floor, + LatLng origin, String type) { + List> result = new ArrayList<>(); + if (floor == null || origin == null) return result; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + if (!type.equals(feature.getIndoorType())) continue; + for (List part : feature.getParts()) { + if (part == null || part.size() < 2) continue; + List line = new ArrayList<>(part.size()); + for (LatLng p : part) { + float dx = (float) UtilFunctions.degreesToMetersLng(p.longitude - origin.longitude, origin.latitude); + float dy = (float) UtilFunctions.degreesToMetersLat(p.latitude - origin.latitude); + line.add(new PointF(dx, dy)); + } + if (!line.isEmpty() && !line.get(0).equals(line.get(line.size() - 1))) { + line.add(new PointF(line.get(0).x, line.get(0).y)); + } + result.add(line); + } + } + return result; + } +} diff --git a/app/src/main/res/layout/fragment_home.xml b/app/src/main/res/layout/fragment_home.xml index 661afbb3..3e6fcffa 100644 --- a/app/src/main/res/layout/fragment_home.xml +++ b/app/src/main/res/layout/fragment_home.xml @@ -11,7 +11,7 @@ android:layout_width="match_parent" android:layout_height="match_parent" android:padding="24dp" - android:background="@color/md_theme_background"> + android:background="#504CAF50"> @@ -36,7 +36,7 @@ android:textSize="14sp" android:textStyle="italic" android:textAlignment="center" - android:textColor="@color/md_theme_primary" + android:textColor="#086108" app:layout_constraintTop_toBottomOf="@id/pageTitle" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" @@ -90,7 +90,7 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_primary" + android:backgroundTint="#52D557" android:text="@string/start_recording" android:textColor="@color/md_theme_onPrimary" app:cornerRadius="16dp" @@ -107,9 +107,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_light_primary" + android:backgroundTint="#8BC34A" android:text="@string/files" - android:textColor="@color/md_theme_light_onPrimary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_folder_24" app:iconGravity="textTop" @@ -124,9 +124,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_tertiary" + android:backgroundTint="#1E5420" android:text="@string/info" - android:textColor="@color/md_theme_onTertiary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_info_24" app:iconGravity="textTop" @@ -141,9 +141,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_tertiary" + android:backgroundTint="#464D07" android:text="@string/measurements" - android:textColor="@color/md_theme_onTertiary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_data_array_24" app:iconGravity="textTop" diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..a0301d25 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -31,37 +31,81 @@ android:padding="8dp" android:gravity="center"> - + android:text="Settings" + android:backgroundTint="#FFCDD2" + android:textColor="@android:color/black" + android:textStyle="bold" + app:icon="@android:drawable/ic_menu_manage" + app:iconTint="@android:color/black" + app:iconPadding="6dp" + app:elevation="5dp" /> - + android:orientation="vertical" + android:gravity="center" + android:visibility="gone"> - + - + + + + + + + + + + + + diff --git a/app/src/main/res/values/strings.xml b/app/src/main/res/values/strings.xml index 125d27c3..04a14db1 100644 --- a/app/src/main/res/values/strings.xml +++ b/app/src/main/res/values/strings.xml @@ -113,7 +113,9 @@ Default building assumptions Floor height in meters Color - 🛰 Show GNSS + 🛰🔵 Show GNSS + 📶🟢 Show WiFi + 🚶🔴 Show PDR Floor Down button Floor Up button Choose Map diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java new file mode 100644 index 00000000..a7f9c042 --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java @@ -0,0 +1,34 @@ +package com.openpositioning.PositionMe.utils; + +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class CrossFloorClassifierTest { + + private final MapMatchingConfig config = new MapMatchingConfig(4.0f, 1.5f, 2.0f, 0.2f, 5.0f); + + @Test + public void classifyReturnsLiftWhenHorizontalIsSmallAndHeightBig() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(0.5, 4.5, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.LIFT, mode); + } + + @Test + public void classifyReturnsStairsWhenHorizontalIsLargeAndHeightBig() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(3.0, 4.5, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.STAIRS, mode); + } + + @Test + public void classifyReturnsUnknownWhenHeightIsTooSmall() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(0.5, 1.0, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.UNKNOWN, mode); + } + + @Test + public void classifyReturnsUnknownWhenBetweenLiftAndStairsThresholds() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(1.6, 5.0, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.UNKNOWN, mode); + } +} diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java new file mode 100644 index 00000000..cf5f3190 --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java @@ -0,0 +1,30 @@ +package com.openpositioning.PositionMe.utils; + +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class MapMatchingConfigTest { + + @Test + public void defaultsAreStable() { + MapMatchingConfig config = new MapMatchingConfig(); + + assertEquals(4.0f, config.baroHeightThreshold, 0.0001f); + assertEquals(1.5f, config.liftHorizontalMax, 0.0001f); + assertEquals(2.0f, config.stairsHorizontalMin, 0.0001f); + assertEquals(0.2f, config.wallPadding, 0.0001f); + assertEquals(5.0f, config.crossFeatureProximity, 0.0001f); + } + + @Test + public void customValuesAreApplied() { + MapMatchingConfig config = new MapMatchingConfig(3.0f, 1.0f, 2.5f, 0.1f, 7.0f); + + assertEquals(3.0f, config.baroHeightThreshold, 0.0001f); + assertEquals(1.0f, config.liftHorizontalMax, 0.0001f); + assertEquals(2.5f, config.stairsHorizontalMin, 0.0001f); + assertEquals(0.1f, config.wallPadding, 0.0001f); + assertEquals(7.0f, config.crossFeatureProximity, 0.0001f); + } +} diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java new file mode 100644 index 00000000..5f71a94c --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java @@ -0,0 +1,38 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import org.junit.Test; + +import java.util.Arrays; +import java.util.List; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertSame; + +public class WallCollisionCorrectorTest { + + @Test + public void stubCorrectionReturnsCandidate() { + PointF prev = new PointF(0f, 0f); + PointF cand = new PointF(5f, 5f); + PointF w1 = new PointF(10f, 0f); + PointF w2 = new PointF(10f, 10f); + + PointF corrected = WallCollisionCorrector.correct(prev, cand, Arrays.asList(Arrays.asList(w1, w2))); + + assertEquals(cand.x, corrected.x, 0.0001f); + assertEquals(cand.y, corrected.y, 0.0001f); + } + + @Test + public void crossingWallReturnsPrevious() { + PointF prev = new PointF(0f, 0f); + PointF cand = new PointF(10f, 0f); + List wall = Arrays.asList(new PointF(5f, -5f), new PointF(5f, 5f)); + + PointF corrected = WallCollisionCorrector.correct(prev, cand, Arrays.asList(wall)); + + assertSame(prev, corrected); + } +} diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/secrets.properties b/secrets.properties index f0dc54fd..649224a0 100644 --- a/secrets.properties +++ b/secrets.properties @@ -1,6 +1,6 @@ # # Modify the variables to set your keys # -MAPS_API_KEY= -OPENPOSITIONING_API_KEY= -OPENPOSITIONING_MASTER_KEY= +MAPS_API_KEY=AIzaSyBOCzNnbeqjbVS9DIIAnJWmn1ovlNoZwiY +OPENPOSITIONING_API_KEY=1v8fHGQ-Eu1q42j_3D_2nw +OPENPOSITIONING_MASTER_KEY=ewireless