TooN: so3.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

so3.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
    00004 //
    00005 // This file is part of the TooN Library.  This library is free
    00006 // software; you can redistribute it and/or modify it under the
    00007 // terms of the GNU General Public License as published by the
    00008 // Free Software Foundation; either version 2, or (at your option)
    00009 // any later version.
    00010
    00011 // This library is distributed in the hope that it will be useful,
    00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
    00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    00014 // GNU General Public License for more details.
    00015
    00016 // You should have received a copy of the GNU General Public License along
    00017 // with this library; see the file COPYING.  If not, write to the Free
    00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
    00019 // USA.
    00020
    00021 // As a special exception, you may use this file as part of a free software
    00022 // library without restriction.  Specifically, if other files instantiate
    00023 // templates or use macros or inline functions from this file, or you compile
    00024 // this file and link it with other files to produce an executable, this
    00025 // file does not by itself cause the resulting executable to be covered by
    00026 // the GNU General Public License.  This exception does not however
    00027 // invalidate any other reasons why the executable file might be covered by
    00028 // the GNU General Public License.
    00029
    00030 #ifndef TOON_INCLUDE_SO3_H
    00031 #define TOON_INCLUDE_SO3_H
    00032
    00033 #include <TooN/TooN.h>
    00034 #include <TooN/helpers.h>
    00035 #include <cassert>
    00036
    00037 namespace TooN {
    00038
    00039 template <typename Precision> class SO3;
    00040 template <typename Precision> class SE3;
    00041 template <typename Precision> class SIM3;
    00042
    00043 template<class Precision> inline std::istream & operator>>(std::istream &, SO3<Precision> & );
    00044 template<class Precision> inline std::istream & operator>>(std::istream &, SE3<Precision> & );
    00045 template<class Precision> inline std::istream & operator>>(std::istream &, SIM3<Precision> & );
    00046
    00047 /// Class to represent a three-dimensional rotation matrix. Three-dimensional rotation
    00048 /// matrices are members of the Special Orthogonal Lie group SO3. This group can be parameterised
    00049 /// three numbers (a vector in the space of the Lie Algebra). In this class, the three parameters are the
    00050 /// finite rotation vector, i.e. a three-dimensional vector whose direction is the axis of rotation
    00051 /// and whose length is the angle of rotation in radians. Exponentiating this vector gives the matrix,
    00052 /// and the logarithm of the matrix gives this vector.
    00053 /// @ingroup gTransforms
    00054 template <typename Precision = DefaultPrecision>
    00055 class SO3 {
    00056 public:
    00057     friend std::istream& operator>> <Precision> (std::istream& is, SO3<Precision> & rhs);
    00058     friend std::istream& operator>> <Precision> (std::istream& is, SE3<Precision> & rhs);
    00059     friend std::istream& operator>> <Precision> (std::istream& is, SIM3<Precision> & rhs);
    00060     friend class SE3<Precision>;
    00061     friend class SIM3<Precision>;
    00062
    00063     /// Default constructor. Initialises the matrix to the identity (no rotation)
    00064     SO3() : my_matrix(Identity) {}
    00065
    00066     /// Construct from the axis of rotation (and angle given by the magnitude).
    00067     template <int S, typename P, typename A>
    00068     SO3(const Vector<S, P, A> & v) { *this = exp(v); }
    00069
    00070     /// Construct from a rotation matrix.
    00071     template <int R, int C, typename P, typename A>
    00072     SO3(const Matrix<R,C,P,A>& rhs) { *this = rhs; }
    00073
    00074     /// creates an SO3 as a rotation that takes Vector a into the direction of Vector b
    00075     /// with the rotation axis along a ^ b. If |a ^ b| == 0, it creates the identity rotation.
    00076     /// An assertion will fail if Vector a and Vector b are in exactly opposite directions.
    00077     /// @param a source Vector
    00078     /// @param b target Vector
    00079     template <int S1, int S2, typename P1, typename P2, typename A1, typename A2>
    00080     SO3(const Vector<S1, P1, A1> & a, const Vector<S2, P2, A2> & b ){
    00081         SizeMismatch<3,S1>::test(3, a.size());
    00082         SizeMismatch<3,S2>::test(3, b.size());
    00083         Vector<3, Precision> n = a ^ b;
    00084         if(norm_sq(n) == 0) {
    00085             //check that the vectors are in the same direction if cross product is 0. If not,
    00086             //this means that the rotation is 180 degrees, which leads to an ambiguity in the rotation axis.
    00087             assert(a*b>=0);
    00088             my_matrix = Identity;
    00089             return;
    00090         }
    00091         n = unit(n);
    00092         Matrix<3> R1;
    00093         R1.T()[0] = unit(a);
    00094         R1.T()[1] = n;
    00095         R1.T()[2] = n ^ R1.T()[0];
    00096         my_matrix.T()[0] = unit(b);
    00097         my_matrix.T()[1] = n;
    00098         my_matrix.T()[2] = n ^ my_matrix.T()[0];
    00099         my_matrix = my_matrix * R1.T();
    00100     }
    00101
    00102     /// Assignment operator from a general matrix. This also calls coerce()
    00103     /// to make sure that the matrix is a valid rotation matrix.
    00104     template <int R, int C, typename P, typename A>
    00105     SO3& operator=(const Matrix<R,C,P,A> & rhs) {
    00106         my_matrix = rhs;
    00107         coerce();
    00108         return *this;
    00109     }
    00110
    00111     /// Modifies the matrix to make sure it is a valid rotation matrix.
    00112     void coerce() {
    00113         my_matrix[0] = unit(my_matrix[0]);
    00114         my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]);
    00115         my_matrix[1] = unit(my_matrix[1]);
    00116         my_matrix[2] -= my_matrix[0] * (my_matrix[0]*my_matrix[2]);
    00117         my_matrix[2] -= my_matrix[1] * (my_matrix[1]*my_matrix[2]);
    00118         my_matrix[2] = unit(my_matrix[2]);
    00119         // check for positive determinant <=> right handed coordinate system of row vectors
    00120         assert( (my_matrix[0] ^ my_matrix[1]) * my_matrix[2] > 0 );
    00121     }
    00122
    00123     /// Exponentiate a vector in the Lie algebra to generate a new SO3.
    00124     /// See the Detailed Description for details of this vector.
    00125     template<int S, typename VP, typename A> inline static SO3 exp(const Vector<S,VP,A>& vect);
    00126
    00127     /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
    00128     /// See the Detailed Description for details of this vector.
    00129     inline Vector<3, Precision> ln() const;
    00130
    00131     /// Returns the inverse of this matrix (=the transpose, so this is a fast operation)
    00132     SO3 inverse() const { return SO3(*this, Invert()); }
    00133
    00134     /// Right-multiply by another rotation matrix
    00135     template <typename P>
    00136     SO3& operator *=(const SO3<P>& rhs) {
    00137         *this = *this * rhs;
    00138         return *this;
    00139     }
    00140
    00141     /// Right-multiply by another rotation matrix
    00142     template<typename P>
    00143     SO3<typename Internal::MultiplyType<Precision, P>::type> operator *(const SO3<P>& rhs) const {
    00144         return SO3<typename Internal::MultiplyType<Precision, P>::type>(*this,rhs);
    00145     }
    00146
    00147     /// Returns the SO3 as a Matrix<3>
    00148     const Matrix<3,3, Precision> & get_matrix() const {return my_matrix;}
    00149
    00150     /// Returns the i-th generator.  The generators of a Lie group are the basis
    00151     /// for the space of the Lie algebra.  For %SO3, the generators are three
    00152     /// \f$3\times3\f$ matrices representing the three possible (linearised)
    00153     /// rotations.
    00154     inline static Matrix<3,3, Precision> generator(int i){
    00155         Matrix<3,3,Precision> result(Zeros);
    00156         result[(i+1)%3][(i+2)%3] = -1;
    00157         result[(i+2)%3][(i+1)%3] = 1;
    00158         return result;
    00159     }
    00160
    00161   /// Returns the i-th generator times pos
    00162   template<typename Base>
    00163   inline static Vector<3,Precision> generator_field(int i, const Vector<3, Precision, Base>& pos)
    00164   {
    00165     Vector<3, Precision> result;
    00166     result[i]=0;
    00167     result[(i+1)%3] = - pos[(i+2)%3];
    00168     result[(i+2)%3] = pos[(i+1)%3];
    00169     return result;
    00170   }
    00171
    00172     /// Transfer a vector in the Lie Algebra from one
    00173     /// co-ordinate frame to another such that for a matrix
    00174     /// \f$ M \f$, the adjoint \f$Adj()\f$ obeys
    00175     /// \f$ e^{\text{Adj}(v)} = Me^{v}M^{-1} \f$
    00176     template <int S, typename A>
    00177     inline Vector<3, Precision> adjoint(const Vector<S, Precision, A>& vect) const
    00178     {
    00179         SizeMismatch<3, S>::test(3, vect.size());
    00180         return *this * vect;
    00181     }
    00182
    00183     template <typename PA, typename PB>
    00184     inline SO3(const SO3<PA>& a, const SO3<PB>& b) : my_matrix(a.get_matrix()*b.get_matrix()) {}
    00185
    00186 private:
    00187     struct Invert {};
    00188     inline SO3(const SO3& so3, const Invert&) : my_matrix(so3.my_matrix.T()) {}
    00189
    00190     Matrix<3,3, Precision> my_matrix;
    00191 };
    00192
    00193 /// Write an SO3 to a stream
    00194 /// @relates SO3
    00195 template <typename Precision>
    00196 inline std::ostream& operator<< (std::ostream& os, const SO3<Precision>& rhs){
    00197     return os << rhs.get_matrix();
    00198 }
    00199
    00200 /// Read from SO3 to a stream
    00201 /// @relates SO3
    00202 template <typename Precision>
    00203 inline std::istream& operator>>(std::istream& is, SO3<Precision>& rhs){
    00204     is >> rhs.my_matrix;
    00205     rhs.coerce();
    00206     return is;
    00207 }
    00208
    00209 ///Compute a rotation exponential using the Rodrigues Formula.
    00210 ///The rotation axis is given by \f$\vec{w}\f$, and the rotation angle must
    00211 ///be computed using \f$ \theta = |\vec{w}|\f$. This is provided as a separate
    00212 ///function primarily to allow fast and rough matrix exponentials using fast
    00213 ///and rough approximations to \e A and \e B.
    00214 ///
    00215 ///@param w Vector about which to rotate.
    00216 ///@param A \f$\frac{\sin \theta}{\theta}\f$
    00217 ///@param B \f$\frac{1 - \cos \theta}{\theta^2}\f$
    00218 ///@param R Matrix to hold the return value.
    00219 ///@relates SO3
    00220 template <typename Precision, int S, typename VP, typename VA, typename MA>
    00221 inline void rodrigues_so3_exp(const Vector<S,VP, VA>& w, const Precision A, const Precision B, Matrix<3,3,Precision,MA>& R){
    00222     SizeMismatch<3,S>::test(3, w.size());
    00223     {
    00224         const Precision wx2 = (Precision)w[0]*w[0];
    00225         const Precision wy2 = (Precision)w[1]*w[1];
    00226         const Precision wz2 = (Precision)w[2]*w[2];
    00227
    00228         R[0][0] = 1.0 - B*(wy2 + wz2);
    00229         R[1][1] = 1.0 - B*(wx2 + wz2);
    00230         R[2][2] = 1.0 - B*(wx2 + wy2);
    00231     }
    00232     {
    00233         const Precision a = A*w[2];
    00234         const Precision b = B*(w[0]*w[1]);
    00235         R[0][1] = b - a;
    00236         R[1][0] = b + a;
    00237     }
    00238     {
    00239         const Precision a = A*w[1];
    00240         const Precision b = B*(w[0]*w[2]);
    00241         R[0][2] = b + a;
    00242         R[2][0] = b - a;
    00243     }
    00244     {
    00245         const Precision a = A*w[0];
    00246         const Precision b = B*(w[1]*w[2]);
    00247         R[1][2] = b - a;
    00248         R[2][1] = b + a;
    00249     }
    00250 }
    00251
    00252 ///Perform the exponential of the matrix \f$ \sum_i w_iG_i\f$
    00253 ///@param w Weightings of the generator matrices.
    00254 template <typename Precision>
    00255 template<int S, typename VP, typename VA>
    00256 inline SO3<Precision> SO3<Precision>::exp(const Vector<S,VP,VA>& w){
    00257     using std::sqrt;
    00258     using std::sin;
    00259     using std::cos;
    00260     SizeMismatch<3,S>::test(3, w.size());
    00261
    00262     static const Precision one_6th = 1.0/6.0;
    00263     static const Precision one_20th = 1.0/20.0;
    00264
    00265     SO3<Precision> result;
    00266
    00267     const Precision theta_sq = w*w;
    00268     const Precision theta = sqrt(theta_sq);
    00269     Precision A, B;
    00270     //Use a Taylor series expansion near zero. This is required for
    00271     //accuracy, since sin t / t and (1-cos t)/t^2 are both 0/0.
    00272     if (theta_sq < 1e-8) {
    00273         A = 1.0 - one_6th * theta_sq;
    00274         B = 0.5;
    00275     } else {
    00276         if (theta_sq < 1e-6) {
    00277             B = 0.5 - 0.25 * one_6th * theta_sq;
    00278             A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
    00279         } else {
    00280             const Precision inv_theta = 1.0/theta;
    00281             A = sin(theta) * inv_theta;
    00282             B = (1 - cos(theta)) * (inv_theta * inv_theta);
    00283         }
    00284     }
    00285     rodrigues_so3_exp(w, A, B, result.my_matrix);
    00286     return result;
    00287 }
    00288
    00289 template <typename Precision>
    00290 inline Vector<3, Precision> SO3<Precision>::ln() const{
    00291     using std::sqrt;
    00292     Vector<3, Precision> result;
    00293
    00294     const Precision cos_angle = (my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2] - 1.0) * 0.5;
    00295     result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
    00296     result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
    00297     result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
    00298
    00299     Precision sin_angle_abs = sqrt(result*result);
    00300     if (cos_angle > M_SQRT1_2) {            // [0 - Pi/4[ use asin
    00301         if(sin_angle_abs > 0){
    00302             result *= asin(sin_angle_abs) / sin_angle_abs;
    00303         }
    00304     } else if( cos_angle > -M_SQRT1_2) {    // [Pi/4 - 3Pi/4[ use acos, but antisymmetric part
    00305         const Precision angle = acos(cos_angle);
    00306         result *= angle / sin_angle_abs;
    00307     } else {  // rest use symmetric part
    00308         // antisymmetric part vanishes, but still large rotation, need information from symmetric part
    00309         const Precision angle = M_PI - asin(sin_angle_abs);
    00310         const Precision d0 = my_matrix[0][0] - cos_angle,
    00311             d1 = my_matrix[1][1] - cos_angle,
    00312             d2 = my_matrix[2][2] - cos_angle;
    00313         TooN::Vector<3, Precision> r2;
    00314         if(d0*d0 > d1*d1 && d0*d0 > d2*d2){ // first is largest, fill with first column
    00315             r2[0] = d0;
    00316             r2[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
    00317             r2[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
    00318         } else if(d1*d1 > d2*d2) {              // second is largest, fill with second column
    00319             r2[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
    00320             r2[1] = d1;
    00321             r2[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
    00322         } else {                                // third is largest, fill with third column
    00323             r2[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
    00324             r2[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
    00325             r2[2] = d2;
    00326         }
    00327         // flip, if we point in the wrong direction!
    00328         if(r2 * result < 0)
    00329             r2 *= -1;
    00330         r2 = unit(r2);
    00331         result = TooN::operator*(angle,r2);
    00332     }
    00333     return result;
    00334 }
    00335
    00336 /// Right-multiply by a Vector
    00337 /// @relates SO3
    00338 template<int S, typename P, typename PV, typename A> inline
    00339 Vector<3, typename Internal::MultiplyType<P, PV>::type> operator*(const SO3<P>& lhs, const Vector<S, PV, A>& rhs){
    00340     return lhs.get_matrix() * rhs;
    00341 }
    00342
    00343 /// Left-multiply by a Vector
    00344 /// @relates SO3
    00345 template<int S, typename P, typename PV, typename A> inline
    00346 Vector<3, typename Internal::MultiplyType<PV, P>::type> operator*(const Vector<S, PV, A>& lhs, const SO3<P>& rhs){
    00347     return lhs * rhs.get_matrix();
    00348 }
    00349
    00350 /// Right-multiply by a matrix
    00351 /// @relates SO3
    00352 template<int R, int C, typename P, typename PM, typename A> inline
    00353 Matrix<3, C, typename Internal::MultiplyType<P, PM>::type> operator*(const SO3<P>& lhs, const Matrix<R, C, PM, A>& rhs){
    00354     return lhs.get_matrix() * rhs;
    00355 }
    00356
    00357 /// Left-multiply by a matrix
    00358 /// @relates SO3
    00359 template<int R, int C, typename P, typename PM, typename A> inline
    00360 Matrix<R, 3, typename Internal::MultiplyType<PM, P>::type> operator*(const Matrix<R, C, PM, A>& lhs, const SO3<P>& rhs){
    00361     return lhs * rhs.get_matrix();
    00362 }
    00363
    00364 #if 0   // will be moved to another header file ?
    00365
    00366 template <class A> inline
    00367 Vector<3> transform(const SO3& pose, const FixedVector<3,A>& x) { return pose*x; }
    00368
    00369 template <class A1, class A2> inline
    00370 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
    00371 {
    00372     J_x = pose.get_matrix();
    00373     return pose * x;
    00374 }
    00375
    00376 template <class A1, class A2, class A3> inline
    00377 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,3,A3>& J_pose)
    00378 {
    00379     J_x = pose.get_matrix();
    00380     const Vector<3> cx = pose * x;
    00381     J_pose[0][0] = J_pose[1][1] = J_pose[2][2] = 0;
    00382     J_pose[1][0] = -(J_pose[0][1] = cx[2]);
    00383     J_pose[0][2] = -(J_pose[2][0] = cx[1]);
    00384     J_pose[2][1] = -(J_pose[1][2] = cx[0]);
    00385     return cx;
    00386 }
    00387
    00388 template <class A1, class A2, class A3> inline
    00389 Vector<2> project_transformed_point(const SO3& pose, const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,3,A3>& J_pose)
    00390 {
    00391     const double z_inv = 1.0/in_frame[2];
    00392     const double x_z_inv = in_frame[0]*z_inv;
    00393     const double y_z_inv = in_frame[1]*z_inv;
    00394     const double cross = x_z_inv * y_z_inv;
    00395     J_pose[0][0] = -cross;
    00396     J_pose[0][1] = 1 + x_z_inv*x_z_inv;
    00397     J_pose[0][2] = -y_z_inv;
    00398     J_pose[1][0] = -1 - y_z_inv*y_z_inv;
    00399     J_pose[1][1] =  cross;
    00400     J_pose[1][2] =  x_z_inv;
    00401
    00402     const TooN::Matrix<3>& R = pose.get_matrix();
    00403     J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
    00404     J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
    00405     J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
    00406     J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
    00407     J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
    00408     J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
    00409
    00410     return makeVector(x_z_inv, y_z_inv);
    00411 }
    00412
    00413
    00414 template <class A1> inline
    00415 Vector<2> transform_and_project(const SO3& pose, const FixedVector<3,A1>& x)
    00416 {
    00417     return project(transform(pose,x));
    00418 }
    00419
    00420 template <class A1, class A2, class A3> inline
    00421 Vector<2> transform_and_project(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,3,A3>& J_pose)
    00422 {
    00423     return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
    00424 }
    00425
    00426 #endif
    00427
    00428 }
    00429
    00430 #endif