compass-lib 0.0.8__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,304 @@
1
+ # -*- coding: utf-8 -*-
2
+ """Traverse-quality weighted least-squares solver (Larry Fish method).
3
+
4
+ Weights each shot by the quality of the traverse it belongs to.
5
+ Shots on good traverses (small misclosure per shot) get **high**
6
+ weight and are protected. Shots on bad traverses (large misclosure
7
+ per shot) get **low** weight and absorb the error.
8
+
9
+ This prevents blunders in bad traverses from contaminating good
10
+ data -- the core insight from Larry Fish's articles on loop closure.
11
+ """
12
+
13
+ from __future__ import annotations
14
+
15
+ import logging
16
+ from collections import deque
17
+ from itertools import combinations
18
+
19
+ import numpy as np
20
+
21
+ from compass_lib.constants import SOLVER_MAX_HEADING_CHANGE
22
+ from compass_lib.constants import SOLVER_MAX_LENGTH_CHANGE
23
+ from compass_lib.solver.base import SurveyAdjuster
24
+ from compass_lib.solver.models import NetworkShot
25
+ from compass_lib.solver.models import SurveyNetwork
26
+ from compass_lib.solver.models import Vector3D
27
+
28
+ logger = logging.getLogger(__name__)
29
+
30
+
31
+ def _find_path(
32
+ start: str,
33
+ end: str,
34
+ adj: dict[str, list[NetworkShot]],
35
+ blocked: frozenset[str] = frozenset(),
36
+ ) -> list[str] | None:
37
+ """BFS shortest path, avoiding blocked stations."""
38
+ if start == end:
39
+ return [start]
40
+ parent: dict[str, str | None] = {start: None}
41
+ queue: deque[str] = deque([start])
42
+ while queue:
43
+ current = queue.popleft()
44
+ for shot in adj.get(current, []):
45
+ n = shot.to_name
46
+ if n in parent or (n in blocked and n != end):
47
+ continue
48
+ parent[n] = current
49
+ if n == end:
50
+ path: list[str] = []
51
+ node: str | None = end
52
+ while node is not None:
53
+ path.append(node)
54
+ node = parent[node]
55
+ path.reverse()
56
+ return path
57
+ queue.append(n)
58
+ return None
59
+
60
+
61
+ def _solve_lse(
62
+ network: SurveyNetwork,
63
+ max_length_frac: float,
64
+ max_angle_frac: float,
65
+ ) -> dict[str, Vector3D]:
66
+ """Traverse-quality weighted lstsq."""
67
+ # Filter out anchors that don't exist in stations (orphan link
68
+ # stations, already warned about upstream).
69
+ anchors = network.anchors & set(network.stations.keys())
70
+ non_anchors = sorted(set(network.stations.keys()) - anchors)
71
+
72
+ if not non_anchors:
73
+ return dict(network.stations)
74
+
75
+ m = len(non_anchors)
76
+ station_to_idx = {name: i for i, name in enumerate(non_anchors)}
77
+ anchor_pos: dict[str, Vector3D] = {name: network.stations[name] for name in anchors}
78
+ adj = network.adjacency
79
+
80
+ # -- Centre coordinates for numerical stability ---------------------------
81
+ # UTM coordinates can be O(10^5..10^6). Subtracting a reference
82
+ # point keeps all lstsq values near zero, avoiding precision loss.
83
+ _all_pos = list(anchor_pos.values())
84
+ origin = Vector3D(
85
+ sum(p.x for p in _all_pos) / len(_all_pos),
86
+ sum(p.y for p in _all_pos) / len(_all_pos),
87
+ sum(p.z for p in _all_pos) / len(_all_pos),
88
+ )
89
+ anchor_pos_c = {n: p - origin for n, p in anchor_pos.items()}
90
+
91
+ # -- Compute traverse quality for each shot ----------------------------
92
+ #
93
+ # For each pair of adjacent anchors, find the traverse path and
94
+ # compute misclosure per shot. Each shot gets the quality score
95
+ # of the BEST traverse it belongs to (lowest misclosure/shot).
96
+
97
+ shot_key_to_quality: dict[tuple[str, str], float] = {}
98
+
99
+ # Build a directed edge lookup for O(1) path-walk.
100
+ edge_delta: dict[tuple[str, str], Vector3D] = {}
101
+ for shot in network.shots:
102
+ edge_delta[(shot.from_name, shot.to_name)] = shot.delta
103
+ edge_delta[(shot.to_name, shot.from_name)] = -shot.delta
104
+
105
+ anchor_list = sorted(anchors)
106
+ all_anchors_frozen = frozenset(anchors)
107
+
108
+ for a, b in combinations(anchor_list, 2):
109
+ other = all_anchors_frozen - {a, b}
110
+ path = _find_path(a, b, adj, blocked=other)
111
+ if path is None or len(path) < 2:
112
+ continue
113
+
114
+ # Forward-propagate to compute misclosure — O(path_length).
115
+ pos = network.stations[a]
116
+ n_shots = 0
117
+ for i in range(len(path) - 1):
118
+ delta = edge_delta.get((path[i], path[i + 1]))
119
+ if delta is not None:
120
+ pos = pos + delta
121
+ n_shots += 1
122
+
123
+ if n_shots == 0:
124
+ continue
125
+
126
+ misclosure = pos - network.stations[b]
127
+ quality = misclosure.length / n_shots # metres per shot
128
+
129
+ # Assign quality to each shot on this traverse (keep best).
130
+ for i in range(len(path) - 1):
131
+ p, q = path[i], path[i + 1]
132
+ key = (p, q) if p < q else (q, p)
133
+ if key not in shot_key_to_quality or quality < shot_key_to_quality[key]:
134
+ shot_key_to_quality[key] = quality
135
+
136
+ logger.debug(
137
+ "Traverse %s -> %s: %d shots, misclosure=%.1f m, quality=%.3f m/shot",
138
+ a,
139
+ b,
140
+ n_shots,
141
+ misclosure.length,
142
+ quality,
143
+ )
144
+
145
+ # -- Build design matrix with traverse-quality weights -----------------
146
+
147
+ row_a_entries: list[tuple[int, int, float]] = []
148
+ row_b_list: list[tuple[float, float, float]] = []
149
+ row_weights: list[float] = []
150
+ row_idx = 0
151
+
152
+ for shot in network.shots:
153
+ from_anc = shot.from_name in anchors
154
+ to_anc = shot.to_name in anchors
155
+ if from_anc and to_anc:
156
+ continue
157
+
158
+ dx, dy, dz = shot.delta.x, shot.delta.y, shot.delta.z
159
+ L = max(shot.distance, 0.1)
160
+
161
+ # Base weight: 1/L^2 (percentage-equalising).
162
+ base_w = 1.0 / (L * L)
163
+
164
+ # Traverse quality factor: shots on good traverses get
165
+ # BOOSTED weight (protected), shots on bad traverses get
166
+ # REDUCED weight (absorb error).
167
+ key = (min(shot.from_name, shot.to_name), max(shot.from_name, shot.to_name))
168
+ quality = shot_key_to_quality.get(key)
169
+
170
+ if quality is not None and quality > 1e-6:
171
+ # quality = misclosure per shot (metres).
172
+ # Good traverse: quality ≈ 0.1 → boost weight 100x
173
+ # Bad traverse: quality ≈ 5.0 → reduce weight 0.04x
174
+ # Factor = 1 / quality^2
175
+ quality_factor = 1.0 / (quality * quality)
176
+ else:
177
+ # Shot not on any traverse (side branch) or perfect
178
+ # quality: use base weight only.
179
+ quality_factor = 1.0
180
+
181
+ w = base_w * quality_factor
182
+
183
+ if from_anc:
184
+ a = anchor_pos_c[shot.from_name]
185
+ row_a_entries.append((row_idx, station_to_idx[shot.to_name], 1.0))
186
+ row_b_list.append((a.x + dx, a.y + dy, a.z + dz))
187
+ elif to_anc:
188
+ a = anchor_pos_c[shot.to_name]
189
+ row_a_entries.append((row_idx, station_to_idx[shot.from_name], -1.0))
190
+ row_b_list.append((dx - a.x, dy - a.y, dz - a.z))
191
+ else:
192
+ row_a_entries.append((row_idx, station_to_idx[shot.from_name], -1.0))
193
+ row_a_entries.append((row_idx, station_to_idx[shot.to_name], 1.0))
194
+ row_b_list.append((dx, dy, dz))
195
+
196
+ row_weights.append(w)
197
+ row_idx += 1
198
+
199
+ n_rows = row_idx
200
+ if n_rows == 0:
201
+ return dict(network.stations)
202
+
203
+ A = np.zeros((n_rows, m), dtype=np.float64)
204
+
205
+ # Vectorised fill via advanced indexing.
206
+ if row_a_entries:
207
+ _a = np.array(row_a_entries, dtype=np.float64)
208
+ A[_a[:, 0].astype(np.intp), _a[:, 1].astype(np.intp)] = _a[:, 2]
209
+
210
+ _b = np.array(row_b_list, dtype=np.float64)
211
+ bx = _b[:, 0].copy()
212
+ by = _b[:, 1].copy()
213
+ bz = _b[:, 2].copy()
214
+
215
+ W = np.sqrt(np.array(row_weights, dtype=np.float64))
216
+ Aw = A * W[:, np.newaxis]
217
+
218
+ sol_x, _, _, _ = np.linalg.lstsq(Aw, bx * W, rcond=None)
219
+ sol_y, _, _, _ = np.linalg.lstsq(Aw, by * W, rcond=None)
220
+ sol_z, _, _, _ = np.linalg.lstsq(Aw, bz * W, rcond=None)
221
+
222
+ # -- Build result dict (translate back from centred coords) ---------------
223
+
224
+ result: dict[str, Vector3D] = {}
225
+ for name in anchors:
226
+ result[name] = network.stations[name]
227
+ for name, idx in station_to_idx.items():
228
+ result[name] = Vector3D(
229
+ float(sol_x[idx]) + origin.x,
230
+ float(sol_y[idx]) + origin.y,
231
+ float(sol_z[idx]) + origin.z,
232
+ )
233
+ for name in network.stations:
234
+ if name not in result:
235
+ result[name] = network.stations[name]
236
+
237
+ # -- Validate (vectorised) ----------------------------------------------
238
+
239
+ if network.shots:
240
+ n_s = len(network.shots)
241
+ _dx = np.empty(n_s, dtype=np.float64)
242
+ _dy = np.empty(n_s, dtype=np.float64)
243
+ _dz = np.empty(n_s, dtype=np.float64)
244
+ _sl = np.empty(n_s, dtype=np.float64)
245
+ for _i, _sh in enumerate(network.shots):
246
+ _f = result[_sh.from_name]
247
+ _t = result[_sh.to_name]
248
+ _dx[_i] = _t.x - _f.x
249
+ _dy[_i] = _t.y - _f.y
250
+ _dz[_i] = _t.z - _f.z
251
+ _sl[_i] = _sh.delta.length
252
+ _el = np.sqrt(_dx * _dx + _dy * _dy + _dz * _dz)
253
+ _mask = _sl > 1e-9
254
+ _ratio = np.abs(_el[_mask] - _sl[_mask]) / _sl[_mask]
255
+ violation_count = int(np.sum(_ratio > max_length_frac))
256
+
257
+ if violation_count:
258
+ logger.warning(
259
+ "%d shot(s) exceed length tolerance (%.0f%%)",
260
+ violation_count,
261
+ max_length_frac * 100,
262
+ )
263
+
264
+ logger.info(
265
+ "Adjusted %d station(s) via LSE solver (%d shots, %d anchors, %d traverses)",
266
+ len(non_anchors),
267
+ n_rows,
268
+ len(anchors),
269
+ len(shot_key_to_quality),
270
+ )
271
+
272
+ return result
273
+
274
+
275
+ class LSESolver(SurveyAdjuster):
276
+ """Traverse-quality weighted solver (Larry Fish method).
277
+
278
+ Shots on good traverses (small misclosure) are protected with
279
+ high weight. Shots on bad traverses absorb the error.
280
+ Blunders stay localised.
281
+ """
282
+
283
+ def __init__(
284
+ self,
285
+ max_length_change: float = SOLVER_MAX_LENGTH_CHANGE,
286
+ max_angle_change: float = SOLVER_MAX_HEADING_CHANGE,
287
+ ) -> None:
288
+ self._max_length_change = max_length_change
289
+ self._max_angle_change = max_angle_change
290
+
291
+ @property
292
+ def name(self) -> str:
293
+ return "LSESolver"
294
+
295
+ def adjust(self, network: SurveyNetwork) -> dict[str, Vector3D]:
296
+ if len(network.anchors) < 2:
297
+ logger.info("Fewer than 2 anchors -- nothing to adjust")
298
+ return dict(network.stations)
299
+
300
+ return _solve_lse(
301
+ network,
302
+ self._max_length_change,
303
+ self._max_angle_change,
304
+ )
@@ -0,0 +1,229 @@
1
+ # -*- coding: utf-8 -*-
2
+ """Data structures for the survey adjustment solver.
3
+
4
+ This module is deliberately decoupled from GeoJSON, UTM, Compass file
5
+ formats, and any other domain-specific concern. It operates purely on
6
+ station names, 3-D delta vectors, and shot lengths so that any solver
7
+ algorithm can be tested in isolation.
8
+ """
9
+
10
+ from __future__ import annotations
11
+
12
+ from dataclasses import dataclass
13
+ from dataclasses import field
14
+ from typing import TYPE_CHECKING
15
+ from typing import NamedTuple
16
+
17
+ if TYPE_CHECKING:
18
+ from compass_lib.geojson import ComputedSurvey
19
+ from compass_lib.geojson import Station
20
+
21
+
22
+ # ---------------------------------------------------------------------------
23
+ # Primitives
24
+ # ---------------------------------------------------------------------------
25
+
26
+
27
+ class Vector3D(NamedTuple):
28
+ """An immutable 3-D vector (easting, northing, elevation) in metres."""
29
+
30
+ x: float
31
+ y: float
32
+ z: float
33
+
34
+ def __add__(self, other: Vector3D) -> Vector3D: # type: ignore[override]
35
+ return Vector3D(self.x + other.x, self.y + other.y, self.z + other.z)
36
+
37
+ def __sub__(self, other: Vector3D) -> Vector3D:
38
+ return Vector3D(self.x - other.x, self.y - other.y, self.z - other.z)
39
+
40
+ def __mul__(self, scalar: float) -> Vector3D: # type: ignore[override]
41
+ return Vector3D(self.x * scalar, self.y * scalar, self.z * scalar)
42
+
43
+ def __rmul__(self, scalar: float) -> Vector3D:
44
+ return self.__mul__(scalar)
45
+
46
+ def __neg__(self) -> Vector3D:
47
+ return Vector3D(-self.x, -self.y, -self.z)
48
+
49
+ @property
50
+ def length(self) -> float:
51
+ """Euclidean length of the vector."""
52
+ return (self.x**2 + self.y**2 + self.z**2) ** 0.5
53
+
54
+
55
+ ZERO = Vector3D(0.0, 0.0, 0.0)
56
+
57
+
58
+ # ---------------------------------------------------------------------------
59
+ # Network primitives
60
+ # ---------------------------------------------------------------------------
61
+
62
+
63
+ @dataclass(frozen=True)
64
+ class NetworkShot:
65
+ """A single shot in the survey network.
66
+
67
+ All values are in metres and already corrected for declination /
68
+ convergence. ``delta`` is the vector **from** ``from_name`` **to**
69
+ ``to_name``.
70
+ """
71
+
72
+ from_name: str
73
+ to_name: str
74
+ delta: Vector3D
75
+ distance: float # original shot length in metres (for weighting)
76
+
77
+
78
+ @dataclass
79
+ class Traverse:
80
+ """A traverse between two fixed anchor stations.
81
+
82
+ A traverse connects two *different* anchor stations whose
83
+ coordinates are both known. The misclosure is the difference
84
+ between the measured path delta and the known coordinate difference
85
+ of the two anchors.
86
+
87
+ Attributes:
88
+ from_anchor: Name of the starting anchor station.
89
+ to_anchor: Name of the ending anchor station.
90
+ station_names: Ordered station names from ``from_anchor`` to
91
+ ``to_anchor`` (inclusive at both ends).
92
+ shots: Ordered ``NetworkShot`` objects along the traverse,
93
+ matching ``station_names`` pairwise.
94
+ misclosure: ``sum(shot.delta) - (to_pos - from_pos)``.
95
+ A perfect traverse has misclosure == ZERO.
96
+ """
97
+
98
+ from_anchor: str
99
+ to_anchor: str
100
+ station_names: list[str]
101
+ shots: list[NetworkShot]
102
+ misclosure: Vector3D = ZERO
103
+
104
+ @property
105
+ def total_length(self) -> float:
106
+ """Sum of all shot distances along the traverse."""
107
+ return sum(s.distance for s in self.shots)
108
+
109
+
110
+ # ---------------------------------------------------------------------------
111
+ # Survey network
112
+ # ---------------------------------------------------------------------------
113
+
114
+
115
+ @dataclass
116
+ class SurveyNetwork:
117
+ """The full survey network used by survey adjustment solvers.
118
+
119
+ This is the *only* object a solver receives. It contains everything
120
+ needed to adjust coordinates, with no coupling to Compass or GeoJSON
121
+ data types.
122
+
123
+ Attributes:
124
+ stations: Station name -> raw BFS-computed position.
125
+ shots: Every shot in the network (directed).
126
+ anchors: Station names whose coordinates are *fixed* and must
127
+ not be adjusted (e.g. GPS-tied stations).
128
+ """
129
+
130
+ stations: dict[str, Vector3D] = field(default_factory=dict)
131
+ shots: list[NetworkShot] = field(default_factory=list)
132
+ anchors: set[str] = field(default_factory=set)
133
+
134
+ # -- adjacency cache (built lazily) ------------------------------------
135
+
136
+ _adjacency: dict[str, list[NetworkShot]] | None = field(
137
+ default=None, init=False, repr=False
138
+ )
139
+
140
+ @property
141
+ def adjacency(self) -> dict[str, list[NetworkShot]]:
142
+ """Undirected adjacency list (lazily built, cached)."""
143
+ if self._adjacency is None:
144
+ adj: dict[str, list[NetworkShot]] = {}
145
+ for shot in self.shots:
146
+ adj.setdefault(shot.from_name, []).append(shot)
147
+ # Add reverse direction
148
+ rev = NetworkShot(
149
+ from_name=shot.to_name,
150
+ to_name=shot.from_name,
151
+ delta=-shot.delta,
152
+ distance=shot.distance,
153
+ )
154
+ adj.setdefault(shot.to_name, []).append(rev)
155
+ self._adjacency = adj
156
+ return self._adjacency
157
+
158
+ # -- factory -----------------------------------------------------------
159
+
160
+ @classmethod
161
+ def from_computed_survey(
162
+ cls,
163
+ survey: ComputedSurvey,
164
+ anchors: dict[str, Station],
165
+ ) -> SurveyNetwork:
166
+ """Build a ``SurveyNetwork`` from an already-propagated survey.
167
+
168
+ Shot deltas are taken from the **measurement-based** displacement
169
+ stored on each leg (``measurement_delta``), NOT from coordinate
170
+ differences. This is critical because coordinate differences
171
+ between consecutive stations already incorporate propagation
172
+ error, masking the actual misclosure that traverse adjustment
173
+ needs to correct.
174
+ """
175
+ stations: dict[str, Vector3D] = {}
176
+ for name, st in survey.stations.items():
177
+ stations[name] = Vector3D(st.easting, st.northing, st.elevation)
178
+
179
+ # Build a reverse lookup: Station object id -> dict key.
180
+ # Station.name is the *display* name (unscoped), but dict keys
181
+ # may be file-scoped. Legs reference Station objects, so we
182
+ # need to recover the scoped key for each leg endpoint.
183
+ station_obj_to_key: dict[int, str] = {
184
+ id(st): key for key, st in survey.stations.items()
185
+ }
186
+
187
+ shots: list[NetworkShot] = []
188
+ seen: set[tuple[str, str]] = set()
189
+ for leg in survey.legs:
190
+ from_key = station_obj_to_key.get(id(leg.from_station))
191
+ to_key = station_obj_to_key.get(id(leg.to_station))
192
+ if from_key is None or to_key is None:
193
+ continue
194
+
195
+ key = (from_key, to_key)
196
+ rev_key = (to_key, from_key)
197
+ if key in seen or rev_key in seen:
198
+ continue
199
+ seen.add(key)
200
+
201
+ if leg.measurement_delta is not None:
202
+ # Use the measurement-based delta (distance, bearing,
203
+ # inclination with declination/convergence applied).
204
+ de, dn, dz = leg.measurement_delta
205
+ delta = Vector3D(de, dn, dz)
206
+ else:
207
+ # Fallback: coordinate difference (less accurate for
208
+ # traverse adjustment, but better than nothing).
209
+ from_pos = stations.get(from_key)
210
+ to_pos = stations.get(to_key)
211
+ if from_pos is None or to_pos is None:
212
+ continue
213
+ delta = to_pos - from_pos
214
+
215
+ shots.append(
216
+ NetworkShot(
217
+ from_name=from_key,
218
+ to_name=to_key,
219
+ delta=delta,
220
+ distance=(delta.length if delta.length > 0 else 1e-6),
221
+ )
222
+ )
223
+
224
+ # Only keep anchors that actually exist in the station dict.
225
+ # Anchors referencing stations not present in the survey data
226
+ # (e.g. link stations with no matching DAT station) are silently
227
+ # dropped — they were already warned about upstream.
228
+ anchor_names = set(anchors.keys()) & set(stations.keys())
229
+ return cls(stations=stations, shots=shots, anchors=anchor_names)
@@ -0,0 +1,28 @@
1
+ # -*- coding: utf-8 -*-
2
+ """No-op survey adjustment solver.
3
+
4
+ Returns station coordinates unchanged. Useful as a default, for
5
+ testing, or when the caller explicitly wants raw BFS coordinates.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ from typing import TYPE_CHECKING
11
+
12
+ from compass_lib.solver.base import SurveyAdjuster
13
+
14
+ if TYPE_CHECKING:
15
+ from compass_lib.solver.models import SurveyNetwork
16
+ from compass_lib.solver.models import Vector3D
17
+
18
+
19
+ class NoopSolver(SurveyAdjuster):
20
+ """Solver that performs no adjustment (identity transform)."""
21
+
22
+ @property
23
+ def name(self) -> str:
24
+ return "NoopSolver"
25
+
26
+ def adjust(self, network: SurveyNetwork) -> dict[str, Vector3D]:
27
+ """Return all station positions unchanged."""
28
+ return dict(network.stations)