123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835 |
- #ifndef SOPHUS_SE2_HPP
- #define SOPHUS_SE2_HPP
- #include "so2.hpp"
- namespace Sophus {
- template <class Scalar_, int Options = 0>
- class SE2;
- using SE2d = SE2<double>;
- using SE2f = SE2<float>;
- }
- namespace Eigen {
- namespace internal {
- template <class Scalar_, int Options>
- struct traits<Sophus::SE2<Scalar_, Options>> {
- using Scalar = Scalar_;
- using TranslationType = Sophus::Vector2<Scalar, Options>;
- using SO2Type = Sophus::SO2<Scalar, Options>;
- };
- template <class Scalar_, int Options>
- struct traits<Map<Sophus::SE2<Scalar_>, Options>>
- : traits<Sophus::SE2<Scalar_, Options>> {
- using Scalar = Scalar_;
- using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
- using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
- };
- template <class Scalar_, int Options>
- struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
- : traits<Sophus::SE2<Scalar_, Options> const> {
- using Scalar = Scalar_;
- using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
- using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;
- };
- }
- }
- namespace Sophus {
- template <class Derived>
- class SE2Base {
- public:
- using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
- using TranslationType =
- typename Eigen::internal::traits<Derived>::TranslationType;
- using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;
-
-
- static int constexpr DoF = 3;
-
-
- static int constexpr num_parameters = 4;
-
- static int constexpr N = 3;
- using Transformation = Matrix<Scalar, N, N>;
- using Point = Vector2<Scalar>;
- using HomogeneousPoint = Vector3<Scalar>;
- using Line = ParametrizedLine2<Scalar>;
- using Tangent = Vector<Scalar, DoF>;
- using Adjoint = Matrix<Scalar, DoF, DoF>;
-
-
-
-
- template <typename OtherDerived>
- using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
- Scalar, typename OtherDerived::Scalar>::ReturnType;
- template <typename OtherDerived>
- using SE2Product = SE2<ReturnScalar<OtherDerived>>;
- template <typename PointDerived>
- using PointProduct = Vector2<ReturnScalar<PointDerived>>;
- template <typename HPointDerived>
- using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
-
-
-
-
-
-
- SOPHUS_FUNC Adjoint Adj() const {
- Matrix<Scalar, 2, 2> const& R = so2().matrix();
- Transformation res;
- res.setIdentity();
- res.template topLeftCorner<2, 2>() = R;
- res(0, 2) = translation()[1];
- res(1, 2) = -translation()[0];
- return res;
- }
-
-
- template <class NewScalarType>
- SOPHUS_FUNC SE2<NewScalarType> cast() const {
- return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
- translation().template cast<NewScalarType>());
- }
-
-
- SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
- const {
- Matrix<Scalar, num_parameters, DoF> J;
- Sophus::Vector2<Scalar> const c = unit_complex();
- Scalar o(0);
- J(0, 0) = o;
- J(0, 1) = o;
- J(0, 2) = -c[1];
- J(1, 0) = o;
- J(1, 1) = o;
- J(1, 2) = c[0];
- J(2, 0) = c[0];
- J(2, 1) = -c[1];
- J(2, 2) = o;
- J(3, 0) = c[1];
- J(3, 1) = c[0];
- J(3, 2) = o;
- return J;
- }
-
-
- SOPHUS_FUNC SE2<Scalar> inverse() const {
- SO2<Scalar> const invR = so2().inverse();
- return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));
- }
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Tangent log() const {
- using std::abs;
- Tangent upsilon_theta;
- Scalar theta = so2().log();
- upsilon_theta[2] = theta;
- Scalar halftheta = Scalar(0.5) * theta;
- Scalar halftheta_by_tan_of_halftheta;
- Vector2<Scalar> z = so2().unit_complex();
- Scalar real_minus_one = z.x() - Scalar(1.);
- if (abs(real_minus_one) < Constants<Scalar>::epsilon()) {
- halftheta_by_tan_of_halftheta =
- Scalar(1.) - Scalar(1. / 12) * theta * theta;
- } else {
- halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
- }
- Matrix<Scalar, 2, 2> V_inv;
- V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,
- halftheta_by_tan_of_halftheta;
- upsilon_theta.template head<2>() = V_inv * translation();
- return upsilon_theta;
- }
-
-
-
-
- SOPHUS_FUNC void normalize() { so2().normalize(); }
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Transformation matrix() const {
- Transformation homogenious_matrix;
- homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
- homogenious_matrix.row(2) =
- Matrix<Scalar, 1, 3>(Scalar(0), Scalar(0), Scalar(1));
- return homogenious_matrix;
- }
-
-
- SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
- Matrix<Scalar, 2, 3> matrix;
- matrix.template topLeftCorner<2, 2>() = rotationMatrix();
- matrix.col(2) = translation();
- return matrix;
- }
-
-
- template <class OtherDerived>
- SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {
- so2() = other.so2();
- translation() = other.translation();
- return *this;
- }
-
-
- template <typename OtherDerived>
- SOPHUS_FUNC SE2Product<OtherDerived> operator*(
- SE2Base<OtherDerived> const& other) const {
- return SE2Product<OtherDerived>(
- so2() * other.so2(), translation() + so2() * other.translation());
- }
-
-
-
-
-
-
-
-
- template <typename PointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<PointDerived, 2>::value>::type>
- SOPHUS_FUNC PointProduct<PointDerived> operator*(
- Eigen::MatrixBase<PointDerived> const& p) const {
- return so2() * p + translation();
- }
-
-
- template <typename HPointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<HPointDerived, 3>::value>::type>
- SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
- Eigen::MatrixBase<HPointDerived> const& p) const {
- const PointProduct<HPointDerived> tp =
- so2() * p.template head<2>() + p(2) * translation();
- return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));
- }
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Line operator*(Line const& l) const {
- return Line((*this) * l.origin(), so2() * l.direction());
- }
-
-
-
- template <typename OtherDerived,
- typename = typename std::enable_if<
- std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
- SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const& other) {
- return *this;
- }
-
-
-
-
-
- SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
- Sophus::Vector<Scalar, num_parameters> p;
- p << so2().params(), translation();
- return p;
- }
-
-
- SOPHUS_FUNC Matrix<Scalar, 2, 2> rotationMatrix() const {
- return so2().matrix();
- }
-
-
-
-
- SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
- return so2().setComplex(complex);
- }
-
-
-
-
- SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
- SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
- SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
- R.determinant());
- typename SO2Type::ComplexTemporaryType const complex(
- Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1)));
- so2().setComplex(complex);
- }
-
-
- SOPHUS_FUNC
- SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
-
-
- SOPHUS_FUNC
- SO2Type const& so2() const {
- return static_cast<Derived const*>(this)->so2();
- }
-
-
- SOPHUS_FUNC
- TranslationType& translation() {
- return static_cast<Derived*>(this)->translation();
- }
-
-
- SOPHUS_FUNC
- TranslationType const& translation() const {
- return static_cast<Derived const*>(this)->translation();
- }
-
-
- SOPHUS_FUNC
- typename Eigen::internal::traits<Derived>::SO2Type::ComplexT const&
- unit_complex() const {
- return so2().unit_complex();
- }
- };
- template <class Scalar_, int Options>
- class SE2 : public SE2Base<SE2<Scalar_, Options>> {
- public:
- using Base = SE2Base<SE2<Scalar_, Options>>;
- static int constexpr DoF = Base::DoF;
- static int constexpr num_parameters = Base::num_parameters;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
- using SO2Member = SO2<Scalar, Options>;
- using TranslationMember = Vector2<Scalar, Options>;
- using Base::operator=;
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-
- SOPHUS_FUNC SE2();
-
-
- SOPHUS_FUNC SE2(SE2 const& other) = default;
-
-
- template <class OtherDerived>
- SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)
- : so2_(other.so2()), translation_(other.translation()) {
- static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
- "must be same Scalar type");
- }
-
-
- template <class OtherDerived, class D>
- SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2,
- Eigen::MatrixBase<D> const& translation)
- : so2_(so2), translation_(translation) {
- static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
- "must be same Scalar type");
- static_assert(std::is_same<typename D::Scalar, Scalar>::value,
- "must be same Scalar type");
- }
-
-
-
-
-
- SOPHUS_FUNC
- SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,
- Point const& translation)
- : so2_(rotation_matrix), translation_(translation) {}
-
-
- SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)
- : so2_(theta), translation_(translation) {}
-
-
-
- SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)
- : so2_(complex), translation_(translation) {}
-
-
-
-
-
- SOPHUS_FUNC explicit SE2(Transformation const& T)
- : so2_(T.template topLeftCorner<2, 2>().eval()),
- translation_(T.template block<2, 1>(0, 2)) {}
-
-
-
-
-
- SOPHUS_FUNC Scalar* data() {
-
- return so2_.data();
- }
-
-
- SOPHUS_FUNC Scalar const* data() const {
-
- return so2_.data();
- }
-
-
- SOPHUS_FUNC SO2Member& so2() { return so2_; }
-
-
- SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
-
-
- SOPHUS_FUNC TranslationMember& translation() { return translation_; }
-
-
- SOPHUS_FUNC TranslationMember const& translation() const {
- return translation_;
- }
-
-
- SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
- Tangent const& upsilon_theta) {
- using std::abs;
- using std::cos;
- using std::pow;
- using std::sin;
- Sophus::Matrix<Scalar, num_parameters, DoF> J;
- Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();
- Scalar theta = upsilon_theta[2];
- if (abs(theta) < Constants<Scalar>::epsilon()) {
- Scalar const o(0);
- Scalar const i(1);
-
- J << o, o, o, o, o, i, i, o, -Scalar(0.5) * upsilon[1], o, i,
- Scalar(0.5) * upsilon[0];
-
- return J;
- }
- Scalar const c0 = sin(theta);
- Scalar const c1 = cos(theta);
- Scalar const c2 = 1.0 / theta;
- Scalar const c3 = c0 * c2;
- Scalar const c4 = -c1 + Scalar(1);
- Scalar const c5 = c2 * c4;
- Scalar const c6 = c1 * c2;
- Scalar const c7 = pow(theta, -2);
- Scalar const c8 = c0 * c7;
- Scalar const c9 = c4 * c7;
- Scalar const o = Scalar(0);
- J(0, 0) = o;
- J(0, 1) = o;
- J(0, 2) = -c0;
- J(1, 0) = o;
- J(1, 1) = o;
- J(1, 2) = c1;
- J(2, 0) = c3;
- J(2, 1) = -c5;
- J(2, 2) =
- -c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1];
- J(3, 0) = c5;
- J(3, 1) = c3;
- J(3, 2) =
- c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0];
- return J;
- }
-
-
- SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
- Dx_exp_x_at_0() {
- Sophus::Matrix<Scalar, num_parameters, DoF> J;
- Scalar const o(0);
- Scalar const i(1);
-
- J << o, o, o, o, o, i, i, o, o, o, i, o;
-
- return J;
- }
-
-
- SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
- return generator(i);
- }
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
- Scalar theta = a[2];
- SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
- Scalar sin_theta_by_theta;
- Scalar one_minus_cos_theta_by_theta;
- using std::abs;
- if (abs(theta) < Constants<Scalar>::epsilon()) {
- Scalar theta_sq = theta * theta;
- sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;
- one_minus_cos_theta_by_theta =
- Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
- } else {
- sin_theta_by_theta = so2.unit_complex().y() / theta;
- one_minus_cos_theta_by_theta =
- (Scalar(1.) - so2.unit_complex().x()) / theta;
- }
- Vector2<Scalar> trans(
- sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],
- one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);
- return SE2<Scalar>(so2, trans);
- }
-
-
- template <class S = Scalar>
- static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
- fitToSE2(Matrix3<Scalar> const& T) {
- return SE2(SO2<Scalar>::fitToSO2(T.template block<2, 2>(0, 0)),
- T.template block<2, 1>(0, 2));
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation generator(int i) {
- SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");
- Tangent e;
- e.setZero();
- e[i] = Scalar(1);
- return hat(e);
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation hat(Tangent const& a) {
- Transformation Omega;
- Omega.setZero();
- Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);
- Omega.col(2).template head<2>() = a.template head<2>();
- return Omega;
- }
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
- Vector2<Scalar> upsilon1 = a.template head<2>();
- Vector2<Scalar> upsilon2 = b.template head<2>();
- Scalar theta1 = a[2];
- Scalar theta2 = b[2];
- return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
- theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0));
- }
-
-
- static SOPHUS_FUNC SE2 rot(Scalar const& x) {
- return SE2(SO2<Scalar>(x), Sophus::Vector2<Scalar>::Zero());
- }
-
-
-
-
- template <class UniformRandomBitGenerator>
- static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
- std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
- return SE2(SO2<Scalar>::sampleUniform(generator),
- Vector2<Scalar>(uniform(generator), uniform(generator)));
- }
-
-
- template <class T0, class T1>
- static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
- return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));
- }
- static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
- return SE2(SO2<Scalar>(), xy);
- }
-
-
- static SOPHUS_FUNC SE2 transX(Scalar const& x) {
- return SE2::trans(x, Scalar(0));
- }
-
-
- static SOPHUS_FUNC SE2 transY(Scalar const& y) {
- return SE2::trans(Scalar(0), y);
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
- SOPHUS_ENSURE(
- Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),
- "Omega: \n%", Omega);
- Tangent upsilon_omega;
- upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
- upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
- return upsilon_omega;
- }
- protected:
- SO2Member so2_;
- TranslationMember translation_;
- };
- template <class Scalar, int Options>
- SE2<Scalar, Options>::SE2() : translation_(TranslationMember::Zero()) {
- static_assert(std::is_standard_layout<SE2>::value,
- "Assume standard layout for the use of offsetof check below.");
- static_assert(
- offsetof(SE2, so2_) + sizeof(Scalar) * SO2<Scalar>::num_parameters ==
- offsetof(SE2, translation_),
- "This class assumes packed storage and hence will only work "
- "correctly depending on the compiler (options) - in "
- "particular when using [this->data(), this-data() + "
- "num_parameters] to access the raw data in a contiguous fashion.");
- }
- }
- namespace Eigen {
- template <class Scalar_, int Options>
- class Map<Sophus::SE2<Scalar_>, Options>
- : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> {
- public:
- using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>>;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
- using Base::operator=;
- using Base::operator*=;
- using Base::operator*;
- SOPHUS_FUNC
- Map(Scalar* coeffs)
- : so2_(coeffs),
- translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
-
-
- SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
-
-
- SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {
- return so2_;
- }
-
-
- SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
- return translation_;
- }
-
-
- SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
- return translation_;
- }
- protected:
- Map<Sophus::SO2<Scalar>, Options> so2_;
- Map<Sophus::Vector2<Scalar>, Options> translation_;
- };
- template <class Scalar_, int Options>
- class Map<Sophus::SE2<Scalar_> const, Options>
- : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> {
- public:
- using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>>;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
- using Base::operator*=;
- using Base::operator*;
- SOPHUS_FUNC Map(Scalar const* coeffs)
- : so2_(coeffs),
- translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
-
-
- SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {
- return so2_;
- }
-
-
- SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
- const {
- return translation_;
- }
- protected:
- Map<Sophus::SO2<Scalar> const, Options> const so2_;
- Map<Sophus::Vector2<Scalar> const, Options> const translation_;
- };
- }
- #endif
|