1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
|
#include <pybind11/pybind11.h>
#include <pybind11/operators.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include "manif/SE3.h"
#include "bindings_optional.h"
#include "bindings_lie_group_base.h"
#include "bindings_tangent_base.h"
namespace py = pybind11;
void wrap_SE3(py::module &m)
{
using SE3d = manif::SE3d;
using Scalar = SE3d::Scalar;
using Quaternion = Eigen::Quaternion<Scalar>;
py::class_<manif::LieGroupBase<SE3d>, std::unique_ptr<manif::LieGroupBase<SE3d>, py::nodelete>> SE3_base(m, "_SE3Base");
py::class_<manif::TangentBase<manif::SE3Tangentd>, std::unique_ptr<manif::TangentBase<manif::SE3Tangentd>, py::nodelete>> SE3_tan_base(m, "_SE3TangentBase");
py::class_<SE3d, manif::LieGroupBase<SE3d>> SE3(m, "SE3");
py::class_<manif::SE3Tangentd, manif::TangentBase<manif::SE3Tangentd>> SE3_tan(m, "SE3Tangent");
// group
wrap_lie_group_base<SE3d, manif::LieGroupBase<SE3d>>(SE3);
SE3.def(py::init<const Scalar, const Scalar, const Scalar,
const Scalar, const Scalar, const Scalar>());
SE3.def(py::init([](const SE3d::Translation& pos, const Eigen::Matrix<Scalar, 4, 1>& quat) {
if(abs(quat.norm() - Scalar(1)) >= manif::Constants<Scalar>::eps) {
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>&>());
SE3.def("transform", &SE3d::transform);
// SE3.def("isometry", &SE3d::isometry);
SE3.def("rotation", &SE3d::rotation);
SE3.def(
"translation",
static_cast<SE3d::Translation (SE3d::*)(void) const>(&SE3d::translation));
SE3.def(
"translation",
static_cast<void (SE3d::*)(const SE3d::Translation&)>(&SE3d::translation),
py::arg("translation"));
SE3.def(
"quat",
[](const manif::SE3d& se3) -> Eigen::Matrix<Scalar, 4, 1> { return se3.coeffs().segment<4>(3); });
SE3.def(
"quat",
[](manif::SE3d& se3, const Eigen::Matrix<Scalar, 4, 1>& quaternion) {
if(abs(quaternion.norm() - Scalar(1)) >= manif::Constants<Scalar>::eps) {
throw pybind11::value_error("The quaternion is not normalized!");
}
se3.quat(quaternion);
},
py::arg("quaternion"));
SE3.def("x", &SE3d::x);
SE3.def("y", &SE3d::y);
SE3.def("z", &SE3d::z);
SE3.def("normalize", &SE3d::normalize);
//tangent
wrap_tangent_base<manif::SE3Tangentd, manif::TangentBase<manif::SE3Tangentd>>(SE3_tan);
// SE3_tan.def("v", &manif::SE3Tangentd::v);
// SE3_tan.def("w", &manif::SE3Tangentd::w);
}
|
