UB fix attempt #2

farm-ng · Dec 18, 2024 · b5a2a12 · b5a2a12
1 parent 2b76bc8
commit b5a2a12
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 4 deletions.
diff --git a/cpp/sophus/lie/pose3.h b/cpp/sophus/lie/pose3.h
@@ -105,7 +105,16 @@ class Pose3 {
   static Expected<Tangent> error(
       Pose3 const& lhs_a_from_b, Pose3 const& rhs_a_from_b) {
     FARM_TRY(Pose3, product, lhs_a_from_b.inverse() * rhs_a_from_b);
-    return product->log();
+
+    FARM_INFO(
+        "lhs:\n {}\n rhs: {}\n prod: {}",
+        lhs_a_from_b.a_from_b_.matrix(),
+        rhs_a_from_b.a_from_b_.matrix(),
+        product.a_from_b_.matrix());
+    Tangent log =  product.log();
+
+    FARM_INFO("log: {}", log);
+    return log;
   }
 
   friend Expected<Pose3> operator*(Pose3 const& lhs, Pose3 const& rhs) {

diff --git a/py/pybind/lie_pybind.cpp b/py/pybind/lie_pybind.cpp
@@ -332,7 +332,7 @@ void bind_lie(py::module_& m) {
           })
       .def_static(
           "error",
-          [](Pose3F64 const& lhs_a_from_b, Pose3F64 const& rhs_a_from_b) {
+          [](Pose3F64 const& lhs_a_from_b, Pose3F64 const& rhs_a_from_b) -> Eigen::Vector<double, 6> {
             farm_ng::Expected<Pose3F64::Tangent> err =
                 Pose3F64::error(lhs_a_from_b, rhs_a_from_b);
             if (err) {

diff --git a/py/tests/test_lie.py b/py/tests/test_lie.py
@@ -119,11 +119,18 @@ def test_pose():
     )
 
     a_from_c = a_from_b * b_from_c
+    print("a_from_c", a_from_c)
     print("woot", ng.Rotation3F64.from_proto(a_from_c.rotation.to_proto()).to_proto())
     ng.Pose3F64.from_proto(a_from_c.to_proto())
     print("Here!", a_from_c.log())
-    assert a_from_c.frame_a == "a"
-    assert a_from_c.frame_b == "c"
+
+    try:
+        assert a_from_c.frame_a == "a"
+    except ValueError:
+        print("something went wrong")
+        assert False
+
+
 
     assert np.allclose(a_from_c.log(), ng.Isometry3F64().log())