Merge branch 'main' into taoshaf.add_SE3.transfrom

theseus/geometry/se3.py

@@ -220,10 +220,12 @@ def _log_map_impl(self) -> torch.Tensor:
             (sine_theta + two_cosine_minus_two) / (theta2_nz * two_cosine_minus_two_nz),
         )
 
+        translation = self[:, :, 3].view(-1, 3, 1)
+        tangent_vector_ang = ret[:, 3:].view(-1, 3, 1)
         ret[:, :3] = a.view(-1, 1) * self[:, :, 3]
         ret[:, :3] -= 0.5 * torch.cross(ret[:, 3:], self[:, :, 3])
         ret[:, :3] += b.view(-1, 1) * (
-            ret[:, 3:].view(-1, 3, 1) @ (ret[:, 3:].view(-1, 1, 3) @ self[:, :, 3:])
+            tangent_vector_ang @ (tangent_vector_ang.transpose(1, 2) @ translation)
         ).view(-1, 3)
 
         return ret

theseus/geometry/tests/test_se3.py

@@ -38,6 +38,7 @@ def test_exp_map():
 
         check_exp_map(tangent_vector, th.SE3)
 
+    # SE3.exp_map uses approximations for small theta
     for batch_size in [1, 20, 100]:
         tangent_vector_ang = torch.rand(batch_size, 3).double() - 0.5
         tangent_vector_ang /= tangent_vector_ang.norm(dim=1, keepdim=True)
@@ -47,6 +48,7 @@ def test_exp_map():
 
         check_exp_map(tangent_vector, th.SE3)
 
+    # SE3.exp_map uses the exact exponential map for small theta
     for batch_size in [1, 20, 100]:
         tangent_vector_ang = torch.rand(batch_size, 3).double() - 0.5
         tangent_vector_ang /= tangent_vector_ang.norm(dim=1, keepdim=True)
@@ -85,6 +87,7 @@ def test_log_map():
 
         check_SE3_log_map(tangent_vector)
 
+    # SE3.log_map uses approximations for small theta
     for batch_size in [1, 20, 100]:
         tangent_vector_ang = torch.rand(batch_size, 3).double() - 0.5
         tangent_vector_ang /= tangent_vector_ang.norm(dim=1, keepdim=True)
@@ -94,6 +97,7 @@ def test_log_map():
 
         check_SE3_log_map(tangent_vector)
 
+    # SE3.log_map uses the exact logarithm map for small theta
     for batch_size in [1, 20, 100]:
         tangent_vector_ang = torch.rand(batch_size, 3).double() - 0.5
         tangent_vector_ang /= tangent_vector_ang.norm(dim=1, keepdim=True)