|
Line
Link Here
|
| 0 |
-- /tmp/tmpLG3iFL-meld/Eigen/src/Geometry/Quaternion.h |
0 |
++ /home/strasdat/src/ScaViSLAM/EXTERNAL/eigen3.1/Eigen/src/Geometry/Quaternion.h |
|
Lines 152-157
Link Here
|
| 152 |
/** \returns an equivalent 3x3 rotation matrix */ |
152 |
/** \returns an equivalent 3x3 rotation matrix */ |
| 153 |
Matrix3 toRotationMatrix() const; |
153 |
Matrix3 toRotationMatrix() const; |
| 154 |
|
154 |
|
|
|
155 |
/** \returns an equivalent 3x3 rotation and scaling matrix */ |
| 156 |
Matrix3 toRotationAndScalingMatrix() const; |
| 157 |
|
| 155 |
/** \returns the quaternion which transform \a a into \a b through a rotation */ |
158 |
/** \returns the quaternion which transform \a a into \a b through a rotation */ |
| 156 |
template<typename Derived1, typename Derived2> |
159 |
template<typename Derived1, typename Derived2> |
| 157 |
Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); |
160 |
Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); |
|
Lines 585-590
Link Here
|
| 585 |
res.coeffRef(2,0) = txz-twy; |
588 |
res.coeffRef(2,0) = txz-twy; |
| 586 |
res.coeffRef(2,1) = tyz+twx; |
589 |
res.coeffRef(2,1) = tyz+twx; |
| 587 |
res.coeffRef(2,2) = Scalar(1)-(txx+tyy); |
590 |
res.coeffRef(2,2) = Scalar(1)-(txx+tyy); |
|
|
591 |
|
| 592 |
return res; |
| 593 |
} |
| 594 |
|
| 595 |
/** Convert the quaternion to a 3x3 rotation and scaling matrix. |
| 596 |
* Thus, the result is s*R, with s being a positive scalar and |
| 597 |
* R an orthogonal matrix with det(R)=1. |
| 598 |
*/ |
| 599 |
template<class Derived> |
| 600 |
inline typename QuaternionBase<Derived>::Matrix3 |
| 601 |
QuaternionBase<Derived>::toRotationAndScalingMatrix(void) const |
| 602 |
{ |
| 603 |
// NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) |
| 604 |
// if not inlined then the cost of the return by value is huge ~ +35%, |
| 605 |
// however, not inlining this function is an order of magnitude slower, so |
| 606 |
// it has to be inlined, and so the return by value is not an issue |
| 607 |
Matrix3 res; |
| 608 |
|
| 609 |
const Scalar nrm = norm(); |
| 610 |
if (nrm == 0) |
| 611 |
{ |
| 612 |
res.setZero(); |
| 613 |
return res; |
| 614 |
} |
| 615 |
const Scalar two_by_norm = 2./nrm; |
| 616 |
const Scalar tx = two_by_norm*this->x(); |
| 617 |
const Scalar ty = two_by_norm*this->y(); |
| 618 |
const Scalar tz = two_by_norm*this->z(); |
| 619 |
const Scalar twx = tx*this->w(); |
| 620 |
const Scalar twy = ty*this->w(); |
| 621 |
const Scalar twz = tz*this->w(); |
| 622 |
const Scalar txx = tx*this->x(); |
| 623 |
const Scalar txy = ty*this->x(); |
| 624 |
const Scalar txz = tz*this->x(); |
| 625 |
const Scalar tyy = ty*this->y(); |
| 626 |
const Scalar tyz = tz*this->y(); |
| 627 |
const Scalar tzz = tz*this->z(); |
| 628 |
|
| 629 |
res.coeffRef(0,0) = nrm-(tyy+tzz); |
| 630 |
res.coeffRef(0,1) = txy-twz; |
| 631 |
res.coeffRef(0,2) = txz+twy; |
| 632 |
res.coeffRef(1,0) = txy+twz; |
| 633 |
res.coeffRef(1,1) = nrm-(txx+tzz); |
| 634 |
res.coeffRef(1,2) = tyz-twx; |
| 635 |
res.coeffRef(2,0) = txz-twy; |
| 636 |
res.coeffRef(2,1) = tyz+twx; |
| 637 |
res.coeffRef(2,2) = nrm-(txx+tyy); |
| 588 |
|
638 |
|
| 589 |
return res; |
639 |
return res; |
| 590 |
} |
640 |
} |