26 #include <gtsam/geometry/Unit3.h> 29 #include <gtsam/base/concepts.h> 30 #include <gtsam/config.h> 35 #ifndef ROT3_DEFAULT_COORDINATES_MODE 36 #ifdef GTSAM_USE_QUATERNIONS 38 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP 41 #ifndef GTSAM_ROT3_EXPMAP 43 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY 45 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP 61 #ifdef GTSAM_USE_QUATERNIONS 63 gtsam::Quaternion quaternion_;
84 Rot3(
double R11,
double R12,
double R13,
85 double R21,
double R22,
double R23,
86 double R31,
double R32,
double R33);
95 template <
typename Derived>
96 #ifdef GTSAM_USE_QUATERNIONS 97 explicit Rot3(
const Eigen::MatrixBase<Derived>& R) {
98 quaternion_ = Matrix3(R);
101 explicit Rot3(
const Eigen::MatrixBase<Derived>& R) : rot_(R) {
109 #ifdef GTSAM_USE_QUATERNIONS 110 explicit Rot3(
const Matrix3& R) : quaternion_(R) {}
112 explicit Rot3(
const Matrix3& R) : rot_(R) {}
118 #ifdef GTSAM_USE_QUATERNIONS 128 Rot3(
const Quaternion& q);
129 Rot3(
double w,
double x,
double y,
double z) :
Rot3(Quaternion(w, x, y, z)) {}
137 static Rot3 Random(std::mt19937 & rng);
145 static Rot3 Rx(
double t);
148 static Rot3 Ry(
double t);
151 static Rot3 Rz(
double t);
154 static Rot3 RzRyRx(
double x,
double y,
double z,
162 assert(xyz.size() == 3);
166 out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
169 out = RzRyRx(xyz(0), xyz(1), xyz(2));
180 static Rot3 Roll (
double t) {
return Rx(t); }
200 return RzRyRx(r, p, y, Hr, Hp, Hy);
205 gtsam::Quaternion q(w, x, y, z);
218 #ifdef GTSAM_USE_QUATERNIONS 219 return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
252 return Rodrigues(Vector3(wx, wy, wz));
283 Rot3 normalized()
const;
290 void print(
const std::string& s=
"")
const;
293 bool equals(
const Rot3& p,
double tol = 1e-9)
const;
309 #ifdef GTSAM_USE_QUATERNIONS 310 return Rot3(quaternion_.inverse());
323 return cRb * (*this) * cRb.
inverse();
341 #ifndef GTSAM_USE_QUATERNIONS 346 #ifndef GTSAM_USE_QUATERNIONS 356 return compose(CayleyChart::Retract(omega));
361 return CayleyChart::Local(between(other));
376 #ifdef GTSAM_USE_QUATERNIONS 390 static Matrix3 ExpmapDerivative(
const Vector3& x);
393 static Matrix3 LogmapDerivative(
const Vector3& x);
443 Matrix3 matrix()
const;
448 Matrix3 transpose()
const;
511 std::pair<Unit3, double> axisAngle()
const;
516 gtsam::Quaternion toQuaternion()
const;
523 Rot3 slerp(
double t,
const Rot3& other)
const;
526 GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os,
const Rot3& p);
531 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 533 friend class boost::serialization::access;
534 template <
class ARCHIVE>
535 void serialize(ARCHIVE& ar,
const unsigned int ) {
536 #ifndef GTSAM_USE_QUATERNIONS 538 ar& boost::serialization::make_nvp(
"rot11", M(0, 0));
539 ar& boost::serialization::make_nvp(
"rot12", M(0, 1));
540 ar& boost::serialization::make_nvp(
"rot13", M(0, 2));
541 ar& boost::serialization::make_nvp(
"rot21", M(1, 0));
542 ar& boost::serialization::make_nvp(
"rot22", M(1, 1));
543 ar& boost::serialization::make_nvp(
"rot23", M(1, 2));
544 ar& boost::serialization::make_nvp(
"rot31", M(2, 0));
545 ar& boost::serialization::make_nvp(
"rot32", M(2, 1));
546 ar& boost::serialization::make_nvp(
"rot33", M(2, 2));
548 ar& boost::serialization::make_nvp(
"w", quaternion_.w());
549 ar& boost::serialization::make_nvp(
"x", quaternion_.x());
550 ar& boost::serialization::make_nvp(
"y", quaternion_.y());
551 ar& boost::serialization::make_nvp(
"z", quaternion_.z());
556 #ifdef GTSAM_USE_QUATERNIONS 564 using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
576 GTSAM_EXPORT std::pair<Matrix3, Vector3>
RQ(
static Rot3 RzRyRx(const Vector &xyz, OptionalJacobian< 3, 3 > H={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3.h:160
Lie Group wrapper for Eigen Quaternions.
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
virtual ~Rot3()
Definition: Rot3.h:140
Rot3(const Eigen::MatrixBase< Derived > &R)
Definition: Rot3.h:101
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, mainly for wrapper
Definition: Rot3.h:564
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
Use the Lie group exponential map to retract.
Definition: Rot3.h:340
GTSAM_EXPORT std::pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H={})
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
static Rot3 Identity()
identity rotation for group operation
Definition: Rot3.h:300
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition: Rot3.h:355
Rot3(const SO3 &R)
Definition: Rot3.h:121
3*3 matrix representation of SO(3)
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:342
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Definition: Rot3.h:231
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:204
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition: Rot3.h:360
Definition: Testable.h:112
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
Rot3 conjugate(const Rot3 &cRb) const
Definition: Rot3.h:321
Matrix3 AdjointMap() const
Definition: Rot3.h:396
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
GTSAM_EXPORT void print(const Matrix &A, const std::string &s, std::ostream &stream)
static Rot3 AxisAngle(const Point3 &axis, double angle)
Definition: Rot3.h:215
Definition: chartTesting.h:28
Definition: OptionalJacobian.h:38
Both LieGroupTraits and Testable.
Definition: Lie.h:229
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:271
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:284
Vector3 Point3
Definition: Point3.h:38
static Rot3 Rodrigues(double wx, double wy, double wz)
Definition: Rot3.h:251
static SO ClosestTo(const MatrixNN &M)
Rot3(const Matrix3 &R)
Definition: Rot3.h:112
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: Matrix.h:210
CoordinatesMode
Definition: Rot3.h:339