examples/calibrate3d/TooN/se3.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 __SE3_H
00021 #define __SE3_H
00022 
00023 #include <TooN/so3.h>
00024 
00025 #ifndef TOON_NO_NAMESPACE
00026 namespace TooN {
00027 #endif
00028 
00029 class SE3 {
00030   friend SE3 operator*(const SO3& lhs, const SE3& rhs);
00031   friend std::istream& operator>> (std::istream& is, SE3& rhs);
00032   
00033  public:
00034   inline SE3();
00035   template <class A> inline SE3(const SO3& R, const FixedVector<3,A>& T) : my_rotation(R), my_translation(T) {}
00036       
00037 
00038   inline SO3& get_rotation(){return my_rotation;}
00039   inline const SO3& get_rotation() const {return my_rotation;}
00040   inline Vector<3>& get_translation() {return my_translation;}
00041   inline const Vector<3>& get_translation() const {return my_translation;}
00042 
00043   static inline SE3 exp(const Vector<6>& vect);
00044   static inline Vector<6> ln(const SE3& se3);
00045   inline Vector<6> ln() const { return SE3::ln(*this); }
00046 
00047   inline SE3 inverse() const;
00048 
00049   inline SE3& operator *=(const SE3& rhs);
00050   inline SE3 operator *(const SE3& rhs) const { return SE3(my_rotation*rhs.my_rotation, my_translation + my_rotation*rhs.my_translation); }
00051   inline SE3& left_multiply_by(const SE3& left);
00052 
00053   static inline Vector<4> generator_field(int i, Vector<4> pos);
00054 
00055 
00056   template<class Accessor>
00057   inline void adjoint(FixedVector<6,Accessor>& vect)const;
00058 
00059   template<class Accessor>
00060   inline void trinvadjoint(FixedVector<6,Accessor>& vect)const;
00061 
00062   template <class Accessor>
00063   inline void adjoint(FixedMatrix<6,6,Accessor>& M)const;
00064 
00065   template <class Accessor>
00066   inline void trinvadjoint(FixedMatrix<6,6,Accessor>& M)const;
00067 
00068 
00069     template <class A1, class A2, class A3> 
00070     Vector<2> project_transformed_point(const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const 
00071     {
00072         const double z_inv = 1.0/in_frame[2];
00073         const double x_z_inv = in_frame[0]*z_inv;
00074         const double y_z_inv = in_frame[1]*z_inv;
00075         const double cross = x_z_inv * y_z_inv;
00076         J_pose[0][0] = J_pose[1][1] = z_inv;
00077         J_pose[0][1] = J_pose[1][0] = 0;
00078         J_pose[0][2] = -x_z_inv * z_inv;
00079         J_pose[1][2] = -y_z_inv * z_inv;
00080         J_pose[0][3] = -cross;
00081         J_pose[0][4] = 1 + x_z_inv*x_z_inv; 
00082         J_pose[0][5] = -y_z_inv;  
00083         J_pose[1][3] = -1 - y_z_inv*y_z_inv;
00084         J_pose[1][4] =  cross;
00085         J_pose[1][5] =  x_z_inv;    
00086         
00087         const TooN::Matrix<3>& R = get_rotation().get_matrix();
00088         J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00089         J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00090         J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00091         J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00092         J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00093         J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00094         
00095         Vector<2> uv;
00096         uv[0] = x_z_inv;
00097         uv[1] = y_z_inv;
00098         return uv;
00099     }
00100 
00101     template <class A> Vector<3> 
00102     transform(const FixedVector<3,A>& x) const { return my_rotation * x + my_translation; }
00103     
00104     template <class A1, class A2, class A3> 
00105     Vector<3> transform(const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose) const {      
00106         const Vector<3> se3_x = *this * x;
00107         J_x = my_rotation.get_matrix();
00108         Identity(J_pose.template slice<0,0,3,3>());
00109         J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
00110         J_pose[0][4] =  se3_x[2]; J_pose[0][5] = -se3_x[1];
00111         J_pose[1][3] = -se3_x[2]; J_pose[1][5] =  se3_x[0];
00112         J_pose[2][3] =  se3_x[1]; J_pose[2][4] = -se3_x[0];
00113         return se3_x;
00114     }
00115 
00116 
00117     template <class A> 
00118     Vector<3> transform_uvq(const FixedVector<3,A>& uvq) const {
00119         const Matrix<3>& R = my_rotation.get_matrix();  
00120         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * my_translation;
00121         const double inv_z = 1.0/ DqT[2];
00122         const double vals[3] = {DqT[0] * inv_z, DqT[1]*inv_z, uvq[2]*inv_z};
00123         return Vector<3>(vals);
00124     }
00125 
00126     
00127     template <class A1, class A2, class A3> 
00128     Vector<2> transform_and_project(const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const 
00129     {
00130         return project_transformed_point(*this * x, J_x, J_pose);
00131     }
00132     
00133     template <class A1, class A2, class A3> 
00134     Vector<2> transform_and_project_uvq(const FixedVector<3,A1>& uvq, FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose) const
00135     {
00136         const Vector<3> DqT = get_rotation() * TooN::unproject(uvq.template slice<0,2>()) + uvq[2] * get_translation();
00137         const Vector<2> uv = project_transformed_point(DqT, J_uvq, J_pose);
00138         J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * get_translation();
00139         J_pose.template slice<0,0,2,3>() *= uvq[2];
00140         return uv;
00141     }
00142 
00143 private:
00144   SO3 my_rotation;
00145   Vector<3> my_translation;
00146 };
00147 
00148 
00149 // left multiply an SE3 by an SO3 
00150 inline SE3 operator*(const SO3& lhs, const SE3& rhs);
00151 
00152 
00153 // transfers a vector in the Lie algebra
00154 // from one coord frame to another
00155 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
00156 template<class Accessor>
00157 inline void SE3::adjoint(FixedVector<6,Accessor>& vect)const{
00158     vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
00159     vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
00160     vect.template slice<0,3>() += my_translation ^ vect.template slice<3,3>();
00161 }
00162 
00163 // tansfers covectors between frames
00164 // (using the transpose of the inverse of the adjoint)
00165 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
00166 template<class Accessor>
00167 inline void SE3::trinvadjoint(FixedVector<6,Accessor>& vect)const{
00168   vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
00169   vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
00170   vect.template slice<3,3>() += my_translation ^ vect.template slice<0,3>();
00171 }
00172 
00173 template <class Accessor>
00174 inline void SE3::adjoint(FixedMatrix<6,6,Accessor>& M)const{
00175   for(int i=0; i<6; i++){
00176     adjoint(M.T()[i]);
00177   }
00178   for(int i=0; i<6; i++){
00179     adjoint(M[i]);
00180   }
00181 }
00182   
00183 template <class Accessor>
00184 inline void SE3::trinvadjoint(FixedMatrix<6,6,Accessor>& M)const{
00185   for(int i=0; i<6; i++){
00186     trinvadjoint(M.T()[i]);
00187   }
00188   for(int i=0; i<6; i++){
00189     trinvadjoint(M[i]);
00190   }
00191 }
00192 
00193 
00194 // operator ostream& <<
00195 inline std::ostream& operator <<(std::ostream& os, const SE3& rhs){
00196   for(int i=0; i<3; i++){
00197     os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] << std::endl;
00198   }
00199   return os;
00200 }
00201 
00202 // operator istream& >>
00203 inline std::istream& operator>>(std::istream& is, SE3& rhs){
00204   for(int i=0; i<3; i++){
00205     is >> rhs.get_rotation().my_matrix[i] >> rhs.get_translation()[i];
00206   }
00207   return is;
00208 }
00209 
00210 
00212 // operator *   //
00213 // SE3 * Vector //
00215 
00216 template<class VectorType>
00217 struct SE3VMult {
00218   inline static void eval(Vector<4>& ret, const SE3& lhs, const VectorType& rhs){
00219     ret.template slice<0,3>()=lhs.get_rotation()*rhs.template slice<0,3>();
00220     ret.template slice<0,3>()+=lhs.get_translation() * rhs[3];
00221     ret[3] = rhs[3];
00222   }
00223 };
00224 
00225 template<class Accessor> inline
00226 Vector<4> operator*(const SE3& lhs, const FixedVector<4,Accessor>& rhs){
00227   return Vector<4>(lhs,rhs,Operator<SE3VMult<FixedVector<4, Accessor> > >());
00228 }
00229 
00230 template<class Accessor> inline
00231 Vector<4> operator*(const SE3& lhs, const DynamicVector<Accessor>& rhs){
00232         //FIXME: size checking
00233   return Vector<4>(lhs,rhs,Operator<SE3VMult<DynamicVector<Accessor> > >());
00234 }
00235 
00236 template <class Accessor> inline
00237 Vector<3> operator*(const SE3& lhs, const FixedVector<3,Accessor>& rhs){
00238     return lhs.transform(rhs);
00239 }
00240 
00241 
00243 // operator *   //
00244 // Vector * SE3 //
00246 
00247 template<class Accessor>
00248 struct VSE3Mult {
00249   inline static void eval(Vector<4>& ret, const FixedVector<4,Accessor>& lhs, const SE3& rhs){
00250     ret.template slice<0,3>() = lhs.template slice<0,3>() * rhs.get_rotation();
00251     ret[3] = lhs[3];
00252     ret[3] += lhs.template slice<0,3>() * rhs.get_translation();
00253   }
00254 };
00255 
00256 template<class Accessor> inline
00257 Vector<4> operator*(const FixedVector<4,Accessor>& lhs, const SE3& rhs){
00258   return Vector<4>(lhs,rhs,Operator<VSE3Mult<Accessor> >());
00259 }
00260 
00261 
00262 
00264 // operator *   //
00265 // SE3 * Matrix //
00267 
00268 template <int RHS, class Accessor>
00269 struct SE3MMult {
00270   inline static void eval(Matrix<4,RHS>& ret, const SE3& lhs, const FixedMatrix<4,RHS,Accessor>& rhs){
00271     for(int i=0; i<RHS; i++){
00272       ret.T()[i].template slice<0,3>() = lhs.get_rotation() * rhs.T()[i].template slice<0,3>();
00273       ret.T()[i].template slice<0,3>() += lhs.get_translation() * rhs(3,i);
00274       ret(3,i) = rhs(3,i);
00275     }
00276   }
00277 };
00278 
00279 
00280 template <int RHS, class Accessor> inline 
00281 Matrix<4,RHS> operator*(const SE3& lhs, const FixedMatrix<4,RHS,Accessor>& rhs){
00282   return Matrix<4,RHS>(lhs,rhs,Operator<SE3MMult<RHS,Accessor> >());
00283 }
00284 
00285 
00287 // operator *   //
00288 // Matrix * SE3 //
00290 
00291 template <int LHS, class Accessor> 
00292 struct MSE3Mult {
00293   inline static void eval(Matrix<LHS,4>& ret, const FixedMatrix<LHS,4,Accessor>& lhs, const SE3& rhs){
00294     for(int i=0; i<LHS; i++){
00295       ret[i].template slice<0,3>() = lhs[i].template slice<0,3>() * rhs.get_rotation();
00296       ret(i,3) = rhs.get_translation() * lhs[i].template slice<0,3>();
00297       ret(i,3) += lhs(i,3);
00298     }
00299   }
00300 };
00301 
00302 
00303 template <int LHS, class Accessor> inline 
00304 Matrix<LHS,4> operator*(const FixedMatrix<LHS,4,Accessor>& lhs, const SE3& rhs){
00305   return Matrix<LHS,4>(lhs,rhs,Operator<MSE3Mult<LHS,Accessor> >());
00306 }
00307 
00308 
00309 namespace SE3static 
00310 {
00311   static double zero[3]={0,0,0};
00312 }
00313 
00314 inline SE3::SE3() :
00315   my_translation(SE3static::zero)
00316 {}
00317 
00318 
00319 inline SE3 SE3::exp(const Vector<6>& mu){
00320     static const double one_6th = 1.0/6.0;
00321     static const double one_20th = 1.0/20.0;
00322 
00323     SE3 result;
00324 
00325     const Vector<3> w = mu.slice<3,3>();
00326     const double theta_sq = w*w;
00327     const double theta = sqrt(theta_sq);
00328     double A, B;
00329     
00330     const Vector<3> cross = w ^ mu.slice<0,3>();
00331     if (theta_sq < 1e-8) {
00332         A = 1.0 - one_6th * theta_sq;
00333         B = 0.5;
00334         result.get_translation() = mu.slice<0,3>() + 0.5 * cross;
00335     } else {
00336         double C;
00337         if (theta_sq < 1e-6) {
00338             C = one_6th*(1.0 - one_20th * theta_sq);
00339             A = 1.0 - theta_sq * C;
00340             B = 0.5 - 0.25 * one_6th * theta_sq;
00341         } else {
00342             const double inv_theta = 1.0/theta;
00343             A = sin(theta) * inv_theta;
00344             B = (1 - cos(theta)) * (inv_theta * inv_theta);
00345             C = (1 - A) * (inv_theta * inv_theta);
00346         }
00347         result.get_translation() = mu.slice<0,3>() + B * cross + C * (w ^ cross);
00348     }
00349     rodrigues_so3_exp(w, A, B, result.my_rotation.my_matrix);
00350     return result;
00351 }
00352 
00353 
00354  inline Vector<6> SE3::ln(const SE3& se3) {
00355     Vector<3> rot = se3.my_rotation.ln();
00356     double theta = sqrt(rot*rot);
00357     double shtot = 0.5;
00358     
00359     if(theta > 0.00001) {
00360         shtot = sin(theta/2)/theta;
00361     }
00362     
00363     // now do the rotation
00364     Vector<3> halfrot = rot * -0.5;
00365     SO3 halfrotator = SO3::exp(halfrot);
00366     
00367     Vector<3> rottrans = halfrotator * se3.my_translation;
00368     
00369     if(theta > 0.001){
00370         rottrans -= rot * ((se3.my_translation * rot) * (1-2*shtot) / (rot*rot));
00371     } else {
00372         rottrans -= rot * ((se3.my_translation * rot)/24);
00373     }
00374     
00375     rottrans /= (2 * shtot);
00376     
00377     Vector<6> result;
00378     result.slice<0,3>()=rottrans;
00379     result.slice<3,3>()=rot;
00380     return result;
00381 }
00382 
00383 inline SE3 SE3::inverse() const {
00384     const SO3& rinv = my_rotation.inverse();
00385     return SE3(rinv, -(rinv*my_translation));
00386 }
00387 
00388 inline SE3& SE3::left_multiply_by(const SE3& left) {
00389     my_translation = left.my_translation + left.get_rotation() * my_translation;
00390     my_rotation = left.my_rotation * my_rotation;
00391     return *this;
00392 }
00393 
00394 inline Vector<4> SE3::generator_field(int i, Vector<4> pos){
00395   double result_d[]={0,0,0,0};
00396   Vector<4> result(result_d);
00397   if(i < 3){
00398     result[i]=pos[3];
00399     return result;
00400   }
00401   result[(i+1)%3] = - pos[(i+2)%3];
00402   result[(i+2)%3] = pos[(i+1)%3];
00403   return result;
00404 }
00405 
00406 
00407 inline SE3 operator*(const SO3& lhs, const SE3& rhs){
00408   SE3 result;
00409   result.my_rotation = lhs*rhs.my_rotation;
00410   result.my_translation = lhs*rhs.my_translation;
00411   return result;
00412 }
00413 
00414 #ifndef TOON_NO_NAMESPACE
00415 }
00416 #endif
00417 
00418 #endif

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