1 #ifndef SOPHUS_RXSO3_HPP 2 #define SOPHUS_RXSO3_HPP 7 template <
class Scalar_,
int Options = 0>
16 template <
class Scalar_,
int Options>
17 struct traits<
Sophus::RxSO3<Scalar_, Options>> {
22 template <
class Scalar_,
int Options>
23 struct traits<Map<
Sophus::RxSO3<Scalar_>, Options>>
24 : traits<Sophus::RxSO3<Scalar_, Options>> {
29 template <
class Scalar_,
int Options>
30 struct traits<Map<
Sophus::RxSO3<Scalar_> const, Options>>
31 : traits<Sophus::RxSO3<Scalar_, Options> const> {
63 template <
class Derived>
66 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
68 typename Eigen::internal::traits<Derived>::QuaternionType;
72 static int constexpr DoF = 4;
74 static int constexpr num_parameters = 4;
76 static int constexpr N = 3;
93 res.template topLeftCorner<3, 3>() = rotationMatrix();
99 template <
class NewScalarType>
117 return quaternion().coeffs().data();
148 Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x();
149 Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y();
150 Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z();
151 Scalar const w_sq = quaternion().w() * quaternion().w();
152 Scalar const two_vx =
Scalar(2) * quaternion().vec().x();
153 Scalar const two_vy =
Scalar(2) * quaternion().vec().y();
154 Scalar const two_vz =
Scalar(2) * quaternion().vec().z();
155 Scalar const two_vx_vy = two_vx * quaternion().vec().y();
156 Scalar const two_vx_vz = two_vx * quaternion().vec().z();
157 Scalar const two_vx_w = two_vx * quaternion().w();
158 Scalar const two_vy_vz = two_vy * quaternion().vec().z();
159 Scalar const two_vy_w = two_vy * quaternion().w();
160 Scalar const two_vz_w = two_vz * quaternion().w();
162 sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq;
163 sR(1, 0) = two_vx_vy + two_vz_w;
164 sR(2, 0) = two_vx_vz - two_vy_w;
166 sR(0, 1) = two_vx_vy - two_vz_w;
167 sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq;
168 sR(2, 1) = two_vx_w + two_vy_vz;
170 sR(0, 2) = two_vx_vz + two_vy_w;
171 sR(1, 2) = -two_vx_w + two_vy_vz;
172 sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq;
178 template <
class OtherDerived>
206 Scalar scale = quaternion().squaredNorm();
207 Point two_vec_cross_p = quaternion().vec().cross(p);
208 two_vec_cross_p += two_vec_cross_p;
209 return scale * p + (quaternion().w() * two_vec_cross_p +
210 quaternion().vec().cross(two_vec_cross_p));
222 Scalar scale = this->scale();
226 quaternion_nonconst().normalize();
238 "Scale factor must be greater-equal epsilon.");
239 static_cast<Derived*
>(
this)->quaternion_nonconst() = quat;
245 return static_cast<Derived const*
>(
this)->quaternion();
252 norm_quad.normalize();
253 return norm_quad.toRotationMatrix();
265 Scalar saved_scale = scale();
266 quaternion_nonconst() = R;
267 quaternion_nonconst().coeffs() *= std::sqrt(saved_scale);
278 quaternion_nonconst().normalize();
279 quaternion_nonconst().coeffs() *= sqrt(scale);
291 (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2));
294 "Scale factor must be greater-equal epsilon.");
295 Scalar scale = sqrt(squared_scale);
296 quaternion_nonconst() = sR / scale;
297 quaternion_nonconst().coeffs() *= sqrt(scale);
304 Scalar saved_scale = scale();
306 quaternion_nonconst().coeffs() *= sqrt(saved_scale);
325 res.template topLeftCorner<3, 3>() =
342 return expAndTheta(a, &theta);
357 Scalar sqrt_scale = sqrt(exp(sigma));
358 Eigen::Quaternion<Scalar> quat =
360 quat.coeffs() *= sqrt_scale;
387 SOPHUS_ENSURE(i >= 0 && i <= 3,
"i should be in range [0,3].");
432 res.template head<3>() = omega1.cross(omega2);
449 return logAndTheta(other, &theta);
460 omega_sigma[3] = log(scale);
461 omega_sigma.template head<3>() =
481 return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0));
488 return static_cast<Derived*
>(
this)->quaternion_nonconst();
493 template <
class Scalar_,
int Options>
508 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
517 template <
class OtherDerived>
519 : quaternion_(other.quaternion()) {}
527 this->setScaledRotationMatrix(sR);
538 "Scale factor must be greater-equal epsilon.");
539 quaternion_.coeffs() *= std::sqrt(scale);
547 : quaternion_(so3.unit_quaternion()) {
549 "Scale factor must be greater-equal epsilon.");
550 quaternion_.coeffs() *= std::sqrt(scale);
559 : quaternion_(quat) {
561 "must be same Scalar type.");
563 "Scale factor must be greater-equal epsilon.");
584 template <
class Scalar_,
int Options>
585 class Map<
Sophus::RxSO3<Scalar_>, Options>
599 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
600 using Base::operator*=;
601 using Base::operator*;
608 Map<Eigen::Quaternion<Scalar>, Options>
const&
quaternion()
const {
624 template <
class Scalar_,
int Options>
625 class Map<
Sophus::RxSO3<Scalar_> const, Options>
636 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
637 using Base::operator*=;
638 using Base::operator*;
646 Map<Eigen::Quaternion<Scalar>
const, Options>
const&
quaternion()
const {
655 #endif // SOPHUS_RXSO3_HPP Map< Eigen::Quaternion< Scalar >, Options > quaternion_
SOPHUS_FUNC RxSO3< Scalar > inverse() const
#define SOPHUS_ENSURE(expr, description,...)
static SOPHUS_FUNC RxSO3< Scalar > exp(Tangent const &a)
static SOPHUS_FUNC Transformation hat(Tangent const &a)
SOPHUS_FUNC RxSO3< Scalar > operator*(RxSO3< Scalar > const &other) const
SOPHUS_FUNC Transformation rotationMatrix() const
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const &sR)
SOPHUS_FUNC Point operator*(Point const &p) const
SOPHUS_FUNC Map(Scalar const *coeffs)
typename Base::Point Point
SOPHUS_FUNC void setSO3(SO3< Scalar > const &so3)
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
typename Eigen::internal::traits< RxSO3< Scalar, Options > >::QuaternionType QuaternionType
static SOPHUS_FUNC Tangent log(RxSO3< Scalar > const &other)
SOPHUS_FUNC Scalar const * data() const
SOPHUS_FUNC QuaternionMember & quaternion_nonconst()
T value(details::expression_node< T > *n)
SOPHUS_FUNC void setScale(Scalar const &scale)
SOPHUS_FUNC Transformation matrix() const
Matrix< Scalar, DoF, DoF > Adjoint
Vector< Scalar, DoF > Tangent
typename Base::Adjoint Adjoint
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
SOPHUS_FUNC Scalar scale() const
SOPHUS_FUNC RxSO3(Scalar const &scale, Transformation const &R)
typename Base::Transformation Transformation
SOPHUS_FUNC SO3< Scalar > so3() const
typename Base::Tangent Tangent
typename Base::Adjoint Adjoint
QuaternionMember quaternion_
typename Base::Transformation Transformation
SOPHUS_FUNC RxSO3Base< Derived > & operator*=(RxSO3< Scalar > const &other)
SOPHUS_FUNC Adjoint Adj() const
static SOPHUS_FUNC Tangent logAndTheta(RxSO3< Scalar > const &other, Scalar *theta)
Map< Eigen::Quaternion< Scalar > const, Options > QuaternionType
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
Eigen::Matrix< Scalar, M, N > Matrix
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar > const, Options > const & quaternion() const
static SOPHUS_FUNC Transformation generator(int i)
static SOPHUS_FUNC Scalar epsilon()
Map< Eigen::Quaternion< Scalar > const, Options > const quaternion_
SOPHUS_FUNC RxSO3(RxSO3Base< OtherDerived > const &other)
typename Base::Point Point
typename Base::Point Point
Vector< Scalar, 3, Options > Vector3
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
SOPHUS_FUNC Tangent log() const
typename Base::Tangent Tangent
SOPHUS_FUNC QuaternionMember const & quaternion() const
typename Base::Tangent Tangent
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
SOPHUS_FUNC void setRotationMatrix(Transformation const &R)
SOPHUS_FUNC RxSO3(Transformation const &sR)
Eigen::Matrix< Scalar, M, 1, Options > Vector
SOPHUS_FUNC RxSO3(Scalar const &scale, SO3< Scalar > const &so3)
typename Base::Adjoint Adjoint
typename Base::Transformation Transformation
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC RxSO3()
SOPHUS_FUNC RxSO3< NewScalarType > cast() const
SOPHUS_FUNC QuaternionType const & quaternion() const
SOPHUS_FUNC QuaternionType & quaternion_nonconst()
Eigen::Quaternion< Scalar, Options > QuaternionType
SOPHUS_FUNC Scalar * data()
Eigen::Quaternion< Scalar, Options > QuaternionMember
typename Eigen::internal::traits< RxSO3< Scalar, Options > >::Scalar Scalar
SOPHUS_FUNC RxSO3Base< Derived > & operator=(RxSO3Base< OtherDerived > const &other)
SOPHUS_FUNC RxSO3(Eigen::QuaternionBase< D > const &quat)
Vector< Scalar, 4 > Vector4
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > & quaternion_nonconst()
Matrix< Scalar, N, N > Transformation
static SOPHUS_FUNC RxSO3< Scalar > expAndTheta(Tangent const &a, Scalar *theta)
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > const & quaternion() const