def get_average_point_color(track: SfmTrack,
images: List[Image]) -> Tuple[int, int, int]:
"""
Args:
track: 3d point/landmark and its corresponding 2d measurements in various cameras
images: list of all images for this scene
Returns:
r: red color intensity, in range [0,255]
g: green color intensity, in range [0,255]
b: blue color intensity, in range [0,255]
"""
rgb_measurements = []
for k in range(track.number_measurements()):
# process each measurement
i, uv_measured = track.measurement(k)
u, v = np.round(uv_measured).astype(np.int32)
# ensure round did not push us out of bounds
u = np.clip(u, 0, images[i].width - 1)
v = np.clip(v, 0, images[i].height - 1)
rgb_measurements += [images[i].value_array[v, u]]
r, g, b = np.array(rgb_measurements).mean(axis=0).astype(np.uint8)
return r, g, b
def compute_track_reprojection_errors(
track_camera_dict: Dict[int, PinholeCameraCal3Bundler],
track: SfmTrack) -> Tuple[np.ndarray, float]:
"""Compute reprojection errors for measurements in the tracks.
Args:
track_camera_dict: Dict of cameras, with camera indices as keys.
track: 3d point/landmark and its corresponding 2d measurements in various cameras
Returns:
reprojection errors for each measurement (measured in pixels).
avg_track_reproj_error: average reprojection error of all meausurements in track.
"""
errors = []
for k in range(track.numberMeasurements()):
# process each measurement
i, uv_measured = track.measurement(k)
# get the camera associated with the measurement
camera = track_camera_dict[i]
# Project to camera
uv_reprojected, success_flag = camera.projectSafe(track.point3())
# Projection error in camera
if success_flag:
errors.append(np.linalg.norm(uv_measured - uv_reprojected))
else:
# failure in projection
errors.append(np.nan)
errors = np.array(errors)
avg_track_reproj_error = np.nan if np.isnan(errors).all() else np.nanmean(
errors)
return errors, avg_track_reproj_error
def add_track(self, track: SfmTrack) -> bool:
"""Add a track, after checking if all the cameras in the track are already added.
Args:
track: track to add.
Returns:
Flag indicating the success of adding operation.
"""
# check if all cameras are already added
for j in range(track.number_measurements()):
i, _ = track.measurement(j)
if i not in self._cameras:
return False
self._tracks.append(track)
return True
class TestSfmData(GtsamTestCase):
"""Tests for SfmData and SfmTrack modules."""
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = Point2(24.88, 82)
# add measurements to the track
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(Point3(0., 0., 0.),
self.tracks.point3())
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
def test_Balbianello(self):
""" Check that we can successfully read a bundler file and create a
factor graph from it
"""
# The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
self.assertEqual(5, sfm_data.numberCameras())
self.assertEqual(544, sfm_data.numberTracks())
track0 = sfm_data.track(0)
self.assertEqual(3, track0.numberMeasurements())
# Check projection of a given point
self.assertEqual(0, track0.measurement(0)[0])
camera0 = sfm_data.camera(0)
expected = camera0.project(track0.point3())
actual = track0.measurement(0)[1]
self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(2,
1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model)
self.assertEqual(1419, graph.size()) # regression
# Get initial estimate
values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
self.assertEqual(549, values.size()) # regression