From 82c5ef1a141fe56cf75e2dce95387355f685e1e6 Mon Sep 17 00:00:00 2001 From: Ryan Ashbaugh Date: Mon, 18 Mar 2024 00:13:38 -0400 Subject: [PATCH 1/2] Fixed issue #9 by updating gtsam repo with newer commits as suggested --- thirdparty/gtsam | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/gtsam b/thirdparty/gtsam index 139c156..df2ac90 160000 --- a/thirdparty/gtsam +++ b/thirdparty/gtsam @@ -1 +1 @@ -Subproject commit 139c156a7a6fb5ef6062549993410a8ab986720b +Subproject commit df2ac901edeed0c40768687fc7946e1d45839796 From 84a068a8a0d5b4c6c920f25007bc6b03ea96f07e Mon Sep 17 00:00:00 2001 From: Ryan Ashbaugh Date: Mon, 18 Mar 2024 02:09:27 -0400 Subject: [PATCH 2/2] fixed changes gtsam naming conventions --- slam/vio_slam.py | 3 ++- slam/visual_frontends/visual_frontend.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/slam/vio_slam.py b/slam/vio_slam.py index 5e0f3ac..df84102 100644 --- a/slam/vio_slam.py +++ b/slam/vio_slam.py @@ -62,7 +62,8 @@ def initial_state(): gtsam.Point3(0.878612, 2.142470, 0.947262)) true_vel = np.array([0.009474,-0.014009,-0.002145]) true_bias = gtsam.imuBias.ConstantBias(np.array([-0.012492,0.547666,0.069073]), np.array([-0.002229,0.020700,0.076350])) - naive_pose = gtsam.Pose3.identity() + # naive_pose = gtsam.Pose3.identity() + naive_pose = gtsam.Pose3.Identity() naive_vel = np.zeros(3) naive_bias = gtsam.imuBias.ConstantBias() initial_pose = true_world_T_imu_t0 diff --git a/slam/visual_frontends/visual_frontend.py b/slam/visual_frontends/visual_frontend.py index 5b1a75b..4c630af 100644 --- a/slam/visual_frontends/visual_frontend.py +++ b/slam/visual_frontends/visual_frontend.py @@ -45,8 +45,8 @@ def lietorch_pose_to_gtsam(pose : lietorch.SE3): def gtsam_pose_to_torch(pose: gtsam.Pose3, device, dtype): t = pose.translation() - q = pose.rotation().quaternion() - return torch.tensor([t[0], t[1], t[2], q[1], q[2], q[3], q[0]], device=device, dtype=dtype) + q = pose.rotation().toQuaternion() + return torch.tensor([t[0], t[1], t[2], q.x(), q.y(), q.z(), q.w()], device=device, dtype=dtype) class VisualFrontend(nn.Module): def __init__(self):