Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dump some data association metrics #104

Merged
merged 34 commits into from
Mar 11, 2021
Merged

Dump some data association metrics #104

merged 34 commits into from
Mar 11, 2021

Conversation

johnwlambert
Copy link
Collaborator

  • Make RANSAC more modular in data association
  • Save json file with some basic DA stats
  • Make it so 3d point outliers dont get plotted

@johnwlambert
Copy link
Collaborator Author

bev
3d
poses_bev
poses_3d

Comment on lines 121 to 122
"mean_track_length": mean_track_length,
"median_track_length": median_track_length,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a good reason to have both median and mean? We should try to reduce numbers.
Also, would it be (more) useful to have median_ per_track_average_error?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was just trying to dump as much as possible now, since space is cheap and performance is not great yet :-)

Do you mean median error within each track, or the median of all avg track errors @akshay-krishnan ?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was referring to these two:
"mean_track_length": mean_track_length,
"median_track_length": median_track_length,
If they are the mean and median of the same quantity, we can drop the mean. It does provide a weak signal about outliers, but we can look at min and max if we need that.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We would need to change the SFMResult class to get the min and max stats, so i'd prefer to leave that for another PR

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

On second thought, i added that.

But "mean track length" is a metric I've seen in published papers like COLMAP so I'd like to keep it for now

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Then we probably won't need median. We'll ultimately end up using one number for the central tendency (bias) and one for the range/variance. But its upto you.


# compute reprojection errors for each measurement
reproj_errors = self.compute_track_reprojection_errors(inlier_track.measurements, triangulated_pt)

# all the measurements should have error < threshold
if not np.all(reproj_errors < self.reproj_error_thresh):
return None
return None, None
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since we have reprojection errors, cant we return the mean here?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's a good point. I was a bit torn whether we should dump statistics of accepted tracks, or of any generated track, even if rejected.

@ayushbaid any thoughts here? maybe we should dump both for completeness now, since DA's failures are a bit of a mystery?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I think having more data right now is useful to debug our DA module.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But I would like a distinction on the errors when we dump it to the file. Like separate sections for rejected v accepted tracks.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I think the error should include failures as well, unless we are tracking them separately. Tracking them separately (another metric) might be the better choice. Maybe accepted_tracks_ratio?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cool, I added these extra metrics now

init_cameras_graph, v_corr_idxs_graph, keypoints_graph
)
ba_input_graph = data_assoc_graph[0]
data_assoc_metrics_graph = data_assoc_graph[1]
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ba_input_graph, data_assoc_metrics_graph = self.data_association_module.create_computation_graph(
            init_cameras_graph, v_corr_idxs_graph, keypoints_graph
        )

Isn't that better?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dask won't allow this unfortunately:

Traceback (most recent call last):
  File "gtsfm/runner/run_scene_optimizer_argoverse.py", line 97, in <module>
    run_scene_optimizer(args)
  File "gtsfm/runner/run_scene_optimizer_argoverse.py", line 37, in run_scene_optimizer
    sfm_result_graph = scene_optimizer.create_computation_graph(
  File "/Users/johnlambert/Documents/gtsfm/gtsfm/scene_optimizer.py", line 184, in create_computation_graph
    (ba_input_graph, ba_output_graph, optimizer_metrics_graph, ) = self.multiview_optimizer.create_computation_graph(
  File "/Users/johnlambert/Documents/gtsfm/gtsfm/multi_view_optimizer.py", line 81, in create_computation_graph
    ba_input_graph, data_assoc_metrics_graph = self.data_association_module.create_computation_graph(
  File "/Users/johnlambert/anaconda3/envs/gtsfm-v1/lib/python3.8/site-packages/dask/delayed.py", line 562, in __iter__
    raise TypeError("Delayed objects of unspecified length are not iterable")
TypeError: Delayed objects of unspecified length are not iterable

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This logic should be moved to DataAssociation's create_computation_graph.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm good with moving it, but is there a particular reason to put it in one place rather than the other?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think its more clear seeing the semantics on what to expect. If a module has three Delayed being returned, we can create richer docstrings.

gtsfm/utils/geometry_comparisons.py Show resolved Hide resolved
gtsfm/utils/geometry_comparisons.py Outdated Show resolved Hide resolved
gtsfm/utils/geometry_comparisons.py Show resolved Hide resolved
gtsfm/utils/geometry_comparisons.py Outdated Show resolved Hide resolved
Comment on lines 121 to 122
"mean_track_length": mean_track_length,
"median_track_length": median_track_length,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was referring to these two:
"mean_track_length": mean_track_length,
"median_track_length": median_track_length,
If they are the mean and median of the same quantity, we can drop the mean. It does provide a weak signal about outliers, but we can look at min and max if we need that.


# compute reprojection errors for each measurement
reproj_errors = self.compute_track_reprojection_errors(inlier_track.measurements, triangulated_pt)

# all the measurements should have error < threshold
if not np.all(reproj_errors < self.reproj_error_thresh):
return None
return None, None
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I think the error should include failures as well, unless we are tracking them separately. Tracking them separately (another metric) might be the better choice. Maybe accepted_tracks_ratio?

gtsfm/utils/geometry_comparisons.py Show resolved Hide resolved
gtsfm/data_association/point3d_initializer.py Outdated Show resolved Hide resolved
camera_estimates.append(self.track_camera_dict.get(i1))
camera_estimates.append(self.track_camera_dict.get(i2))

img_measurements = Point2Vector()
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cant we just do img_measurements = [uv1, uv2]

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

GTSAM requires the Point2Vector type as argument

Comment on lines 95 to 96
camera_estimates.append(self.track_camera_dict.get(i1))
camera_estimates.append(self.track_camera_dict.get(i2))
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cant we do camera_estimates = [self.track_camera_dict.get(i1), self.track_camera_dict.get(i2)]?

Comment on lines 87 to 117
k1, k2 = measurement_pairs[sample_idxs]

i1, uv1 = track_2d.measurements[k1]
i2, uv2 = track_2d.measurements[k2]

camera_estimates = CameraSetCal3Bundler()
# check for unestimated cameras
if self.track_camera_dict.get(i1) != None and self.track_camera_dict.get(i2) != None:
camera_estimates.append(self.track_camera_dict.get(i1))
camera_estimates.append(self.track_camera_dict.get(i2))

img_measurements = Point2Vector()
img_measurements.append(uv1)
img_measurements.append(uv2)

# triangulate point for track
try:
triangulated_pt = gtsam.triangulatePoint3(
camera_estimates,
img_measurements,
rank_tol=SVD_DLT_RANK_TOL,
optimize=True,
)
except RuntimeError:
# TODO: handle cheirality exception properly?
logger.info(
"Cheirality exception from GTSAM's triangulatePoint3() likely due to outlier, skipping track"
)
continue

errors = self.compute_track_reprojection_errors(track_2d.measurements, triangulated_pt)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it would be more modular and cleaner if you made the above changes, and moved this code to a different function compute_reprojection_error_for_hypothesis(track_2d.measurements, k1, k2)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@akshay-krishnan can you point me to which line you were referring to?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was referring to all the lines I selected. Basically, the code for computing the errors for each sample_idx could be moved to a different function as this function is getting a little too big. But it's fine if you'd like to keep it this way.

gtsfm/multi_view_optimizer.py Outdated Show resolved Hide resolved
gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
gtsfm/data_association/point3d_initializer.py Show resolved Hide resolved

# compute reprojection errors for each measurement
reproj_errors = self.compute_track_reprojection_errors(inlier_track.measurements, triangulated_pt)

# all the measurements should have error < threshold
if not np.all(reproj_errors < self.reproj_error_thresh):
return None
return None, None
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I think having more data right now is useful to debug our DA module.


# compute reprojection errors for each measurement
reproj_errors = self.compute_track_reprojection_errors(inlier_track.measurements, triangulated_pt)

# all the measurements should have error < threshold
if not np.all(reproj_errors < self.reproj_error_thresh):
return None
return None, None
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But I would like a distinction on the errors when we dump it to the file. Like separate sections for rejected v accepted tracks.

gtsfm/data_association/point3d_initializer.py Outdated Show resolved Hide resolved
)
except RuntimeError:
# TODO: handle cheirality exception properly?
logger.info(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do we have a way to log the counts of this exception?

@johnwlambert
Copy link
Collaborator Author

Dumps something like:

{
    "mean_2d_track_length": 2.8,
    "accepted_tracks_ratio": 0.977,
    "track_cheirality_failure_ratio": 0.007,
    "num_accepted_tracks": 2498,
    "mean_3d_track_length": 2.404,
    "median_3d_track_length": 2.0,
    "per_rejected_track_avg_errors": [
        null,
        null,
        null,
...
}

gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
"num_len_10_tracks": int(np.sum(track_lengths_3d == 10)),
"per_rejected_track_avg_errors": per_rejected_track_avg_errors,
"per_accepted_track_avg_errors": per_accepted_track_avg_errors,
"points_3d": points_3d,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why dump just 3d points? Why not dump the track?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was visualizing them in Mayavi on my local machine for debugging.

I recommend in a separate PR we dump the output in COLMAP-format. Then we could remove my points_3d dump later.

gtsfm/data_association/point3d_initializer.py Show resolved Hide resolved
gtsfm/data_association/point3d_initializer.py Show resolved Hide resolved
init_cameras_graph, v_corr_idxs_graph, keypoints_graph
)
ba_input_graph = data_assoc_graph[0]
data_assoc_metrics_graph = data_assoc_graph[1]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This logic should be moved to DataAssociation's create_computation_graph.

gtsfm/utils/geometry_comparisons.py Show resolved Hide resolved
Comment on lines +147 to +201
def test_get_points_within_radius_of_cameras():
"""Verify that points that fall outside of 10 meter radius of two camera poses.

Cameras are placed at (0,0,0) and (10,0,0).
"""
wTi0 = Pose3(Rot3(), np.zeros(3))
wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0]))
wTi_list = [wTi0, wTi1]
points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]])
radius = 10.0
nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras(wTi_list, points_3d, radius)

expected_nearby_points_3d = np.array(
[
[-5, 0, 0],
[15, 0, 0],
]
)
np.testing.assert_allclose(nearby_points_3d, expected_nearby_points_3d)


def test_get_points_within_radius_of_cameras_negative_radius():
"""Catch degenerate input."""
wTi0 = Pose3(Rot3(), np.zeros(3))
wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0]))
wTi_list = [wTi0, wTi1]
points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]])
radius = -5
nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras(wTi_list, points_3d, radius)
assert nearby_points_3d is None, "Non-positive radius is not allowed"


def test_get_points_within_radius_of_cameras_no_points():
"""Catch degenerate input."""

wTi0 = Pose3(Rot3(), np.zeros(3))
wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0]))
wTi_list = [wTi0, wTi1]
points_3d = np.zeros((0, 3))
radius = 10.0

nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras(wTi_list, points_3d, radius)
assert nearby_points_3d is None, "At least one 3d point must be provided"


def test_get_points_within_radius_of_cameras_no_poses():
"""Catch degenerate input."""
wTi_list = []
points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]])
radius = 10.0

nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras(wTi_list, points_3d, radius)
assert nearby_points_3d is None, "At least one camera pose must be provided"


Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should have these tests inside the class as we have done it in other tests.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm fine either way. Technically it's not a class, so i was going more for the functional, free-function approach

Copy link
Collaborator

@akshay-krishnan akshay-krishnan left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good John, adding some optional comments for improved readability.

gtsfm/data_association/point3d_initializer.py Outdated Show resolved Hide resolved
gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
Comment on lines 150 to 151
"per_rejected_track_avg_errors": per_rejected_track_avg_errors,
"per_accepted_track_avg_errors": per_accepted_track_avg_errors,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just curious, why per_ here?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't have a good way of differentiating between names for
(1) average error within a track
(2) average of per-track avg errors

but i'm open to suggestions


logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks)
logger.debug("[Data association] output avg. track length: %s", mean_3d_track_length)

# dump the 3d point cloud before Bundle Adjustment for offline visualization
points_3d = [list(triangulated_data.track(j).point3()) for j in range(num_accepted_tracks)]
# bin edges are halfway between each integer
histogram_track_lengths, _ = np.histogram(track_lengths_3d, bins=np.linspace(-0.5, 10.5, 12))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

track_lengths_histogram?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, why bin it from -0.5. We can use the np.histogram(track_lengths_3d, bins=8, range=(2, 10))

Copy link
Contributor

@ayushbaid ayushbaid left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Left some small suggestions. Looks good otherwise.


logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks)
logger.debug("[Data association] output avg. track length: %s", mean_3d_track_length)

# dump the 3d point cloud before Bundle Adjustment for offline visualization
points_3d = [list(triangulated_data.track(j).point3()) for j in range(num_accepted_tracks)]
# bin edges are halfway between each integer
histogram_track_lengths, _ = np.histogram(track_lengths_3d, bins=np.linspace(-0.5, 10.5, 12))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, why bin it from -0.5. We can use the np.histogram(track_lengths_3d, bins=8, range=(2, 10))

gtsfm/data_association/data_assoc.py Outdated Show resolved Hide resolved
init_cameras_graph, v_corr_idxs_graph, keypoints_graph
)
ba_input_graph = data_assoc_graph[0]
data_assoc_metrics_graph = data_assoc_graph[1]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think its more clear seeing the semantics on what to expect. If a module has three Delayed being returned, we can create richer docstrings.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants