00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
00042 template<class Precision> inline std::istream & operator>>(std::istream &, SO3<Precision> & );
00043 template<class Precision> inline std::istream & operator>>(std::istream &, SE3<Precision> & );
00044
00045
00046
00047
00048
00049
00050
00051
00052 template <typename Precision = double>
00053 class SO3 {
00054 public:
00055 friend std::istream& operator>> <Precision> (std::istream& is, SO3<Precision> & rhs);
00056 friend std::istream& operator>> <Precision> (std::istream& is, SE3<Precision> & rhs);
00057 friend class SE3<Precision>;
00058
00059
00060 SO3() : my_matrix(Identity) {}
00061
00062
00063 template <int S, typename P, typename A>
00064 SO3(const Vector<S, P, A> & v) { *this = exp(v); }
00065
00066
00067 template <int R, int C, typename P, typename A>
00068 SO3(const Matrix<R,C,P,A>& rhs) { *this = rhs; }
00069
00070
00071
00072
00073
00074
00075 template <int S1, int S2, typename P1, typename P2, typename A1, typename A2>
00076 SO3(const Vector<S1, P1, A1> & a, const Vector<S2, P2, A2> & b ){
00077 SizeMismatch<3,S1>::test(3, a.size());
00078 SizeMismatch<3,S2>::test(3, b.size());
00079 Vector<3, Precision> n = a ^ b;
00080 if(norm_sq(n) == 0) {
00081
00082
00083 assert(a*b>=0);
00084 my_matrix = Identity;
00085 return;
00086 }
00087 n = unit(n);
00088 Matrix<3> R1;
00089 R1.T()[0] = unit(a);
00090 R1.T()[1] = n;
00091 R1.T()[2] = n ^ R1.T()[0];
00092 my_matrix.T()[0] = unit(b);
00093 my_matrix.T()[1] = n;
00094 my_matrix.T()[2] = n ^ my_matrix.T()[0];
00095 my_matrix = my_matrix * R1.T();
00096 }
00097
00098
00099
00100 template <int R, int C, typename P, typename A>
00101 SO3& operator=(const Matrix<R,C,P,A> & rhs) {
00102 my_matrix = rhs;
00103 coerce();
00104 return *this;
00105 }
00106
00107
00108 void coerce() {
00109 my_matrix[0] = unit(my_matrix[0]);
00110 my_matrix[1] -= my_matrix[0] * (my_matrix[0]*my_matrix[1]);
00111 my_matrix[1] = unit(my_matrix[1]);
00112 my_matrix[2] -= my_matrix[0] * (my_matrix[0]*my_matrix[2]);
00113 my_matrix[2] -= my_matrix[1] * (my_matrix[1]*my_matrix[2]);
00114 my_matrix[2] = unit(my_matrix[2]);
00115
00116 assert( (my_matrix[0] ^ my_matrix[1]) * my_matrix[2] > 0 );
00117 }
00118
00119
00120
00121 template<int S, typename VP, typename A> inline static SO3 exp(const Vector<S,VP,A>& vect);
00122
00123
00124
00125 inline Vector<3, Precision> ln() const;
00126
00127
00128 SO3 inverse() const { return SO3(*this, Invert()); }
00129
00130
00131 SO3& operator *=(const SO3& rhs) {
00132 *this = *this * rhs;
00133 return *this;
00134 }
00135
00136
00137 template<typename P>
00138 SO3<typename Internal::MultiplyType<Precision, P>::type> operator *(const SO3<P>& rhs) const {
00139 return SO3<typename Internal::MultiplyType<Precision, P>::type>(*this,rhs);
00140 }
00141
00142
00143 const Matrix<3,3, Precision> & get_matrix() const {return my_matrix;}
00144
00145
00146
00147
00148
00149 inline static Matrix<3,3, Precision> generator(int i){
00150 Matrix<3,3,Precision> result(Zeros);
00151 result[(i+1)%3][(i+2)%3] = -1;
00152 result[(i+2)%3][(i+1)%3] = 1;
00153 return result;
00154 }
00155
00156
00157 template<typename Base>
00158 inline static Vector<3,Precision> generator_field(int i, const Vector<3, Precision, Base>& pos)
00159 {
00160 Vector<3, Precision> result;
00161 result[i]=0;
00162 result[(i+1)%3] = - pos[(i+2)%3];
00163 result[(i+2)%3] = pos[(i+1)%3];
00164 return result;
00165 }
00166
00167
00168
00169
00170
00171 template <int S, typename A>
00172 inline Vector<3, Precision> adjoint(const Vector<S, Precision, A>& vect) const
00173 {
00174 SizeMismatch<3, S>::test(3, vect.size());
00175 return *this * vect;
00176 }
00177
00178 template <typename PA, typename PB>
00179 inline SO3(const SO3<PA>& a, const SO3<PB>& b) : my_matrix(a.get_matrix()*b.get_matrix()) {}
00180
00181 private:
00182 struct Invert {};
00183 inline SO3(const SO3& so3, const Invert&) : my_matrix(so3.my_matrix.T()) {}
00184
00185 Matrix<3,3, Precision> my_matrix;
00186 };
00187
00188
00189
00190 template <typename Precision>
00191 inline std::ostream& operator<< (std::ostream& os, const SO3<Precision>& rhs){
00192 return os << rhs.get_matrix();
00193 }
00194
00195
00196
00197 template <typename Precision>
00198 inline std::istream& operator>>(std::istream& is, SO3<Precision>& rhs){
00199 return is >> rhs.my_matrix;
00200 rhs.coerce();
00201 }
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214 template <typename Precision, int S, typename VP, typename VA, typename MA>
00215 inline void rodrigues_so3_exp(const Vector<S,VP, VA>& w, const Precision A, const Precision B, Matrix<3,3,Precision,MA>& R){
00216 SizeMismatch<3,S>::test(3, w.size());
00217 {
00218 const Precision wx2 = (Precision)w[0]*w[0];
00219 const Precision wy2 = (Precision)w[1]*w[1];
00220 const Precision wz2 = (Precision)w[2]*w[2];
00221
00222 R[0][0] = 1.0 - B*(wy2 + wz2);
00223 R[1][1] = 1.0 - B*(wx2 + wz2);
00224 R[2][2] = 1.0 - B*(wx2 + wy2);
00225 }
00226 {
00227 const Precision a = A*w[2];
00228 const Precision b = B*(w[0]*w[1]);
00229 R[0][1] = b - a;
00230 R[1][0] = b + a;
00231 }
00232 {
00233 const Precision a = A*w[1];
00234 const Precision b = B*(w[0]*w[2]);
00235 R[0][2] = b + a;
00236 R[2][0] = b - a;
00237 }
00238 {
00239 const Precision a = A*w[0];
00240 const Precision b = B*(w[1]*w[2]);
00241 R[1][2] = b - a;
00242 R[2][1] = b + a;
00243 }
00244 }
00245
00246
00247
00248 template <typename Precision>
00249 template<int S, typename VP, typename VA>
00250 inline SO3<Precision> SO3<Precision>::exp(const Vector<S,VP,VA>& w){
00251 using std::sqrt;
00252 using std::sin;
00253 using std::cos;
00254 SizeMismatch<3,S>::test(3, w.size());
00255
00256 static const Precision one_6th = 1.0/6.0;
00257 static const Precision one_20th = 1.0/20.0;
00258
00259 SO3<Precision> result;
00260
00261 const Precision theta_sq = w*w;
00262 const Precision theta = sqrt(theta_sq);
00263 Precision A, B;
00264
00265
00266 if (theta_sq < 1e-8) {
00267 A = 1.0 - one_6th * theta_sq;
00268 B = 0.5;
00269 } else {
00270 if (theta_sq < 1e-6) {
00271 B = 0.5 - 0.25 * one_6th * theta_sq;
00272 A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
00273 } else {
00274 const Precision inv_theta = 1.0/theta;
00275 A = sin(theta) * inv_theta;
00276 B = (1 - cos(theta)) * (inv_theta * inv_theta);
00277 }
00278 }
00279 rodrigues_so3_exp(w, A, B, result.my_matrix);
00280 return result;
00281 }
00282
00283 template <typename Precision>
00284 inline Vector<3, Precision> SO3<Precision>::ln() const{
00285 Vector<3, Precision> result;
00286
00287 const Precision cos_angle = (my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2] - 1.0) * 0.5;
00288 result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
00289 result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
00290 result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
00291
00292 Precision sin_angle_abs = sqrt(result*result);
00293 if (cos_angle > M_SQRT1_2) {
00294 if(sin_angle_abs > 0){
00295 result *= asin(sin_angle_abs) / sin_angle_abs;
00296 }
00297 } else if( cos_angle > -M_SQRT1_2) {
00298 const Precision angle = acos(cos_angle);
00299 result *= angle / sin_angle_abs;
00300 } else {
00301
00302 const Precision angle = M_PI - asin(sin_angle_abs);
00303 const Precision d0 = my_matrix[0][0] - cos_angle,
00304 d1 = my_matrix[1][1] - cos_angle,
00305 d2 = my_matrix[2][2] - cos_angle;
00306 TooN::Vector<3, Precision> r2;
00307 if(d0*d0 > d1*d1 && d0*d0 > d2*d2){
00308 r2[0] = d0;
00309 r2[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
00310 r2[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
00311 } else if(d1*d1 > d2*d2) {
00312 r2[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
00313 r2[1] = d1;
00314 r2[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
00315 } else {
00316 r2[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
00317 r2[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
00318 r2[2] = d2;
00319 }
00320
00321 if(r2 * result < 0)
00322 r2 *= -1;
00323 r2 = unit(r2);
00324 result = TooN::operator*(angle,r2);
00325 }
00326 return result;
00327 }
00328
00329
00330
00331 template<int S, typename P, typename PV, typename A> inline
00332 Vector<3, typename Internal::MultiplyType<P, PV>::type> operator*(const SO3<P>& lhs, const Vector<S, PV, A>& rhs){
00333 return lhs.get_matrix() * rhs;
00334 }
00335
00336
00337
00338 template<int S, typename P, typename PV, typename A> inline
00339 Vector<3, typename Internal::MultiplyType<PV, P>::type> operator*(const Vector<S, PV, A>& lhs, const SO3<P>& rhs){
00340 return lhs * rhs.get_matrix();
00341 }
00342
00343
00344
00345 template<int R, int C, typename P, typename PM, typename A> inline
00346 Matrix<3, C, typename Internal::MultiplyType<P, PM>::type> operator*(const SO3<P>& lhs, const Matrix<R, C, PM, A>& rhs){
00347 return lhs.get_matrix() * rhs;
00348 }
00349
00350
00351
00352 template<int R, int C, typename P, typename PM, typename A> inline
00353 Matrix<R, 3, typename Internal::MultiplyType<PM, P>::type> operator*(const Matrix<R, C, PM, A>& lhs, const SO3<P>& rhs){
00354 return lhs * rhs.get_matrix();
00355 }
00356
00357 #if 0 // will be moved to another header file ?
00358
00359 template <class A> inline
00360 Vector<3> transform(const SO3& pose, const FixedVector<3,A>& x) { return pose*x; }
00361
00362 template <class A1, class A2> inline
00363 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
00364 {
00365 J_x = pose.get_matrix();
00366 return pose * x;
00367 }
00368
00369 template <class A1, class A2, class A3> inline
00370 Vector<3> transform(const SO3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,3,A3>& J_pose)
00371 {
00372 J_x = pose.get_matrix();
00373 const Vector<3> cx = pose * x;
00374 J_pose[0][0] = J_pose[1][1] = J_pose[2][2] = 0;
00375 J_pose[1][0] = -(J_pose[0][1] = cx[2]);
00376 J_pose[0][2] = -(J_pose[2][0] = cx[1]);
00377 J_pose[2][1] = -(J_pose[1][2] = cx[0]);
00378 return cx;
00379 }
00380
00381 template <class A1, class A2, class A3> inline
00382 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)
00383 {
00384 const double z_inv = 1.0/in_frame[2];
00385 const double x_z_inv = in_frame[0]*z_inv;
00386 const double y_z_inv = in_frame[1]*z_inv;
00387 const double cross = x_z_inv * y_z_inv;
00388 J_pose[0][0] = -cross;
00389 J_pose[0][1] = 1 + x_z_inv*x_z_inv;
00390 J_pose[0][2] = -y_z_inv;
00391 J_pose[1][0] = -1 - y_z_inv*y_z_inv;
00392 J_pose[1][1] = cross;
00393 J_pose[1][2] = x_z_inv;
00394
00395 const TooN::Matrix<3>& R = pose.get_matrix();
00396 J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00397 J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00398 J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00399 J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00400 J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00401 J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00402
00403 return makeVector(x_z_inv, y_z_inv);
00404 }
00405
00406
00407 template <class A1> inline
00408 Vector<2> transform_and_project(const SO3& pose, const FixedVector<3,A1>& x)
00409 {
00410 return project(transform(pose,x));
00411 }
00412
00413 template <class A1, class A2, class A3> inline
00414 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)
00415 {
00416 return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
00417 }
00418
00419 #endif
00420
00421 }
00422
00423 #endif