Add the possibility to construct a SE3 from two vectors in python bin…

…dings
orcilano · Mar 16, 2021 · a802537 · a802537
1 parent 2bce485
commit a802537
Showing 1 changed file with 9 additions and 1 deletion.
diff --git a/python/bindings_se3.cpp b/python/bindings_se3.cpp
@@ -29,7 +29,15 @@ void wrap_SE3(py::module &m)
 
   SE3.def(py::init<const Scalar, const Scalar, const Scalar,
                    const Scalar, const Scalar, const Scalar>());
-  // SE3.def(py::init<const Translation&, const Quaternion&>());
+  SE3.def(py::init([](const SE3d::Translation& pos, const Eigen::Vector4d& quat) {
+                       if(abs(quat.norm() - Scalar(1)) >= manif::Constants<Scalar>::eps_s) {
+                           throw pybind11::value_error("The quaternion is not normalized!");
+                       }
+                       return manif::SE3d(pos, quat);
+                   }),
+      py::arg("position"),
+      py::arg("quaternion"));
+
   // SE3.def(py::init<const Translation&, const Eigen::AngleAxis<Scalar>&>());
   // SE3.def(py::init<const Translation&, const manif::SO3<Scalar>&>());
   // SE3.def(py::init<igen::Transform<Scalar, 3, Eigen::Isometry>&>());