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 
00036 namespace TooN {
00037 
00038 template <typename Precision> class SO3;
00039 template <typename Precision> class SE3;
00040 
00041 template<class Precision> inline std::istream & operator>>(std::istream &, SO3<Precision> & );
00042 template<class Precision> inline std::istream & operator>>(std::istream &, SE3<Precision> & );
00043 
00051 template <typename Precision = double>
00052 class SO3 {
00053 public:
00054     friend std::istream& operator>> <Precision> (std::istream& is, SO3<Precision> & rhs);
00055     friend std::istream& operator>> <Precision> (std::istream& is, SE3<Precision> & rhs);
00056     friend class SE3<Precision>;
00057     
00059     SO3() : my_matrix(Identity) {}
00060     
00061     template <int S, typename P, typename A>
00062     SO3(const Vector<S, P, A> & v) { *this = exp(v); }
00063     
00064     template <int R, int C, typename P, typename A>
00065     SO3(const Matrix<R,C,P,A>& rhs) { *this = rhs; }
00066     
00071     template <int S1, int S2, typename P1, typename P2, typename A1, typename A2>
00072     SO3(const Vector<S1, P1, A1> & a, const Vector<S2, P2, A2> & b ){
00073         SizeMismatch<3,S1>::test(3, a.size());
00074         SizeMismatch<3,S2>::test(3, b.size());
00075         Vector<3, Precision> n = a ^ b;
00076         if(norm_sq(n) == 0) {
00077             my_matrix = Identity;
00078             return;
00079         }
00080         n = unit(n);
00081         Matrix<3> R1;
00082         R1.T()[0] = unit(a);
00083         R1.T()[1] = n;
00084         R1.T()[2] = n ^ R1.T()[0];
00085         my_matrix.T()[0] = unit(b);
00086         my_matrix.T()[1] = n;
00087         my_matrix.T()[2] = n ^ my_matrix.T()[0];
00088         my_matrix = my_matrix * R1.T();
00089     }
00090     
00093     template <int R, int C, typename P, typename A>
00094     SO3& operator=(const Matrix<R,C,P,A> & rhs) {
00095         my_matrix = rhs;
00096         coerce();
00097         return *this;
00098     }
00099     
00101     void coerce() {
00102         my_matrix[0] = unit(my_matrix[0]);
00103         my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]);
00104         my_matrix[1] = unit(my_matrix[1]);
00105         my_matrix[2] -= my_matrix[0] * (my_matrix[0]*my_matrix[2]);
00106         my_matrix[2] -= my_matrix[1] * (my_matrix[1]*my_matrix[2]);
00107         my_matrix[2] = unit(my_matrix[2]);
00108     }
00109     
00112     template<int S, typename A> inline static SO3 exp(const Vector<S,Precision,A>& vect);
00113     
00116     inline Vector<3, Precision> ln() const;
00117     
00119     SO3 inverse() const { return SO3(*this, Invert()); }
00120 
00122     SO3& operator *=(const SO3& rhs) {
00123         *this = *this * rhs;
00124         return *this;
00125     }
00126 
00128     SO3 operator *(const SO3& rhs) const { return SO3(*this,rhs); }
00129 
00131     const Matrix<3,3, Precision> & get_matrix() const {return my_matrix;}
00132 
00137     inline static Matrix<3,3, Precision> generator(int i){
00138         Matrix<3,3,Precision> result(Zeros);
00139         result[(i+1)%3][(i+2)%3] = -1;
00140         result[(i+2)%3][(i+1)%3] = 1;
00141         return result;
00142     }
00143 
00145   template<typename Base>
00146   inline static Vector<3,Precision> generator_field(int i, const Vector<3, Precision, Base>& pos)
00147   {
00148     Vector<3, Precision> result;
00149     result[i]=0;
00150     result[(i+1)%3] = - pos[(i+2)%3];
00151     result[(i+2)%3] = pos[(i+1)%3];
00152     return result;
00153   }
00154 
00159     template <int S, typename A>
00160     inline Vector<3, Precision> adjoint(Vector<3, Precision, A> vect) const { return *this * vect; }
00161     
00162 private:
00163     struct Invert {};
00164     inline SO3(const SO3& so3, const Invert&) : my_matrix(so3.my_matrix.T()) {}
00165     inline SO3(const SO3& a, const SO3& b) : my_matrix(a.my_matrix*b.my_matrix) {}
00166     
00167     Matrix<3,3, Precision> my_matrix;
00168 };
00169 
00172 template <typename Precision>
00173 inline std::ostream& operator<< (std::ostream& os, const SO3<Precision>& rhs){
00174     return os << rhs.get_matrix();
00175 }
00176 
00179 template <typename Precision>
00180 inline std::istream& operator>>(std::istream& is, SO3<Precision>& rhs){
00181     return is >> rhs.my_matrix;
00182     rhs.coerce();
00183 }
00184 
00185 template <typename Precision, typename VA, typename MA>
00186 inline void rodrigues_so3_exp(const Vector<3,Precision, VA>& w, const Precision A, const Precision B, Matrix<3,3,Precision,MA>& R){
00187     {
00188         const Precision wx2 = w[0]*w[0];
00189         const Precision wy2 = w[1]*w[1];
00190         const Precision wz2 = w[2]*w[2];
00191     
00192         R[0][0] = 1.0 - B*(wy2 + wz2);
00193         R[1][1] = 1.0 - B*(wx2 + wz2);
00194         R[2][2] = 1.0 - B*(wx2 + wy2);
00195     }
00196     {
00197         const Precision a = A*w[2];
00198         const Precision b = B*(w[0]*w[1]);
00199         R[0][1] = b - a;
00200         R[1][0] = b + a;
00201     }
00202     {
00203         const Precision a = A*w[1];
00204         const Precision b = B*(w[0]*w[2]);
00205         R[0][2] = b + a;
00206         R[2][0] = b - a;
00207     }
00208     {
00209         const Precision a = A*w[0];
00210         const Precision b = B*(w[1]*w[2]);
00211         R[1][2] = b - a;
00212         R[2][1] = b + a;
00213     }
00214 }
00215 
00216 template <typename Precision>
00217 template<int S, typename VA>
00218 inline SO3<Precision> SO3<Precision>::exp(const Vector<S,Precision,VA>& w){
00219     SizeMismatch<3,S>::test(3, w.size());
00220     
00221     static const Precision one_6th = 1.0/6.0;
00222     static const Precision one_20th = 1.0/20.0;
00223     
00224     SO3<Precision> result;
00225     
00226     const Precision theta_sq = w*w;
00227     const Precision theta = sqrt(theta_sq);
00228     Precision A, B;
00229     
00230     if (theta_sq < 1e-8) {
00231         A = 1.0 - one_6th * theta_sq;
00232         B = 0.5;
00233     } else {
00234         if (theta_sq < 1e-6) {
00235             B = 0.5 - 0.25 * one_6th * theta_sq;
00236             A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
00237         } else {
00238             const Precision inv_theta = 1.0/theta;
00239             A = sin(theta) * inv_theta;
00240             B = (1 - cos(theta)) * (inv_theta * inv_theta);
00241         }
00242     }
00243     rodrigues_so3_exp(w, A, B, result.my_matrix);
00244     return result;
00245 }
00246 
00247 template <typename Precision>
00248 inline Vector<3, Precision> SO3<Precision>::ln() const{
00249     Vector<3, Precision> result;
00250     
00251     const Precision cos_angle = (my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2] - 1.0) * 0.5;
00252     result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
00253     result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
00254     result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
00255     
00256     Precision sin_angle_abs = sqrt(result*result);
00257     if (cos_angle > M_SQRT1_2) {            // [0 - Pi/4[ use asin
00258         if(sin_angle_abs > 0){
00259             result *= asin(sin_angle_abs) / sin_angle_abs;
00260         }
00261     } else if( cos_angle > -M_SQRT1_2) {    // [Pi/4 - 3Pi/4[ use acos, but antisymmetric part
00262         double angle = acos(cos_angle);
00263         result *= angle / sin_angle_abs;        
00264     } else {  // rest use symmetric part
00265         // antisymmetric part vanishes, but still large rotation, need information from symmetric part
00266         const Precision angle = M_PI - asin(sin_angle_abs);
00267         const Precision d0 = my_matrix[0][0] - cos_angle,
00268             d1 = my_matrix[1][1] - cos_angle,
00269             d2 = my_matrix[2][2] - cos_angle;
00270         TooN::Vector<3> r2;
00271         if(fabs(d0) > fabs(d1) && fabs(d0) > fabs(d2)){ // first is largest, fill with first column
00272             r2[0] = d0;
00273             r2[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
00274             r2[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
00275         } else if(fabs(d1) > fabs(d2)) {                // second is largest, fill with second column
00276             r2[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
00277             r2[1] = d1;
00278             r2[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
00279         } else {                                // third is largest, fill with third column
00280             r2[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
00281             r2[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
00282             r2[2] = d2;
00283         }
00284         // flip, if we point in the wrong direction!
00285         if(r2 * result < 0)
00286             r2 *= -1;
00287         r2 = unit(r2);
00288         result = angle * r2;
00289     } 
00290     return result;
00291 }
00292 
00295 template<int S, typename P, typename PV, typename A> inline
00296 Vector<3, typename Internal::MultiplyType<P, PV>::type> operator*(const SO3<P>& lhs, const Vector<S, PV, A>& rhs){
00297     return lhs.get_matrix() * rhs;
00298 }
00299 
00302 template<int S, typename P, typename PV, typename A> inline
00303 Vector<3, typename Internal::MultiplyType<PV, P>::type> operator*(const Vector<S, PV, A>& lhs, const SO3<P>& rhs){
00304     return lhs * rhs.get_matrix();
00305 }
00306 
00309 template<int R, int C, typename P, typename PM, typename A> inline
00310 Matrix<3, C, typename Internal::MultiplyType<P, PM>::type> operator*(const SO3<P>& lhs, const Matrix<R, C, PM, A>& rhs){
00311     return lhs.get_matrix() * rhs;
00312 }
00313 
00316 template<int R, int C, typename P, typename PM, typename A> inline
00317 Matrix<R, 3, typename Internal::MultiplyType<PM, P>::type> operator*(const Matrix<R, C, PM, A>& lhs, const SO3<P>& rhs){
00318     return lhs * rhs.get_matrix();
00319 }
00320 
00321 #if 0   // will be moved to another header file ?
00322 
00323 template <class A> inline
00324 Vector<3> transform(const SO3& pose, const FixedVector<3,A>& x) { return pose*x; }
00325 
00326 template <class A1, class A2> inline
00327 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
00328 {
00329     J_x = pose.get_matrix();
00330     return pose * x;
00331 }
00332 
00333 template <class A1, class A2, class A3> inline
00334 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,3,A3>& J_pose)
00335 {
00336     J_x = pose.get_matrix();
00337     const Vector<3> cx = pose * x;
00338     J_pose[0][0] = J_pose[1][1] = J_pose[2][2] = 0;
00339     J_pose[1][0] = -(J_pose[0][1] = cx[2]);
00340     J_pose[0][2] = -(J_pose[2][0] = cx[1]);
00341     J_pose[2][1] = -(J_pose[1][2] = cx[0]);
00342     return cx;
00343 }
00344 
00345 template <class A1, class A2, class A3> inline
00346 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)
00347 {
00348     const double z_inv = 1.0/in_frame[2];
00349     const double x_z_inv = in_frame[0]*z_inv;
00350     const double y_z_inv = in_frame[1]*z_inv;
00351     const double cross = x_z_inv * y_z_inv;
00352     J_pose[0][0] = -cross;
00353     J_pose[0][1] = 1 + x_z_inv*x_z_inv;
00354     J_pose[0][2] = -y_z_inv;
00355     J_pose[1][0] = -1 - y_z_inv*y_z_inv;
00356     J_pose[1][1] =  cross;
00357     J_pose[1][2] =  x_z_inv;
00358 
00359     const TooN::Matrix<3>& R = pose.get_matrix();
00360     J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00361     J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00362     J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00363     J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00364     J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00365     J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00366 
00367     return makeVector(x_z_inv, y_z_inv);
00368 }
00369 
00370 
00371 template <class A1> inline
00372 Vector<2> transform_and_project(const SO3& pose, const FixedVector<3,A1>& x)
00373 {
00374     return project(transform(pose,x));
00375 }
00376 
00377 template <class A1, class A2, class A3> inline
00378 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)
00379 {
00380     return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
00381 }
00382 
00383 #endif
00384 
00385 }
00386 
00387 #endif

Generated on Thu May 7 20:28:41 2009 for TooN by  doxygen 1.5.3