refactor: load reference as a converter object

bin/export_gps

@@ -20,7 +20,7 @@ args = parser.parse_args()
 
 # Load data
 data = dataset.DataSet(args.dataset)
-reference = data.load_reference_lla()
+reference = data.load_reference()
 reconstructions = data.load_reconstruction()
 
 # Compute lat, lon

bin/update_geotag

@@ -36,7 +36,7 @@ args = parser.parse_args()
 data_path = args.dataset
 
 data = dataset.DataSet(data_path)
-reference = data.load_reference_lla()
+reference = data.load_reference()
 
 updated_image_path = os.path.join(data.data_path, 'images_updated_geotag')
 if not args.overwrite_originals and not os.path.exists(updated_image_path):

opensfm/commands/export_geocoords.py

@@ -55,7 +55,7 @@ def run(self, args):
             print(' --transformation, --image-positions, --reconstruction, --dense')
 
         data = dataset.DataSet(args.dataset)
-        reference = data.load_reference_lla()
+        reference = data.load_reference()
 
         projection = pyproj.Proj(args.proj)
         transformation = self._get_transformation(reference, projection)
@@ -108,9 +108,7 @@ def _write_transformation(self, transformation, filename):
 
     def _transform(self, point, reference, projection):
         """Transform on point from local coords to a proj4 projection."""
-        lat, lon, altitude = geo.lla_from_topocentric(
-            point[0], point[1], point[2],
-            reference['latitude'], reference['longitude'], reference['altitude'])
+        lat, lon, altitude = reference.to_lla(point[0], point[1], point[2])
         easting, northing = projection(lon, lat)
         return [easting, northing, altitude]
 

opensfm/commands/match_features.py

@@ -107,9 +107,8 @@ def match_candidates_by_distance(images, exifs, reference, max_neighbors, max_di
     points = np.zeros((len(images), 3))
     for i, image in enumerate(images):
         gps = exifs[image]['gps']
-        points[i] = geo.topocentric_from_lla(
-            gps['latitude'], gps['longitude'], 0,
-            reference['latitude'], reference['longitude'], 0)
+        points[i] = reference.to_topocentric(
+            gps['latitude'], gps['longitude'], 0)
 
     tree = spatial.cKDTree(points)
 
@@ -169,7 +168,7 @@ def match_candidates_from_metadata(images, exifs, data):
 
     if not data.reference_lla_exists():
         data.invent_reference_lla()
-    reference = data.load_reference_lla()
+    reference = data.load_reference()
 
     if not all(map(has_gps_info, exifs.values())):
         if gps_neighbors != 0:

opensfm/dataset.py

@@ -14,6 +14,7 @@
 from opensfm import io
 from opensfm import config
 from opensfm import context
+from opensfm import geo
 
 
 logger = logging.getLogger(__name__)
@@ -563,6 +564,12 @@ def load_reference_lla(self):
         with io.open_rt(self._reference_lla_path()) as fin:
             return io.json_load(fin)
 
+    def load_reference(self):
+        """Load reference as a topocentric converter."""
+        lla = self.load_reference_lla()
+        return geo.TopocentricConverter(
+            lla['latitude'], lla['longitude'], lla['altitude'])
+
     def reference_lla_exists(self):
         return os.path.isfile(self._reference_lla_path())
 
@@ -665,10 +672,10 @@ def load_ground_control_points(self):
         to topocentric reference frame.
         """
         exif = {image: self.load_exif(image) for image in self.images()}
+        reference = self.load_reference()
 
         with io.open_rt(self._ground_control_points_file()) as fin:
-            return io.read_ground_control_points_list(
-                fin, self.load_reference_lla(), exif)
+            return io.read_ground_control_points_list(fin, reference, exif)
 
     def image_as_array(self, image):
         logger.warning("image_as_array() is deprecated. Use load_image() instead.")

opensfm/geo.py

@@ -146,3 +146,23 @@ def gps_distance(latlon_1, latlon_2):
     dis = np.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
 
     return dis
+
+
+class TopocentricConverter(object):
+    """Convert to and from a topocentric reference frame."""
+
+    def __init__(self, reflat, reflon, refalt):
+        """Init the converter given the reference origin."""
+        self.lat = reflat
+        self.lon = reflon
+        self.alt = refalt
+
+    def to_topocentric(self, lat, lon, alt):
+        """Convert lat, lon, alt to topocentric x, y, z."""
+        return topocentric_from_lla(lat, lon, alt,
+                                    self.lat, self.lon, self.alt)
+
+    def to_lla(self, x, y, z):
+        """Convert topocentric x, y, z to lat, lon, alt."""
+        return lla_from_topocentric(x, y, z,
+                                    self.lat, self.lon, self.alt)

opensfm/io.py

@@ -353,7 +353,7 @@ def cameras_to_json(cameras):
     return obj
 
 
-def _read_gcp_list_line(line, projection, reference_lla, exif):
+def _read_gcp_list_line(line, projection, reference, exif):
     words = line.split()
     easting, northing, alt, pixel_x, pixel_y = map(float, words[:5])
     shot_id = words[5]
@@ -363,11 +363,7 @@ def _read_gcp_list_line(line, projection, reference_lla, exif):
         lon, lat = projection(easting, northing, inverse=True)
     else:
         lon, lat = easting, northing
-    x, y, z = geo.topocentric_from_lla(
-        lat, lon, alt,
-        reference_lla['latitude'],
-        reference_lla['longitude'],
-        reference_lla['altitude'])
+    x, y, z = reference.to_topocentric(lat, lon, alt)
 
     # Convert 2D coordinates
     d = exif[shot_id]
@@ -417,15 +413,15 @@ def _valid_gcp_line(line):
     return stripped and stripped[0] != '#'
 
 
-def read_ground_control_points_list(fileobj, reference_lla, exif):
+def read_ground_control_points_list(fileobj, reference, exif):
     """Read a ground control point list file.
 
     It requires the points to be in the WGS84 lat, lon, alt format.
     """
     all_lines = fileobj.readlines()
     lines = iter(filter(_valid_gcp_line, all_lines))
     projection = _parse_projection(next(lines))
-    points = [_read_gcp_list_line(line, projection, reference_lla, exif)
+    points = [_read_gcp_list_line(line, projection, reference, exif)
               for line in lines]
     return points
 

opensfm/large/tools.py

@@ -34,13 +34,12 @@ def kmeans(samples, nclusters, max_iter=100, attempts=20):
 
 
 def add_cluster_neighbors(positions, labels, centers, max_distance):
-    reference = np.mean(positions, 0)
+    reflla = np.mean(positions, 0)
+    reference = geo.TopocentricConverter(reflla[0], reflla[1], 0)
 
     topocentrics = []
     for position in positions:
-        x, y, z = geo.topocentric_from_lla(
-            position[0], position[1], 0,
-            reference[0], reference[1], 0)
+        x, y, z = reference.to_topocentric(position[0], position[1], 0)
         topocentrics.append([x, y])
 
     topocentrics = np.array(topocentrics)

opensfm/reconstruction.py

@@ -445,7 +445,7 @@ def get_image_metadata(data, image):
     """Get image metadata as a ShotMetadata object."""
     metadata = types.ShotMetadata()
     exif = data.load_exif(image)
-    reflla = data.load_reference_lla()
+    reference = data.load_reference()
     if ('gps' in exif and
             'latitude' in exif['gps'] and
             'longitude' in exif['gps']):
@@ -455,9 +455,7 @@ def get_image_metadata(data, image):
             alt = exif['gps'].get('altitude', 2.0)
         else:
             alt = 2.0  # Arbitrary value used to align the reconstruction
-        x, y, z = geo.topocentric_from_lla(
-            lat, lon, alt,
-            reflla['latitude'], reflla['longitude'], reflla['altitude'])
+        x, y, z = reference.to_topocentric(lat, lon, alt)
         metadata.gps_position = [x, y, z]
         metadata.gps_dop = exif['gps'].get('dop', 15.0)
     else:
@@ -931,9 +929,7 @@ def remove_outliers(graph, reconstruction, config):
 def shot_lla_and_compass(shot, reference):
     """Lat, lon, alt and compass of the reconstructed shot position."""
     topo = shot.pose.get_origin()
-    lat, lon, alt = geo.lla_from_topocentric(
-        topo[0], topo[1], topo[2],
-        reference['latitude'], reference['longitude'], reference['altitude'])
+    lat, lon, alt = reference.to_lla(*topo)
 
     dz = shot.viewing_direction()
     angle = np.rad2deg(np.arctan2(dz[0], dz[1]))

opensfm/synthetic_data/synthetic_generator.py

@@ -133,7 +133,7 @@ def generate_exifs(reconstruction, gps_noise, speed_ms=10):
     previous_pose = None
     previous_time = 0
     exifs = {}
-    reference = {'latitude': 0, 'longitude': 0, 'altitude': 0}
+    reference = geo.TopocentricConverter(0, 0, 0)
     for shot_name in sorted(reconstruction.shots.keys()):
         shot = reconstruction.shots[shot_name]
         exif = {}
@@ -153,8 +153,7 @@ def generate_exifs(reconstruction, gps_noise, speed_ms=10):
 
         shot_copy = copy.deepcopy(shot)
         shot_copy.pose.set_origin(pose)
-        lat, lon, alt, comp = rc.shot_lla_and_compass(shot_copy,
-                                                      reference)
+        lat, lon, alt, comp = rc.shot_lla_and_compass(shot_copy, reference)
 
         exif['gps'] = {}
         exif['gps']['latitude'] = lat