Merge pull request #85 from Stanford-NavLab/derek/kaggle

Derek/kaggle

Author: kanhereashwin

.github/workflows/build.yml

@@ -16,7 +16,7 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        python-version: ["3.7.1", "3.8", "3.10"]
+        python-version: ["3.8", "3.10"]
         os: [ubuntu-latest, macos-latest, windows-latest]
       fail-fast : false
     defaults:

data/unit_test/android_2022/device_gnss.csv

@@ -0,0 +1,2 @@
+int,float,datetime,string
+4,4.321,2022-11-14,testing

data/unit_test/android_2022/ground_truth.csv

@@ -82,18 +82,14 @@ def solve_wls(measurements, weight_type = None,
         try:
             position = wls(position, pos_sv_m, corr_pr_m, weights,
                            only_bias, tol, max_count)
-
             states.append([timestamp] + np.squeeze(position).tolist())
         except RuntimeError as error:
             warnings.warn("RuntimeError encountered at gps_millis: " \
                         + str(int(timestamp)) + " RuntimeError: " \
                         + str(error), RuntimeWarning)
-    states = np.array(states)
+            states.append([timestamp, np.nan, np.nan, np.nan, np.nan])
 
-    if states.size == 0:
-        warnings.warn("No valid state estimate computed in WLS, "\
-                    + "returning None.", RuntimeWarning)
-        return None
+    states = np.array(states)
 
     state_estimate = NavData()
     state_estimate["gps_millis"] = states[:,0]
@@ -102,6 +98,11 @@ def solve_wls(measurements, weight_type = None,
     state_estimate["z_rx_m"] = states[:,3]
     state_estimate["b_rx_m"] = states[:,4]
 
+    if np.isnan(states[:,1:]).all():
+        warnings.warn("No valid state estimate computed in WLS, "\
+                    + "returning NaNs.", RuntimeWarning)
+        return state_estimate
+
     lat,lon,alt = ecef_to_geodetic(state_estimate[["x_rx_m","y_rx_m",
                                    "z_rx_m"]].reshape(3,-1))
     state_estimate["lat_rx_deg"] = lat

data/unit_test/navdata/navdata_test_dtypes.csv

@@ -15,7 +15,9 @@
 
 from gnss_lib_py.parsers.navdata import NavData
 from gnss_lib_py.utils.coordinates import geodetic_to_ecef
+from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 from gnss_lib_py.utils.time_conversions import unix_to_gps_millis
+from gnss_lib_py.utils.time_conversions import gps_to_unix_millis
 
 class AndroidDerived2021(NavData):
     """Class handling derived measurements from Android dataset.
@@ -82,7 +84,7 @@ def postprocess(self):
                      - self['intersignal_bias_m'] \
                      - self['tropo_delay_m'] \
                      - self['iono_delay_m']
-        self['corr_pr_m'] = pr_corrected        
+        self['corr_pr_m'] = pr_corrected
         # rename gnss_id column to constellation type
         constellation_map = {0.:"unknown",
                              1.:"gps",
@@ -234,7 +236,10 @@ def _row_map():
                    'TroposphericDelayMeters' : 'tropo_delay_m',
                    'Cn0DbHz': 'cn0_dbhz',
                    'AccumulatedDeltaRangeMeters' : 'accumulated_delta_range_m',
-                   'AccumulatedDeltaRangeUncertaintyMeters': 'accumulated_delta_range_sigma_m'
+                   'AccumulatedDeltaRangeUncertaintyMeters': 'accumulated_delta_range_sigma_m',
+                   'WlsPositionXEcefMeters' : 'x_rx_m',
+                   'WlsPositionYEcefMeters' : 'y_rx_m',
+                   'WlsPositionZEcefMeters' : 'z_rx_m',
                    }
         return row_map
 
@@ -488,3 +493,137 @@ def make_csv(input_path, output_directory, field, show_path=False):
         print(output_path)
 
     return output_path
+
+def solve_kaggle_baseline(navdata):
+    """Convert Decimeter challenge baseline into state_estimate.
+
+    The baseline solution was provided in 2022, but not in 2021.
+
+    Parameters
+    ----------
+    navdata : gnss_lib_py.parsers.android.AndroidDerived2022
+        Instance of the AndroidDerived2022 class.
+
+    Returns
+    -------
+    state_estimate : gnss_lib_py.parsers.navdata.NavData
+        Baseline state estimate.
+
+    """
+
+    columns = ["unix_millis",
+               "x_rx_m",
+               "y_rx_m",
+               "z_rx_m",
+               ]
+    navdata.in_rows(columns)
+    data_df = (navdata.pandas_df().drop_duplicates(subset='unix_millis')[columns]
+               .reset_index(drop=True))
+    lat,lon,alt = np.transpose(ecef_to_geodetic(data_df[["x_rx_m",
+                                                         "y_rx_m",
+                                                         "z_rx_m",
+                                                         ]].to_numpy()))
+
+    state_estimate = NavData()
+    state_estimate["gps_millis"] = unix_to_gps_millis(
+                                     data_df["unix_millis"].to_numpy())
+    state_estimate["lat_rx_deg"] = lat
+    state_estimate["lon_rx_deg"] = lon
+    state_estimate["alt_rx_deg"] = alt
+
+    return state_estimate
+
+def prepare_kaggle_submission(state_estimate, trip_id="trace/phone"):
+    """Converts from gnss_lib_py receiver state to Kaggle submission.
+
+    Parameters
+    ----------
+    state_estimate : gnss_lib_py.parsers.navdata.NavData
+        Estimated receiver position in latitude and longitude as an
+        instance of the NavData class with the following
+        rows: ``gps_millis``, ``lat_*_deg``, ``lon_*_deg``.
+    trip_id : string
+        Value for the tripId column in kaggle submission which is a
+        fusion of the data and phone type.
+
+    Returns
+    -------
+    output : gnss_lib_py.parsers.navdata.NavData
+        NavData structure ready for Kaggle submission.
+
+    """
+
+    state_estimate.in_rows("gps_millis")
+    wildcards = state_estimate.find_wildcard_indexes(["lat_*_deg",
+                            "lon_*_deg"],max_allow = 1)
+
+    output = NavData()
+    output["tripId"] = np.array([trip_id] * state_estimate.shape[1])
+    output["UnixTimeMillis"] = gps_to_unix_millis(state_estimate["gps_millis"])
+    output.orig_dtypes["UnixTimeMillis"] = np.int64
+    output["LatitudeDegrees"] = state_estimate[wildcards["lat_*_deg"]]
+    output["LongitudeDegrees"] = state_estimate[wildcards["lon_*_deg"]]
+
+    output.interpolate("UnixTimeMillis",["LatitudeDegrees",
+                                         "LongitudeDegrees"],inplace=True)
+    return output
+
+def solve_kaggle_dataset(folder_path, solver, verbose=False, *args):
+    """Run solver on all kaggle traces.
+
+    Additional ``*args`` arguments are passed into the ``solver``
+    function.
+
+    Parameters
+    ----------
+    folder_path: string or path-like
+        Path to folder containing all traces (e.g. full path to "train"
+        or "test" directories.
+    solver : function
+        State estimate solver that takes an instance of
+        AndroidDerived2022 and outputs a state_estimate NavData object.
+        Additional ``*args`` arguments are passed into this ``solver``
+        function.
+    verbose : bool
+        If verbose, will print each trace trajectory name and phone name
+	pair when it is solving the state estimate for that pair.
+
+    Returns
+    -------
+    submission : gnss_lib_py.parsers.navdata.NavData
+        Full solution submission across all traces. Can then be saved
+        using submission.to_csv().
+
+    """
+
+    # create solution NavData object
+    solution = NavData()
+
+    # iterate through all trace options
+    for trace_name in sorted(os.listdir(folder_path)):
+        trace_path = os.path.join(folder_path, trace_name)
+        # iterate through all phone types
+        for phone_type in sorted(os.listdir(trace_path)):
+            data_path = os.path.join(folder_path,trace_name,
+                                     phone_type,"device_gnss.csv")
+            try:
+                # convert data to Measurement class
+                derived_data = AndroidDerived2022(data_path)
+
+                if verbose:
+                    print("solving:",trace_name,phone_type)
+
+                # compute state estimate using provided solver function
+                state_estimate = solver(derived_data, *args)
+
+                trip_id = "/".join([trace_name,phone_type])
+                output = prepare_kaggle_submission(state_estimate,
+                                                   trip_id)
+
+                # concatenate solution to previous solutions
+                solution.concat(navdata=output, inplace=True)
+
+            except FileNotFoundError:
+                continue
+
+    return solution