00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00064 inline static Vector<3> generator_field(int i, Vector<3> pos);
00065
00066
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
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
00253
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
00274
00275
00276
00277
00278
00279
00280
00281
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