--- /tmp/tmpLG3iFL-meld/Eigen/src/Geometry/Quaternion.h +++ /home/strasdat/src/ScaViSLAM/EXTERNAL/eigen3.1/Eigen/src/Geometry/Quaternion.h @@ -152,6 +152,9 @@ /** \returns an equivalent 3x3 rotation matrix */ Matrix3 toRotationMatrix() const; + /** \returns an equivalent 3x3 rotation and scaling matrix */ + Matrix3 toRotationAndScalingMatrix() const; + /** \returns the quaternion which transform \a a into \a b through a rotation */ template Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); @@ -585,6 +588,53 @@ res.coeffRef(2,0) = txz-twy; res.coeffRef(2,1) = tyz+twx; res.coeffRef(2,2) = Scalar(1)-(txx+tyy); + + return res; +} + +/** Convert the quaternion to a 3x3 rotation and scaling matrix. + * Thus, the result is s*R, with s being a positive scalar and + * R an orthogonal matrix with det(R)=1. + */ +template +inline typename QuaternionBase::Matrix3 +QuaternionBase::toRotationAndScalingMatrix(void) const +{ + // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) + // if not inlined then the cost of the return by value is huge ~ +35%, + // however, not inlining this function is an order of magnitude slower, so + // it has to be inlined, and so the return by value is not an issue + Matrix3 res; + + const Scalar nrm = norm(); + if (nrm == 0) + { + res.setZero(); + return res; + } + const Scalar two_by_norm = 2./nrm; + const Scalar tx = two_by_norm*this->x(); + const Scalar ty = two_by_norm*this->y(); + const Scalar tz = two_by_norm*this->z(); + const Scalar twx = tx*this->w(); + const Scalar twy = ty*this->w(); + const Scalar twz = tz*this->w(); + const Scalar txx = tx*this->x(); + const Scalar txy = ty*this->x(); + const Scalar txz = tz*this->x(); + const Scalar tyy = ty*this->y(); + const Scalar tyz = tz*this->y(); + const Scalar tzz = tz*this->z(); + + res.coeffRef(0,0) = nrm-(tyy+tzz); + res.coeffRef(0,1) = txy-twz; + res.coeffRef(0,2) = txz+twy; + res.coeffRef(1,0) = txy+twz; + res.coeffRef(1,1) = nrm-(txx+tzz); + res.coeffRef(1,2) = tyz-twx; + res.coeffRef(2,0) = txz-twy; + res.coeffRef(2,1) = tyz+twx; + res.coeffRef(2,2) = nrm-(txx+tyy); return res; }