diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 0baa6bbe..bc56c074 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -90,4 +90,5 @@ jobs: --output_dir outputs/quickstart_test --visualize 0 python runners/hypersim/triangulation.py --default_config_file cfgs/triangulation/default_fast.yaml \ --output_dir outputs/quickstart_test --triangulation.use_exhaustive_matcher --skip_exists --visualize 0 + python runners/tests/localization.py diff --git a/runners/tests/localization.py b/runners/tests/localization.py index 2eb1fcec..b8c13c99 100644 --- a/runners/tests/localization.py +++ b/runners/tests/localization.py @@ -132,17 +132,21 @@ def main(): log += f"GT Pose (qvec, tvec): \ {data['pose_gt'].qvec}, {data['pose_gt'].tvec}\n\n" - R, t = final_pose.R(), final_pose.tvec + R, t = data["pose_point"].R(), data["pose_point"].tvec e_t = np.linalg.norm(-R_gt.T @ t_gt + R.T @ t, axis=0) cos = np.clip((np.trace(np.dot(R_gt.T, R)) - 1) / 2, -1.0, 1.0) e_R = np.rad2deg(np.abs(np.arccos(cos))) - log += f"Result(P+L) Pose errors: {e_t:.3f}m, {e_R:.3f}deg\n" + log += f"HLoc(Point) Pose errors: {e_t:.3f}m, {e_R:.3f}deg\n" + np.testing.assert_(e_t < 0.5, f"e_t = {e_t:.3f}m") + np.testing.assert_(e_R < 2.0, f"e_R = {e_R:.3f}deg") - R, t = data["pose_point"].R(), data["pose_point"].tvec + R, t = final_pose.R(), final_pose.tvec e_t = np.linalg.norm(-R_gt.T @ t_gt + R.T @ t, axis=0) cos = np.clip((np.trace(np.dot(R_gt.T, R)) - 1) / 2, -1.0, 1.0) e_R = np.rad2deg(np.abs(np.arccos(cos))) - log += f"HLoc(Point) Pose errors: {e_t:.3f}m, {e_R:.3f}deg" + log += f"Result(P+L) Pose errors: {e_t:.3f}m, {e_R:.3f}deg" + np.testing.assert_(e_t < 0.1, f"e_t = {e_t:.3f}m") + np.testing.assert_(e_R < 2.0, f"e_R = {e_R:.3f}deg") logger.info(log)