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