|
1 | 1 | #!/usr/bin/env python3
|
2 | 2 | import functools
|
3 |
| -import rospy |
4 |
| -import numpy as np |
5 |
| -import matplotlib |
6 |
| -from scipy.spatial.transform import Rotation as R |
7 | 3 |
|
| 4 | +import matplotlib |
8 | 5 | import matplotlib.pyplot as plt
|
9 |
| - |
10 |
| -from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge |
11 |
| - |
| 6 | +import numpy as np |
| 7 | +import rospy |
12 | 8 | import tf2_ros
|
| 9 | +from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge |
| 10 | +from scipy.spatial.transform import Rotation as R |
13 | 11 |
|
14 |
| - |
15 |
| -from vlc_db.vlc_db import VlcDb |
16 |
| -from vlc_db.spark_loop_closure import SparkLoopClosure |
17 |
| -from vlc_db.gt_lc_utils import ( |
| 12 | +import ouroboros as ob |
| 13 | +from ouroboros.vlc_db.gt_lc_utils import ( |
18 | 14 | VlcPose,
|
19 | 15 | compute_descriptor_similarity,
|
20 | 16 | recover_pose,
|
21 | 17 | )
|
22 | 18 |
|
23 |
| - |
24 | 19 | # Hydra takes too long to add agent poses to the backend, so if we send the LC
|
25 | 20 | # immediately it will get rejected. To work around this, we can't send the loop
|
26 | 21 | # closure until several seconds after it is detected
|
@@ -127,7 +122,7 @@ def compute_lc(
|
127 | 122 | match_kps, match_descriptors = vlc_db.get_keypoints(match_uid)
|
128 | 123 |
|
129 | 124 | from_T_to = recover_pose(query_descriptors, match_descriptors)
|
130 |
| - lc = SparkLoopClosure( |
| 125 | + lc = ob.SparkLoopClosure( |
131 | 126 | from_image_uuid=0, to_image_uuid=0, f_T_t=from_T_to, quality=1
|
132 | 127 | )
|
133 | 128 | vlc_db.add_lc(lc, session_id)
|
@@ -193,7 +188,7 @@ def compute_lc(
|
193 | 188 | line = plt.plot([], [], color="g")[0]
|
194 | 189 | points = plt.scatter([], [], color="g")
|
195 | 190 |
|
196 |
| - vlc_db = VlcDb(8) |
| 191 | + vlc_db = ob.VlcDb(8) |
197 | 192 | session_id = vlc_db.add_session(0)
|
198 | 193 |
|
199 | 194 | rate = rospy.Rate(10)
|
|
0 commit comments