It’s not quite the visual positioning like the above but I thought it was interesting to explore!
If I have a 360 video, I can feed it into a visual simultaneous localization and mapping (vSLAM) application where it visually tracks the movement and maps the trajectory. It still will not know the position yet. So, it would need to be determined manually or based on some known good positions like when camera reports good GPS accuracy or RTK fix before/after entering an urban canyon. Then with known GPS locations, SLAM can be used to fill in the gaps of where GPS struggled.
Of course, more testing would be needed. In the sample, the rider only turned to another street once and it was in New York City where the roads are straight. I had some concern where moving trucks very close along with camera might cause tracking confusion but that was not the case. It can handle smaller movements well like when rider needed to go around a truck blocking the intersection. I tested at 10fps so for a car, a much higher framerate would likely be needed.
Some AI generated code for testing to convert the trajectory to gpx if anyone is interested.
#!/usr/bin/env python3
import argparse
import math
import xml.etree.ElementTree as ET
from datetime import datetime, timezone
R = 6378137.0
def parse_latlon_pair(value: str):
try:
lat_s, lon_s = value.split(",", 1)
return float(lat_s), float(lon_s)
except Exception:
raise argparse.ArgumentTypeError("Expected format: lat,lon")
def unix_to_gpx_time(timestamp: float) -> str:
dt = datetime.fromtimestamp(timestamp, tz=timezone.utc)
return dt.isoformat(timespec="milliseconds").replace("+00:00", "Z")
def read_start_end_from_gpx(path):
root = ET.parse(path).getroot()
if "}" in root.tag:
ns_uri = root.tag.split("}")[0].strip("{")
ns = {"gpx": ns_uri}
pts = root.findall(".//gpx:trkpt", ns) or root.findall(".//gpx:rtept", ns) or root.findall(".//gpx:wpt", ns)
else:
pts = root.findall(".//trkpt") or root.findall(".//rtept") or root.findall(".//wpt")
if len(pts) < 2:
raise RuntimeError("Reference GPX must contain at least 2 points")
start = pts[0]
end = pts[-1]
return (
float(start.attrib["lat"]),
float(start.attrib["lon"]),
float(end.attrib["lat"]),
float(end.attrib["lon"]),
)
def latlon_to_xy(lat, lon, lat0, lon0):
x = math.radians(lon - lon0) * R * math.cos(math.radians(lat0))
y = math.radians(lat - lat0) * R
return x, y
def xy_to_latlon(x, y, lat0, lon0):
lat = lat0 + math.degrees(y / R)
lon = lon0 + math.degrees(x / (R * math.cos(math.radians(lat0))))
return lat, lon
def read_stella_trajectory(path):
poses = []
with open(path, "r", encoding="utf-8") as f:
for line in f:
line = line.strip()
if not line or line.startswith("#"):
continue
vals = list(map(float, line.split()))
if len(vals) < 4:
continue
timestamp = vals[0]
x = vals[1]
y = vals[2]
z = vals[3]
poses.append((timestamp, x, y, z))
if len(poses) < 2:
raise RuntimeError("Trajectory must contain at least 2 valid poses")
return poses
def choose_ground_plane(poses, plane):
if plane == "xz":
return [(p[1], p[3]) for p in poses]
if plane == "xy":
return [(p[1], p[2]) for p in poses]
if plane == "zy":
return [(p[3], p[2]) for p in poses]
if plane == "zx":
return [(p[3], p[1]) for p in poses]
if plane == "negxz":
return [(-p[1], p[3]) for p in poses]
if plane == "xnegz":
return [(p[1], -p[3]) for p in poses]
raise ValueError(f"Unsupported plane: {plane}")
def convert_to_gpx(
traj_path,
out_path,
start_lat,
start_lon,
end_lat,
end_lon,
plane="xz",
elevation_axis="y",
include_time=True,
):
poses = read_stella_trajectory(traj_path)
slam_pts = choose_ground_plane(poses, plane)
sx0, sy0 = slam_pts[0]
sx1, sy1 = slam_pts[-1]
slam_dx = sx1 - sx0
slam_dy = sy1 - sy0
slam_len = math.hypot(slam_dx, slam_dy)
if slam_len == 0:
raise RuntimeError("SLAM start and end points are identical; cannot align")
gx1, gy1 = latlon_to_xy(end_lat, end_lon, start_lat, start_lon)
gps_len = math.hypot(gx1, gy1)
if gps_len == 0:
raise RuntimeError("GPS start and end points are identical; cannot align")
scale = gps_len / slam_len
slam_ang = math.atan2(slam_dy, slam_dx)
gps_ang = math.atan2(gy1, gx1)
rot = gps_ang - slam_ang
gpx_points = []
for pose, slam_pt in zip(poses, slam_pts):
timestamp, x, y, z = pose
px = slam_pt[0] - sx0
py = slam_pt[1] - sy0
rx = scale * (px * math.cos(rot) - py * math.sin(rot))
ry = scale * (px * math.sin(rot) + py * math.cos(rot))
lat, lon = xy_to_latlon(rx, ry, start_lat, start_lon)
if elevation_axis == "x":
ele = x * scale
elif elevation_axis == "y":
ele = y * scale
elif elevation_axis == "z":
ele = z * scale
else:
ele = 0.0
gpx_points.append((lat, lon, ele, timestamp))
with open(out_path, "w", encoding="utf-8") as f:
f.write('<?xml version="1.0" encoding="UTF-8"?>\n')
f.write(
'<gpx version="1.1" creator="stella_vslam_to_gpx" '
'xmlns="http://www.topografix.com/GPX/1/1">\n'
)
f.write(" <trk>\n")
f.write(" <name>stella_vslam trajectory</name>\n")
f.write(" <trkseg>\n")
for lat, lon, ele, timestamp in gpx_points:
f.write(f' <trkpt lat="{lat:.8f}" lon="{lon:.8f}">\n')
f.write(f" <ele>{ele:.3f}</ele>\n")
if include_time:
f.write(f" <time>{unix_to_gpx_time(timestamp)}</time>\n")
f.write(" </trkpt>\n")
f.write(" </trkseg>\n")
f.write(" </trk>\n")
f.write("</gpx>\n")
print(f"Wrote: {out_path}")
print(f"Points: {len(gpx_points)}")
print(f"Scale factor: {scale}")
print(f"Plane used: {plane}")
print(f"Time included: {include_time}")
def main():
parser = argparse.ArgumentParser(
description="Convert stella_vslam frame_trajectory.txt to GPX using manual start/end GPS or a reference GPX."
)
parser.add_argument("--traj", required=True, help="Path to frame_trajectory.txt")
parser.add_argument("--out", default="output.gpx", help="Output GPX path")
group = parser.add_mutually_exclusive_group(required=True)
group.add_argument(
"--ref",
help="Reference GPX. First point is start GPS, last point is end GPS.",
)
group.add_argument(
"--start-end",
nargs=2,
metavar=("START_LAT,LON", "END_LAT,LON"),
type=parse_latlon_pair,
help="Manual start and end GPS, e.g. --start-end 12.3,123.4 12.3,123.4",
)
parser.add_argument(
"--plane",
default="xz",
choices=["xz", "xy", "zy", "zx", "negxz", "xnegz"],
help="Which SLAM axes to use as ground plane. Default: xz",
)
parser.add_argument(
"--elevation-axis",
default="y",
choices=["x", "y", "z", "none"],
help="Which SLAM axis to use as GPX elevation. Default: y",
)
parser.add_argument(
"--no-time",
action="store_true",
help="Do not write GPX <time> elements.",
)
args = parser.parse_args()
if args.ref:
start_lat, start_lon, end_lat, end_lon = read_start_end_from_gpx(args.ref)
else:
start, end = args.start_end
start_lat, start_lon = start
end_lat, end_lon = end
convert_to_gpx(
traj_path=args.traj,
out_path=args.out,
start_lat=start_lat,
start_lon=start_lon,
end_lat=end_lat,
end_lon=end_lon,
plane=args.plane,
elevation_axis=args.elevation_axis,
include_time=not args.no_time,
)
if __name__ == "__main__":
main()
) avec une précision de positionnement 5* et prises en voiture où on doit avoir une orientation correcte (sans doute plus fiable qu’en vélo).
