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
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) {
00258 if(sin_angle_abs > 0){
00259 result *= asin(sin_angle_abs) / sin_angle_abs;
00260 }
00261 } else if( cos_angle > -M_SQRT1_2) {
00262 double angle = acos(cos_angle);
00263 result *= angle / sin_angle_abs;
00264 } else {
00265
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)){
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)) {
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 {
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
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