Skip to content

Group4 code pull request #114

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JinwenYe0124 wants to merge 2 commits into
base: develop
Choose a base branch
from
Open

Group4 code pull request #114

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
03f2c31
1
JinwenYe0124 Apr 1, 2026
0eb605a
Merge remote-tracking branch 'origin/develop' into develop
JinwenYe0124 Apr 1, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions .gitattributes
View file
Open in desktop

This file was deleted.

16 changes: 0 additions & 16 deletions .gitignore
View file
Open in desktop

This file was deleted.

48 changes: 46 additions & 2 deletions app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.Iterator;
import java.util.List;

Expand Down Expand Up @@ -372,13 +373,56 @@ private List<FloorShapes> parseMapShapes(String mapShapesJson) {
while (it.hasNext()) {
keys.add(it.next());
}
Collections.sort(keys);
// Sort floor keys in building-logical order:
// Basement levels (B*) first, then Ground (G*), then upper floors (F1, F2, ...)
Collections.sort(keys, new Comparator<String>() {
@Override
public int compare(String a, String b) {
return floorKeyOrder(a) - floorKeyOrder(b);
}

private int floorKeyOrder(String key) {
String upper = key.toUpperCase();
// Basement levels: B1=-2, B2=-3, etc.
if (upper.startsWith("B")) {
try {
return -Integer.parseInt(upper.substring(1)) - 1;
} catch (NumberFormatException e) {
return -1;
}
}
// Ground floor
if (upper.startsWith("G")) return 0;
// Upper floors: F1=1, F2=2, etc.
if (upper.startsWith("F")) {
try {
return Integer.parseInt(upper.substring(1));
} catch (NumberFormatException e) {
return 100;
}
}
// Pure numeric keys
try {
return Integer.parseInt(upper);
} catch (NumberFormatException e) {
return 100;
}
}
});

for (String key : keys) {
Log.e(TAG, "FLOOR_PARSE: building floor keys (sorted): " + keys);

for (int idx = 0; idx < keys.size(); idx++) {
String key = keys.get(idx);
JSONObject floorCollection = root.getJSONObject(key);
String displayName = floorCollection.optString("name", key);
JSONArray features = floorCollection.optJSONArray("features");

Log.e(TAG, "FLOOR_PARSE: index=" + idx
+ " | key=\"" + key + "\""
+ " | displayName=\"" + displayName + "\""
+ " | features=" + (features != null ? features.length() : 0));

List<MapShapeFeature> shapeFeatures = new ArrayList<>();
if (features != null) {
for (int i = 0; i < features.length(); i++) {
Expand Down
166 changes: 150 additions & 16 deletions ...src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
import java.util.ArrayList;
import java.util.List;

import android.util.Log;
import android.widget.Toast;


Expand Down Expand Up @@ -65,6 +66,8 @@

public class RecordingFragment extends Fragment {

private static final String TAG = "RECORDING_DEBUG";

// UI elements
private MaterialButton completeButton, cancelButton;
private ImageView recIcon;
Expand Down Expand Up @@ -147,7 +150,7 @@ public void onViewCreated(@NonNull View view,
recIcon = view.findViewById(R.id.redDot);
timeRemaining = view.findViewById(R.id.timeRemainingBar);
view.findViewById(R.id.btn_test_point).setOnClickListener(v -> onAddTestPoint());

view.findViewById(R.id.btn_refresh_position).setOnClickListener(v -> onRefreshPosition());

// Hide or initialize default values
gnssError.setVisibility(View.GONE);
Expand Down Expand Up @@ -247,51 +250,161 @@ private void onAddTestPoint() {
trajectoryMapFragment.addTestPointMarker(idx, ts, cur);
}

/**
* Manually refresh the fused position by resetting the particle filter
* and re-initializing it with the current GNSS or WiFi position.
* This is useful when the position has drifted significantly and needs correction.
*/
private void onRefreshPosition() {
// Record old fused position for logging
LatLng oldFused = sensorFusion.getFusedPosition();

// Try GNSS position first (current, not start)
float[] gnssLatLng = sensorFusion.getGNSSLatitude(false);
double newLat = 0;
double newLng = 0;
String source = null;

if (gnssLatLng != null && (gnssLatLng[0] != 0 || gnssLatLng[1] != 0)) {
newLat = gnssLatLng[0];
newLng = gnssLatLng[1];
source = "GNSS";
} else {
// Fallback to WiFi positioning
LatLng wifiPos = sensorFusion.getLatLngWifiPositioning();
if (wifiPos != null) {
newLat = wifiPos.latitude;
newLng = wifiPos.longitude;
source = "WiFi";
}
}

if (source != null) {
// Reset and re-initialize the particle filter at the new position
sensorFusion.getPositionFusion().reset();
sensorFusion.getPositionFusion().init(newLat, newLng);

Log.e("POSITION_REFRESH", "Position refreshed using " + source
+ " | old=(" + (oldFused != null ? String.format("%.7f", oldFused.latitude)
+ "," + String.format("%.7f", oldFused.longitude) : "null")
+ ") | new=(" + String.format("%.7f", newLat)
+ "," + String.format("%.7f", newLng) + ")");

Toast.makeText(requireContext(),
getString(R.string.refresh_position_success),
Toast.LENGTH_SHORT).show();
} else {
Log.e("POSITION_REFRESH", "No valid GNSS or WiFi position available for refresh");
Toast.makeText(requireContext(),
getString(R.string.refresh_position_no_source),
Toast.LENGTH_SHORT).show();
}
}

/**
* Update the UI with sensor data and pass map updates to TrajectoryMapFragment.
*/
private void updateUIandPosition() {
float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR);
if (pdrValues == null) return;
if (pdrValues == null) {
Log.e(TAG, "PDR values are null, skipping update");
return;
}

// Distance
distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2)
+ Math.pow(pdrValues[1] - previousPosY, 2));
float dx = pdrValues[0] - previousPosX;
float dy = pdrValues[1] - previousPosY;
double stepDist = Math.sqrt(dx * dx + dy * dy);
distance += stepDist;
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:
// Log PDR state periodically (every ~1s = every 5th call at 200ms interval)
if (((int)(distance * 10)) % 5 == 0) {
Log.e(TAG, "=== PDR State ===");
Log.e(TAG, "PDR raw: x=" + pdrValues[0] + " y=" + pdrValues[1]);
Log.e(TAG, "PDR delta: dx=" + String.format("%.4f", dx)
+ " dy=" + String.format("%.4f", dy)
+ " stepDist=" + String.format("%.3f", stepDist) + "m");
Log.e(TAG, "Total distance: " + String.format("%.2f", distance)
+ "m | Elevation: " + String.format("%.1f", elevationVal)
+ "m | Elevator: " + sensorFusion.getElevator());
}

// Current location — use fused (smooth) or raw PDR based on toggle
LatLng fusedPos = sensorFusion.getFusedPosition();
float[] latLngArray = sensorFusion.getGNSSLatitude(true);
if (latLngArray != null) {
LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally
boolean useSmooth = trajectoryMapFragment != null && trajectoryMapFragment.isSmoothEnabled();

if (useSmooth && fusedPos != null) {
// Smooth ON: use fused position (PDR + GNSS + WiFi corrections)
LatLng oldLocation = trajectoryMapFragment.getCurrentLocation();
if (oldLocation == null) {
Log.e(TAG, "=== Initial Fused Position ===");
Log.e(TAG, "Fused pos: " + fusedPos.latitude + ", " + fusedPos.longitude);
}

if (trajectoryMapFragment != null) {
float orientDeg = (float) Math.toDegrees(sensorFusion.passOrientation());
trajectoryMapFragment.updateUserLocation(fusedPos, orientDeg);

// Log fused vs raw PDR comparison periodically
if (latLngArray != null && ((int)(distance * 10)) % 10 == 0) {
LatLng pdrOnly = UtilFunctions.calculateNewPos(
new LatLng(latLngArray[0], latLngArray[1]),
pdrValues);
double fusedPdrDiff = UtilFunctions.distanceBetweenPoints(fusedPos, pdrOnly);
Log.e(TAG, "=== Fusion vs Raw PDR ===");
Log.e(TAG, "Fused: " + String.format("%.7f", fusedPos.latitude)
+ ", " + String.format("%.7f", fusedPos.longitude));
Log.e(TAG, "RawPDR: " + String.format("%.7f", pdrOnly.latitude)
+ ", " + String.format("%.7f", pdrOnly.longitude));
Log.e(TAG, "Fused-PDR diff: " + String.format("%.2f", fusedPdrDiff) + "m");
}
}
} else if (latLngArray != null) {
// Smooth OFF or fusion not initialized: use raw PDR
LatLng oldLocation = trajectoryMapFragment.getCurrentLocation();
LatLng newLocation = UtilFunctions.calculateNewPos(
oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation,
new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY }
new float[]{ dx, dy }
);

// Pass the location + orientation to the map
if (oldLocation == null) {
Log.e(TAG, "=== Initial Position (PDR" + (useSmooth ? " fallback" : " raw") + ") ===");
Log.e(TAG, "Start location: lat=" + latLngArray[0] + " lng=" + latLngArray[1]);
} else if (stepDist > 2.0) {
Log.e(TAG, "WARNING: Large single step: " + String.format("%.2f", stepDist)
+ "m | dx=" + dx + " dy=" + dy);
}

if (trajectoryMapFragment != null) {
trajectoryMapFragment.updateUserLocation(newLocation,
(float) Math.toDegrees(sensorFusion.passOrientation()));
float orientDeg = (float) Math.toDegrees(sensorFusion.passOrientation());
trajectoryMapFragment.updateUserLocation(newLocation, orientDeg);
}
} else {
Log.e(TAG, "WARNING: No position available (fusion=null, startGNSS=null)");
}

// GNSS logic if you want to show GNSS error, etc.
// GNSS logic
float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG);
if (gnss != null && trajectoryMapFragment != null) {
// If user toggles showing GNSS in the map, call e.g.
if (trajectoryMapFragment.isGnssEnabled()) {
LatLng gnssLocation = new LatLng(gnss[0], gnss[1]);
LatLng currentLoc = trajectoryMapFragment.getCurrentLocation();

Log.e(TAG, "=== GNSS Display ===");
Log.e(TAG, "GNSS location: " + gnss[0] + ", " + gnss[1]);
if (currentLoc != null) {
double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation);
Log.e(TAG, "PDR location: " + currentLoc.latitude + ", " + currentLoc.longitude);
Log.e(TAG, "GNSS-PDR error: " + String.format("%.2f", errorDist) + "m");
if (errorDist > 100) {
Log.e(TAG, "WARNING: GNSS-PDR divergence >100m! Possible GNSS or PDR issue");
}
gnssError.setVisibility(View.VISIBLE);
gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist));
}
Expand All @@ -302,6 +415,27 @@ private void updateUIandPosition() {
}
}

// WiFi positioning — display on map + log
LatLng wifiPos = sensorFusion.getLatLngWifiPositioning();
if (wifiPos != null && trajectoryMapFragment != null) {
if (trajectoryMapFragment.isWifiEnabled()) {
trajectoryMapFragment.updateWiFi(wifiPos);
} else {
trajectoryMapFragment.clearWiFi();
}

if (((int)(distance * 10)) % 10 == 0) {
Log.e(TAG, "=== WiFi Position Status ===");
Log.e(TAG, "WiFi pos: " + wifiPos.latitude + ", " + wifiPos.longitude
+ " floor=" + sensorFusion.getWifiFloor());
LatLng currentLoc = trajectoryMapFragment.getCurrentLocation();
if (currentLoc != null) {
double wifiPdrDist = UtilFunctions.distanceBetweenPoints(currentLoc, wifiPos);
Log.e(TAG, "WiFi-PDR distance: " + String.format("%.2f", wifiPdrDist) + "m");
}
}
}

// Update previous
previousPosX = pdrValues[0];
previousPosY = pdrValues[1];
Expand Down
42 changes: 36 additions & 6 deletions ...main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,12 +300,18 @@ private void onBuildingSelected(String buildingName, Polygon polygon) {
// Compute building centre from polygon points
LatLng center = computePolygonCenter(polygon);

// Move the marker to building centre
// Use GNSS position if it falls inside the building, otherwise use building centre
float[] gnss = sensorFusion.getGNSSLatitude(false);
LatLng gnssPos = new LatLng(gnss[0], gnss[1]);
boolean gnssValid = gnss[0] != 0 || gnss[1] != 0;
LatLng bestStart = (gnssValid && isPointInPolygon(gnssPos, polygon)) ? gnssPos : center;

// Move the marker to the best start position
if (startMarker != null) {
startMarker.setPosition(center);
startMarker.setPosition(bestStart);
}
startPosition[0] = (float) center.latitude;
startPosition[1] = (float) center.longitude;
startPosition[0] = (float) bestStart.latitude;
startPosition[1] = (float) bestStart.longitude;

// Zoom to the building
mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(center, 20f));
Expand Down Expand Up @@ -426,6 +432,30 @@ private String formatBuildingName(String apiName) {
return sb.toString();
}

/**
* Ray-casting algorithm to test if a point lies inside a polygon.
*
* @param point the point to test
* @param polygon the polygon to test against
* @return true if the point is inside the polygon
*/
private boolean isPointInPolygon(LatLng point, Polygon polygon) {
List<LatLng> vertices = polygon.getPoints();
int n = vertices.size();
boolean inside = false;
for (int i = 0, j = n - 1; i < n; j = i++) {
LatLng vi = vertices.get(i);
LatLng vj = vertices.get(j);
if ((vi.latitude > point.latitude) != (vj.latitude > point.latitude)
&& point.longitude < (vj.longitude - vi.longitude)
* (point.latitude - vi.latitude) / (vj.latitude - vi.latitude)
+ vi.longitude) {
inside = !inside;
}
}
return inside;
}

/**
* Computes the centroid of a Google Maps Polygon by averaging all vertices.
*
Expand Down Expand Up @@ -468,9 +498,9 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat
}

if (requireActivity() instanceof RecordingActivity) {
// Start sensor recording + set the start location
sensorFusion.startRecording();
// Set start location BEFORE recording so PositionFusion.init() sees it
sensorFusion.setStartGNSSLatitude(startPosition);
sensorFusion.startRecording();
// Write trajectory_id, initial_position and initial heading to protobuf
sensorFusion.writeInitialMetadata();

Expand Down
Loading