examples/homography/TooN/so3.h

00001 
00002 /*
00003          Copyright (C) 2005 Tom Drummond
00004 
00005      This library is free software; you can redistribute it and/or
00006      modify it under the terms of the GNU Lesser General Public
00007      License as published by the Free Software Foundation; either
00008      version 2.1 of the License, or (at your option) any later version.
00009 
00010      This library is distributed in the hope that it will be useful,
00011      but WITHOUT ANY WARRANTY; without even the implied warranty of
00012      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013      Lesser General Public License for more details.
00014 
00015      You should have received a copy of the GNU Lesser General Public
00016      License along with this library; if not, write to the Free Software
00017      Foundation, Inc.
00018      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020 #ifndef __SO3_H
00021 #define __SO3_H
00022 
00023 #include <TooN/TooN.h>
00024 #include <TooN/LU.h>
00025 #include <TooN/helpers.h>
00026 
00027 #ifndef TOON_NO_NAMESPACE
00028 namespace TooN {
00029 #endif
00030 
00031     class SE3;
00032 
00033 class SO3 {
00034 public:
00035   friend std::istream& operator>>(std::istream& is, SO3& rhs);
00036   friend std::istream& operator>>(std::istream& is, class SE3& rhs);
00037   friend class SE3;
00038   inline SO3();
00039   inline SO3(const Matrix<3>& rhs);
00040 
00041   inline SO3& operator=(const Matrix<3>& rhs);
00042   template <class Accessor> static inline void coerce(FixedMatrix<3,3,Accessor>& M);
00043 
00044   template<class Accessor> inline static SO3 exp(const FixedVector<3,Accessor>& vect);
00045   template <class Accessor> inline static double exp_with_half(const FixedVector<3,Accessor>& vect, SO3& first, SO3& second, double& shtot);
00046 
00047   inline Vector<3> ln() const;
00048 
00049   inline double operator[](int i){return my_matrix[i/3][i%3];}
00050 
00051   inline SO3 inverse() const;
00052 
00053   inline SO3& operator *=(const SO3& rhs){
00054     my_matrix=my_matrix*rhs.my_matrix;
00055     return *this;
00056   }
00057 
00058   inline SO3 operator *(const SO3& rhs) const {
00059       return SO3(*this,rhs);
00060   }
00061 
00062   inline const Matrix<3>& get_matrix()const {return my_matrix;}
00063   // returns i-th generator times pos
00064   inline static Vector<3> generator_field(int i, Vector<3> pos);
00065 
00066   // adjoint transformation on the Lie algebra
00067   inline Vector<3> adjoint(Vector<3> vect) const ;
00068 
00069  private:
00070   struct Invert {};
00071   inline SO3(const SO3& so3, const Invert&) : my_matrix(so3.my_matrix.T()) {}
00072   inline SO3(const SO3& a, const SO3& b) : my_matrix(a.my_matrix*b.my_matrix) {}
00073       
00074   Matrix<3> my_matrix;
00075 };
00076 
00077 inline std::ostream& operator<< (std::ostream& os, const SO3& rhs){
00078   return os << rhs.get_matrix();
00079 }
00080 
00081 inline std::istream& operator>>(std::istream& is, SO3& rhs){
00082   return is >> rhs.my_matrix;
00083 }
00084 
00085 
00086 
00087 
00088 template<class Accessor> inline
00089 Vector<3> operator*(const SO3& lhs, const FixedVector<3,Accessor>& rhs){
00090   return lhs.get_matrix() * rhs;
00091 }
00092 
00093 template<class Accessor> inline
00094 Vector<3> operator*(const SO3& lhs, const DynamicVector<Accessor>& rhs){
00095   assert(rhs.size() == 3);
00096   return lhs.get_matrix() * rhs;
00097 }
00098 
00099 
00100 
00101 
00102 template<class Accessor> inline
00103 Vector<3> operator*(const DynamicVector<Accessor>& lhs, const SO3& rhs){
00104   assert(lhs.size() == 3);
00105   return lhs * rhs.get_matrix();
00106 }
00107 
00108 template<class Accessor> inline
00109 Vector<3> operator*(const FixedVector<3,Accessor>& lhs, const SO3& rhs){
00110   return lhs * rhs.get_matrix();
00111 }
00112 
00113 template <int RHS, class Accessor> inline
00114 Matrix<3,RHS> operator*(const SO3& lhs, const FixedMatrix<3,RHS,Accessor>& rhs){
00115   return lhs.get_matrix() * rhs;
00116 }
00117 
00118 template <int LHS, class Accessor> inline
00119 Matrix<LHS,3> operator*(const FixedMatrix<LHS,3,Accessor>& lhs, const SO3& rhs){
00120   return lhs * rhs.get_matrix();
00121 }
00122 
00123 namespace SO3static
00124 {
00125   static double identity[9]={1,0,0,0,1,0,0,0,1};
00126 }
00127 
00128 inline SO3::SO3() :
00129   my_matrix(SO3static::identity)
00130 {}
00131 
00132 inline SO3::SO3(const Matrix<3>& rhs){
00133     *this = rhs;
00134 }
00135 
00136 inline SO3& SO3::operator=(const Matrix<3>& rhs){
00137   my_matrix = rhs;
00138   coerce(my_matrix);
00139   return *this;
00140 }
00141 
00142  template <class Accessor> inline void SO3::coerce(FixedMatrix<3,3, Accessor>& M){
00143      normalize(M[0]);
00144      M[1] -= M[0] * (M[0]*M[1]);
00145      normalize(M[1]);
00146      M[2] -= M[0] * (M[0]*M[2]);
00147      M[2] -= M[1] * (M[1]*M[2]);
00148      normalize(M[2]);
00149 }
00150 
00151 template <class A1, class A2> 
00152 inline void rodrigues_so3_exp(const TooN::FixedVector<3,A1>& w, const double A, const double B, TooN::FixedMatrix<3,3,A2>& R)
00153 {
00154     {
00155         const double wx2 = w[0]*w[0];
00156         const double wy2 = w[1]*w[1];
00157         const double wz2 = w[2]*w[2];
00158         
00159         R[0][0] = 1.0 - B*(wy2 + wz2);
00160         R[1][1] = 1.0 - B*(wx2 + wz2);
00161         R[2][2] = 1.0 - B*(wx2 + wy2);
00162     }
00163     {
00164         const double a = A*w[2];
00165         const double b = B*(w[0]*w[1]);
00166         R[0][1] = b - a;
00167         R[1][0] = b + a;
00168     }
00169     {
00170         const double a = A*w[1];
00171         const double b = B*(w[0]*w[2]);
00172         R[0][2] = b + a;
00173         R[2][0] = b - a;
00174     }
00175     {
00176         const double a = A*w[0];
00177         const double b = B*(w[1]*w[2]);
00178         R[1][2] = b - a;
00179         R[2][1] = b + a;
00180     }
00181 }
00182 
00183 template <class Accessor> 
00184 inline SO3 SO3::exp(const FixedVector<3,Accessor>& w){
00185     static const double one_6th = 1.0/6.0;
00186     static const double one_20th = 1.0/20.0;
00187 
00188     SO3 result;
00189 
00190     const double theta_sq = w*w;
00191     const double theta = sqrt(theta_sq);
00192     double A, B;
00193     
00194     if (theta_sq < 1e-8) {
00195         A = 1.0 - one_6th * theta_sq;
00196         B = 0.5;
00197     } else {
00198         if (theta_sq < 1e-6) {
00199             B = 0.5 - 0.25 * one_6th * theta_sq;
00200             A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
00201         } else {
00202             const double inv_theta = 1.0/theta;
00203             A = sin(theta) * inv_theta;
00204             B = (1 - cos(theta)) * (inv_theta * inv_theta);
00205         }
00206     }
00207     rodrigues_so3_exp(w, A, B, result.my_matrix);
00208     return result;
00209 }
00210 
00211 inline Vector<3> SO3::ln() const{
00212   Vector<3> result;
00213 
00214   double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
00215  
00216   // 2 * cos(theta) - 1 == trace
00217 
00218   result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
00219   result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
00220   result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
00221 
00222   if (trace > -0.95) {
00223       double sin_angle_abs = sqrt(result*result);
00224       if (sin_angle_abs > 0.00001) {
00225           double angle;
00226           if(sin_angle_abs > 1.0) {
00227               sin_angle_abs = 1.0;
00228               angle = M_PI_2;
00229           } else {
00230               angle = asin(sin_angle_abs);
00231               if (trace < 1)
00232                   angle = M_PI - angle;
00233           }
00234           result *= angle / sin_angle_abs;
00235       }
00236       return result;
00237   } else {
00238       TooN::Matrix<3> A = my_matrix;
00239       A[0][0] -= 1.0;
00240       A[1][1] -= 1.0;
00241       A[2][2] -= 1.0;
00242       TooN::LU<3> lu(A);     
00243       const TooN::Matrix<3,3,TooN::ColMajor>& u = lu.get_lu().T();
00244       const double u0 = fabs(u[0][0]);
00245       const double u1 = fabs(u[1][1]);
00246       const double u2 = fabs(u[2][2]);
00247       int row;      
00248       if (u0 < u1)
00249           row = u0 < u2 ? 0 : 2;
00250       else
00251           row = u1 < u2 ? 1 : 2;      
00252       //std::cerr << u << std::endl;
00253       //std::cerr << "row = " << row << std::endl;
00254       TooN::Vector<3> r;
00255       const double factor = acos(0.5*(trace-1));
00256       switch (row) {
00257       case 0: r[0] = factor; r[1] = 0; r[2] = 0; break;
00258       case 1: r[0] = u[0][1]*u[2][2]; r[1] = -u[0][0]*u[2][2]; r[2] = 0; r *= factor/sqrt(r*r); break;
00259       case 2: r[0] = u[0][1]*u[1][2] - u[0][2]*u[1][1]; r[1] = -u[0][0]*u[1][2]; r[2] = u[0][0]*u[1][1];  r *= factor/sqrt(r*r); break;
00260       }
00261       if (result * r < 0)
00262           return -r;
00263       else
00264           return r;
00265   }
00266 }
00267 
00268 inline SO3 SO3::inverse() const{
00269     return SO3(*this, Invert());
00270 }
00271 
00272 
00273 // SO3& SO3::operator *=(const SO3& rhs){
00274 //   *this = *this * rhs;
00275 //   return *this;
00276 // }
00277 
00278 // SO3 SO3::operator *(const SO3& rhs)const{
00279 //   SO3 result;
00280 //   result.my_matrix = my_matrix * rhs.my_matrix;
00281 //   return result;
00282 // }
00283 
00284 inline Vector<3> SO3::generator_field(int i, Vector<3> pos){
00285   Vector<3> result;
00286   result[i]=0;
00287   result[(i+1)%3] = - pos[(i+2)%3];
00288   result[(i+2)%3] = pos[(i+1)%3];
00289   return result;
00290 }
00291 
00292 inline Vector<3> SO3::adjoint(Vector<3> vect)const {
00293   return my_matrix * vect;
00294 }
00295 
00296 #ifndef TOON_NO_NAMESPACE
00297 }
00298 #endif
00299 
00300 #endif

Generated on Fri Feb 22 18:26:54 2008 for QVision by  doxygen 1.5.3