14 #ifndef ECL_GEOMETRY_LEGACY_POSE3D_HPP_ 15 #define ECL_GEOMETRY_LEGACY_POSE3D_HPP_ 42 template <
class Float,
typename Enable =
void>
59 template<
typename Float>
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
80 LegacyPose3D() : rot(RotationMatrix::Identity()), trans(Translation::Zero()) {}
93 template<
typename Rot,
typename Trans>
95 LegacyPose3D(
const ecl::linear_algebra::EigenBase<Rot>& rotation,
const ecl::linear_algebra::EigenBase<Trans>& translation) :
106 template <enum Pose2DStorageType Storage_>
108 rot(RotationMatrix::Identity()),
109 trans(Translation::Zero())
111 rot.template block<2,2>(0,0) = pose.rotationMatrix();
112 trans.template segment<2>(0) = pose.translation();
122 template<
typename Trans>
123 LegacyPose3D(
const ecl::linear_algebra::AngleAxis<Float>& angle_axis,
const ecl::linear_algebra::MatrixBase<Trans>& translation) :
124 rot(RotationMatrix::Identity()),
125 trans(Translation::Zero())
136 template<
typename Trans>
137 LegacyPose3D(
const ecl::linear_algebra::Quaternion<Float>& quaternion,
const ecl::linear_algebra::MatrixBase<Trans>& translation) :
138 rot(quaternion.toRotationMatrix()),
147 rot(pose.rotation()),
148 trans(pose.translation())
161 template <enum Pose2DStorageType Storage_>
163 rot.template block<2,2>(0,0) = pose.rotationMatrix();
164 (rot.template block<2,1>(0,2)) << 0.0, 0.0;
165 (rot.template block<1,3>(2,0)) << 0.0, 0.0, 1.0;
166 trans.template segment<2>(0) = pose.translation();
177 rot = pose.rotation();
178 trans = pose.translation();
203 template <
typename Trans>
204 void translation(
const ecl::linear_algebra::MatrixBase<Trans>& translation) {
205 this->trans = translation;
219 const RotationMatrix&
rotation()
const {
return rot; }
242 inverse.rotation(rot.transpose());
243 inverse.translation(-1*(inverse.rot*trans));
251 template <
typename Float_>
253 return LegacyPose3D<Float>(rotation()*pose.rotation(),translation() + rotation()*pose.translation());
264 template <
typename Float_>
267 *
this = (*this)*pose;
278 template <
typename Float_>
280 return pose.inverse()*(*this);
288 template <
typename OutputStream,
typename Float_>
289 friend OutputStream& operator<<(OutputStream &ostream , const LegacyPose3D<Float_> &pose);
308 template <
typename OutputStream,
typename Float_>
309 OutputStream& operator<<(OutputStream &ostream , const LegacyPose3D<Float_> &pose) {
313 for (
unsigned int i = 0; i < 3; ++i ) {
315 for (
unsigned int j = 0; j < 3; ++j ) {
316 ostream << format(pose.rot(i,j)) <<
" ";
319 ostream << format(pose.trans(i)) <<
" ]\n";
LegacyPose3D(const LegacyPose2D< Float, Storage_ > &pose)
Initialise from a 2D pose.
Embedded control libraries.
void translation(const ecl::linear_algebra::MatrixBase< Trans > &translation)
Set the translation vector.
LegacyPose3D(const ecl::linear_algebra::Quaternion< Float > &quaternion, const ecl::linear_algebra::MatrixBase< Trans > &translation)
Initialise from an eigen Quaternion and a translation.
const Translation & translation() const
Return a const handle to the rotational storage component.
RotationMatrix & rotation()
ecl::linear_algebra::Matrix< Float, 3, 3 > RotationMatrix
The type used to represent rotation matrices.
LegacyPose3D()
Initialise with zero rotation and zero translation.
Translation & translation()
Return a mutable handle to the rotational storage component.
Parent template definition for Pose2D.
LegacyPose3D(const ecl::linear_algebra::AngleAxis< Float > &angle_axis, const ecl::linear_algebra::MatrixBase< Trans > &translation)
Initialise from an eigen AngleAxis and a translation.
LegacyPose3D< Float > & operator*=(const LegacyPose3D< Float_ > &pose)
Transform this pose by the specified input pose.
LegacyPose3D< Float > operator*(const LegacyPose3D< Float_ > &pose) const
Combine this pose with the incoming pose.
LegacyPose3D< Float > relative(const LegacyPose3D< Float_ > &pose) const
Relative pose between this pose and another.
const RotationMatrix & rotation() const
Return a mutable handle to the translation vector.
LegacyPose3D< Float > & operator=(const LegacyPose3D< Float > &pose)
Assign from another Pose2D instance.
Parent template definition for Pose3D.
LegacyPose3D< Float > inverse() const
Calculate the inverse pose.
void rotation(const RotationMatrix &rotation)
Set the rotational component.
ecl::linear_algebra::Matrix< Float, 3, 1 > Translation
The type used to represent translations.
LegacyPose3D(const LegacyPose3D< Float > &pose)
Copy constructor for 3d poses.
LegacyPose3D< Float > & operator=(const LegacyPose2D< Float, Storage_ > &pose)
Assign from another Pose2D instance.
RotationMatrix rotationMatrix() const
Return a const handle to the translation vector.
LegacyPose3D(const ecl::linear_algebra::EigenBase< Rot > &rotation, const ecl::linear_algebra::EigenBase< Trans > &translation)
Initialise with rotation matrix and translation.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Float Scalar
Eigen style declaration of the element type.