123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740 |
- #ifndef SOPHUS_SIM3_HPP
- #define SOPHUS_SIM3_HPP
- #include "rxso3.hpp"
- #include "sim_details.hpp"
- namespace Sophus {
- template <class Scalar_, int Options = 0>
- class Sim3;
- using Sim3d = Sim3<double>;
- using Sim3f = Sim3<float>;
- }
- namespace Eigen {
- namespace internal {
- template <class Scalar_, int Options>
- struct traits<Sophus::Sim3<Scalar_, Options>> {
- using Scalar = Scalar_;
- using TranslationType = Sophus::Vector3<Scalar, Options>;
- using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
- };
- template <class Scalar_, int Options>
- struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
- : traits<Sophus::Sim3<Scalar_, Options>> {
- using Scalar = Scalar_;
- using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
- using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
- };
- template <class Scalar_, int Options>
- struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
- : traits<Sophus::Sim3<Scalar_, Options> const> {
- using Scalar = Scalar_;
- using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
- using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
- };
- }
- }
- namespace Sophus {
- template <class Derived>
- class Sim3Base {
- public:
- using Scalar = typename ;
- using TranslationType =
- typename Eigen::internal::traits<Derived>::TranslationType;
- using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
- using QuaternionType = typename RxSO3Type::QuaternionType;
-
-
- static int constexpr DoF = 7;
-
-
- static int constexpr num_parameters = 7;
-
- static int constexpr N = 4;
- using Transformation = Matrix<Scalar, N, N>;
- using Point = Vector3<Scalar>;
- using HomogeneousPoint = Vector4<Scalar>;
- using Line = ParametrizedLine3<Scalar>;
- using Tangent = Vector<Scalar, DoF>;
- using Adjoint = Matrix<Scalar, DoF, DoF>;
-
-
-
-
- template <typename OtherDerived>
- using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
- Scalar, typename ;
- template <typename OtherDerived>
- using Sim3Product = Sim3<ReturnScalar<OtherDerived>>;
- template <typename PointDerived>
- using PointProduct = Vector3<ReturnScalar<PointDerived>>;
- template <typename HPointDerived>
- using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
-
-
-
-
-
-
- SOPHUS_FUNC Adjoint Adj() const {
- Matrix3<Scalar> const ;
- Adjoint res;
- res.setZero();
- res.template block<3, 3>(0, 0) = rxso3().matrix();
- res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R;
- res.template block<3, 1>(0, 6) = -translation();
- res.template block<3, 3>(3, 3) = R;
- res(6, 6) = Scalar(1);
- return res;
- }
-
-
- template <class NewScalarType>
- SOPHUS_FUNC Sim3<NewScalarType> cast() const {
- return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
- translation().template cast<NewScalarType>());
- }
-
-
- SOPHUS_FUNC Sim3<Scalar> inverse()
- RxSO3<Scalar> invR ;
- return Sim3<Scalar>(invR, invR ;
- }
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Tangent log() const {
-
-
-
-
-
-
- Tangent res;
- auto omega_sigma_and_theta = rxso3().logAndTheta();
- Vector3<Scalar> const
- omega_sigma_and_theta.tangent.template ;
- Scalar sigma ;
- Matrix3<Scalar> const ;
- Matrix3<Scalar> const
- Omega, omega_sigma_and_theta.theta, sigma, scale());
- res.segment(0, 3) = W_inv * translation();
- res.segment(3, 3) = omega;
- res[6] = sigma;
- return res;
- }
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Transformation matrix() const {
- Transformation homogenious_matrix;
- homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
- homogenious_matrix.row(3) =
- Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
- return homogenious_matrix;
- }
-
-
- SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4()
- Matrix<Scalar, 3, 4> matrix;
- matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
- matrix.col(3) = translation();
- return matrix;
- }
-
-
- template <class OtherDerived>
- SOPHUS_FUNC Sim3Base<Derived>& operator=(
- Sim3Base<OtherDerived> const& other) {
- rxso3() = other.rxso3();
- translation() = other.translation();
- return *this;
- }
-
-
-
-
-
- template <typename OtherDerived>
- SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
- Sim3Base<OtherDerived> const& other) const {
- return Sim3Product<OtherDerived>(
- rxso3() * other.rxso3(), translation() + rxso3() * other.translation());
- }
-
-
-
-
-
-
-
-
- template <typename PointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<PointDerived, 3>::value>::type>
- SOPHUS_FUNC PointProduct<PointDerived> operator*(
- Eigen::MatrixBase<PointDerived> const& p) const {
- return rxso3() * p + translation();
- }
-
-
- template <typename HPointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<HPointDerived, 4>::value>::type>
- SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
- Eigen::MatrixBase<HPointDerived> const& p) const {
- const PointProduct<HPointDerived> tp =
- rxso3() * p.template head<3>() + p(3) * translation();
- return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
- }
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Line operator*(Line const& l) const {
- Line rotatedLine = rxso3() * l;
- return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
- }
-
-
-
- template <typename OtherDerived,
- typename = typename std::enable_if<
- std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
- SOPHUS_FUNC
- Sim3Base<OtherDerived> const& other) {
- *static_cast<Derived*>(this) = *this ;
- return *this;
- }
-
-
-
-
-
- SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params()
- Sophus::Vector<Scalar, num_parameters> p;
- p << rxso3().params(), translation();
- return p;
- }
-
-
-
-
- SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
- rxso3().setQuaternion(quat);
- }
-
-
- SOPHUS_FUNC QuaternionType const& quaternion() const {
- return rxso3().quaternion();
- }
-
-
- SOPHUS_FUNC Matrix3<Scalar> rotationMatrix()
- return ;
- }
-
-
- SOPHUS_FUNC RxSO3Type& rxso3() {
- return static_cast<Derived*>(this)->rxso3();
- }
-
-
- SOPHUS_FUNC RxSO3Type const& rxso3() const {
- return static_cast<Derived const*>(this)->rxso3();
- }
-
-
- SOPHUS_FUNC Scalar scale() ; }
-
-
- SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
- rxso3().setRotationMatrix(R);
- }
-
-
-
-
-
- SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }
-
-
-
-
-
- SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
- rxso3().setScaledRotationMatrix(sR);
- }
-
-
- SOPHUS_FUNC TranslationType& translation() {
- return static_cast<Derived*>(this)->translation();
- }
-
-
- SOPHUS_FUNC TranslationType const& translation() const {
- return static_cast<Derived const*>(this)->translation();
- }
- };
- template <class Scalar_, int Options>
- class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
- public:
- using Base = Sim3Base<Sim3<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 RxSo3Member = RxSO3<Scalar, Options>;
- using TranslationMember = Vector3<Scalar, Options>;
- using Base::operator=;
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-
- SOPHUS_FUNC Sim3();
-
-
- SOPHUS_FUNC Sim3(Sim3 const& other) = default;
-
-
- template <class OtherDerived>
- SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
- : rxso3_(other.rxso3()), 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 Sim3(RxSO3Base<OtherDerived> const& rxso3,
- Eigen::MatrixBase<D> const& translation)
- : rxso3_(rxso3), 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");
- }
-
-
-
-
- template <class D1, class D2>
- SOPHUS_FUNC Sim3(Eigen::QuaternionBase<D1> const& quaternion,
- Eigen::MatrixBase<D2> const& translation)
- : rxso3_(quaternion), translation_(translation) {
- static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
- "must be same Scalar type");
- static_assert(std::is_same<typename D2::Scalar, Scalar>::value,
- "must be same Scalar type");
- }
-
-
-
-
-
- SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T)
- : rxso3_(T.template
- translation_(T.template
-
-
-
-
-
- SOPHUS_FUNC
-
- return ;
- }
-
-
- SOPHUS_FUNC Scalar const* data()
-
- return ;
- }
-
-
- SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
-
-
- SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
-
-
- SOPHUS_FUNC TranslationMember& translation() { return translation_; }
-
-
- SOPHUS_FUNC TranslationMember const& translation() const {
- return translation_;
- }
-
-
- SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
- return generator(i);
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Sim3<Scalar> exp(Tangent
-
-
-
-
- Vector3<Scalar> const ;
- Vector3<Scalar> const ;
- Scalar const ;
- Scalar theta;
- RxSO3<Scalar> rxso3
- RxSO3<Scalar>::expAndTheta(a.template ;
- Matrix3<Scalar> const ;
- Matrix3<Scalar> const ;
- return Sim3<Scalar>(rxso3, W ;
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation generator(int i) {
- SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
- Tangent e;
- e.setZero();
- e[i] = Scalar(1);
- return hat(e);
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation hat(Tangent const& a) {
- Transformation Omega;
- Omega.template topLeftCorner<3, 3>() =
- RxSO3<Scalar>::hat(a.template ;
- Omega.col(3).template head<3>() = a.template head<3>();
- Omega.row(3).setZero();
- return Omega;
- }
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
- Vector3<Scalar> const ;
- Vector3<Scalar> const ;
- Vector3<Scalar> const ;
- Vector3<Scalar> const ;
- Scalar sigma1 ;
- Scalar sigma2 ;
- Tangent res;
- res.template head<3>() = SO3<Scalar>::hat(omega1)
- SO3<Scalar>::hat(upsilon1)
- sigma1 ;
- res.template segment<3>(3) = omega1.cross(omega2);
- res[6] = Scalar(0);
- return res;
- }
-
-
-
-
-
-
- template <class UniformRandomBitGenerator>
- static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
- std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
- return Sim3(RxSO3<Scalar>::sampleUniform(generator),
- Vector3<Scalar>(uniform(generator), uniform(generator),
- uniform(generator)));
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
- Tangent upsilon_omega_sigma;
- upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
- upsilon_omega_sigma.template tail<4>() =
- RxSO3<Scalar>::vee(Omega.template ;
- return upsilon_omega_sigma;
- }
- protected:
- RxSo3Member rxso3_;
- TranslationMember translation_;
- };
- template <class Scalar, int
- Sim3<Scalar, Options>::Sim3()
- static_assert(std::is_standard_layout<Sim3>::value,
- "Assume standard layout for the use of offsetof check below.");
- static_assert(
- offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters
- offsetof(Sim3, 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::Sim3<Scalar_>, Options>
- : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
- public:
- using Base = Sophus::Sim3Base<Map<Sophus::Sim3<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)
- : rxso3_(coeffs),
- translation_(coeffs
-
-
- SOPHUS_FUNC ; }
-
-
- SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3()
- return ;
- }
-
-
- SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation()
- return ;
- }
-
- SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation()
- return ;
- }
- protected:
- Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
- Map<Sophus::Vector3<Scalar>, Options> translation_;
- };
- template <class Scalar_, int Options>
- class Map<Sophus::Sim3<Scalar_> const, Options>
- : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
- using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;
- public:
- 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)
- : rxso3_(coeffs),
- translation_(coeffs
-
-
- SOPHUS_FUNC
- return ;
- }
-
-
- SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
- const
- return ;
- }
- protected:
- Map<Sophus::RxSO3<Scalar> const, Options> const ;
- Map<Sophus::Vector3<Scalar> const, Options> const ;
- };
- }
- #endif
|