diff --git a/cpp/sophus/lie/pose3.h b/cpp/sophus/lie/pose3.h index ba5fd8ae..8fb7b152 100644 --- a/cpp/sophus/lie/pose3.h +++ b/cpp/sophus/lie/pose3.h @@ -105,7 +105,16 @@ class Pose3 { static Expected 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 operator*(Pose3 const& lhs, Pose3 const& rhs) { diff --git a/py/pybind/lie_pybind.cpp b/py/pybind/lie_pybind.cpp index 8e6b1a87..0cf101cc 100644 --- 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 { farm_ng::Expected 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 index a412149f..7b78a587 100644 --- 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())