se3.h

00001 // -*- c++ -*-
00002 
00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
00004 //
00005 // This file is part of the TooN Library.  This library is free
00006 // software; you can redistribute it and/or modify it under the
00007 // terms of the GNU General Public License as published by the
00008 // Free Software Foundation; either version 2, or (at your option)
00009 // any later version.
00010 
00011 // This library is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 
00016 // You should have received a copy of the GNU General Public License along
00017 // with this library; see the file COPYING.  If not, write to the Free
00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
00019 // USA.
00020 
00021 // As a special exception, you may use this file as part of a free software
00022 // library without restriction.  Specifically, if other files instantiate
00023 // templates or use macros or inline functions from this file, or you compile
00024 // this file and link it with other files to produce an executable, this
00025 // file does not by itself cause the resulting executable to be covered by
00026 // the GNU General Public License.  This exception does not however
00027 // invalidate any other reasons why the executable file might be covered by
00028 // the GNU General Public License.
00029 
00030 #ifndef TOON_INCLUDE_SE3_H
00031 #define TOON_INCLUDE_SE3_H
00032 
00033 #include <TooN/so3.h>
00034 
00035 namespace TooN {
00036 
00037 
00049 template <typename Precision = double>
00050 class SE3 {
00051 public:
00053     inline SE3() : my_translation(Zeros) {}
00054 
00055     template <int S, typename P, typename A> 
00056     SE3(const SO3<Precision> & R, const Vector<S, P, A>& T) : my_rotation(R), my_translation(T) {}
00057     template <int S, typename P, typename A>
00058     SE3(const Vector<S, P, A> & v) { *this = exp(v); }
00059 
00061     inline SO3<Precision>& get_rotation(){return my_rotation;}
00063     inline const SO3<Precision>& get_rotation() const {return my_rotation;}
00064 
00066     inline Vector<3, Precision>& get_translation() {return my_translation;}
00068     inline const Vector<3, Precision>& get_translation() const {return my_translation;}
00069 
00073     template <int S, typename P, typename A>
00074     static inline SE3 exp(const Vector<S, P, A>& vect);
00075 
00076 
00079     static inline Vector<6, Precision> ln(const SE3& se3);
00081     inline Vector<6, Precision> ln() const { return SE3::ln(*this); }
00082 
00083     inline SE3 inverse() const {
00084         const SO3<Precision> rinv = get_rotation().inverse();
00085         return SE3(rinv, -(rinv*my_translation));
00086     }
00087 
00090     inline SE3& operator *=(const SE3& rhs) {
00091         get_translation() += get_rotation() * rhs.get_translation();
00092         get_rotation() *= rhs.get_rotation();
00093         return *this;
00094     }
00095 
00098     inline SE3 operator *(const SE3& rhs) const { return SE3(get_rotation()*rhs.get_rotation(), get_translation() + get_rotation()*rhs.get_translation()); }
00099 
00100     inline SE3& left_multiply_by(const SE3& left) {
00101         get_translation() = left.get_translation() + left.get_rotation() * get_translation();
00102         get_rotation() = left.get_rotation() * get_rotation();
00103         return *this;
00104     }
00105 
00106     static inline Matrix<4,4,Precision> generator(int i){
00107         Matrix<4,4,Precision> result(Zeros);
00108         if(i < 3){
00109             result[i][3]=1;
00110             return result;
00111         }
00112         result[(i+1)%3][(i+2)%3] = -1;
00113         result[(i+2)%3][(i+1)%3] = 1;
00114         return result;
00115     }
00116 
00118   template<typename Base>
00119   inline static Vector<4,Precision> generator_field(int i, const Vector<4, Precision, Base>& pos)
00120   {
00121     Vector<4, Precision> result(Zeros);
00122     if(i < 3){
00123       result[i]=pos[3];
00124       return result;
00125     }
00126     result[(i+1)%3] = - pos[(i+2)%3];
00127     result[(i+2)%3] = pos[(i+1)%3];
00128     return result;
00129   }
00130   
00136     template<int S, typename Accessor>
00137     inline Vector<6, Precision> adjoint(const Vector<S,Precision, Accessor>& vect)const;
00138 
00141     template<int S, typename Accessor>
00142     inline Vector<6, Precision> trinvadjoint(const Vector<S,Precision,Accessor>& vect)const;
00143     
00145     template <int R, int C, typename Accessor>
00146     inline Matrix<6,6,Precision> adjoint(const Matrix<R,C,Precision,Accessor>& M)const;
00147 
00149     template <int R, int C, typename Accessor>
00150     inline Matrix<6,6,Precision> trinvadjoint(const Matrix<R,C,Precision,Accessor>& M)const;
00151 
00152 private:
00153     SO3<Precision> my_rotation;
00154     Vector<3, Precision> my_translation;
00155 };
00156 
00157 // transfers a vector in the Lie algebra
00158 // from one coord frame to another
00159 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
00160 template<typename Precision>
00161 template<int S, typename Accessor>
00162 inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,Precision, Accessor>& vect) const{
00163     SizeMismatch<6,S>::test(6, vect.size());
00164     Vector<6, Precision> result;
00165     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00166     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00167     result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
00168     return result;
00169 }
00170 
00171 // tansfers covectors between frames
00172 // (using the transpose of the inverse of the adjoint)
00173 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
00174 template<typename Precision>
00175 template<int S, typename Accessor>
00176 inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const Vector<S,Precision, Accessor>& vect) const{
00177     SizeMismatch<6,S>::test(6, vect.size());
00178     Vector<6, Precision> result;
00179     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00180     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00181     result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
00182     return result;
00183 }
00184 
00185 template<typename Precision>
00186 template<int R, int C, typename Accessor>
00187 inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const Matrix<R,C,Precision,Accessor>& M)const{
00188     SizeMismatch<6,R>::test(6, M.num_cols());
00189     SizeMismatch<6,C>::test(6, M.num_rows());
00190 
00191     Matrix<6,6,Precision> result;
00192     for(int i=0; i<6; i++){
00193         result.T()[i] = adjoint(M.T()[i]);
00194     }
00195     for(int i=0; i<6; i++){
00196         result[i] = adjoint(result[i]);
00197     }
00198     return result;
00199 }
00200 
00201 template<typename Precision>
00202 template<int R, int C, typename Accessor>
00203 inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const Matrix<R,C,Precision,Accessor>& M)const{
00204     SizeMismatch<6,R>::test(6, M.num_cols());
00205     SizeMismatch<6,C>::test(6, M.num_rows());
00206 
00207     Matrix<6,6,Precision> result;
00208     for(int i=0; i<6; i++){
00209         result.T()[i] = trinvadjoint(M.T()[i]);
00210     }
00211     for(int i=0; i<6; i++){
00212         result[i] = trinvadjoint(result[i]);
00213     }
00214     return result;
00215 }
00216 
00219 template <typename Precision>
00220 inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
00221     for(int i=0; i<3; i++){
00222         os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] << std::endl;
00223     }
00224     return os;
00225 }
00226 
00227 
00230 template <typename Precision>
00231 inline std::istream& operator>>(std::istream& is, SE3<Precision>& rhs){
00232     for(int i=0; i<3; i++){
00233         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
00234     }
00235     rhs.get_rotation().coerce();
00236     return is;
00237 }
00238 
00240 // operator *   //
00241 // SE3 * Vector //
00243 
00244 namespace Internal {
00245 template<int S, typename PV, typename A, typename P>
00246 struct SE3VMult;
00247 }
00248 
00249 template<int S, typename PV, typename A, typename P>
00250 struct Operator<Internal::SE3VMult<S,PV,A,P> > {
00251     const SE3<P> & lhs;
00252     const Vector<S,PV,A> & rhs;
00253     
00254     Operator(const SE3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
00255     
00256     template <int S0, typename P0, typename A0>
00257     void eval(Vector<S0, P0, A0> & res ) const {
00258         SizeMismatch<4,S>::test(4, rhs.size());
00259         res.template slice<0,3>()=lhs.get_rotation() * rhs.template slice<0,3>();
00260         res.template slice<0,3>()+=lhs.get_translation() * rhs[3];
00261         res[3] = rhs[3];
00262     }
00263     int size() const { return 4; }
00264 };
00265 
00268 template<int S, typename PV, typename A, typename P> inline
00269 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P> & lhs, const Vector<S,PV,A>& rhs){
00270     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE3VMult<S,PV,A,P> >(lhs,rhs));
00271 }
00272 
00275 template <typename PV, typename A, typename P> inline
00276 Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>& lhs, const Vector<3,PV,A>& rhs){
00277     return lhs.get_translation() + lhs.get_rotation() * rhs;
00278 }
00279 
00281 // operator *   //
00282 // Vector * SE3 //
00284 
00285 namespace Internal {
00286 template<int S, typename PV, typename A, typename P>
00287 struct VSE3Mult;
00288 }
00289 
00290 template<int S, typename PV, typename A, typename P>
00291 struct Operator<Internal::VSE3Mult<S,PV,A,P> > {
00292     const Vector<S,PV,A> & lhs;
00293     const SE3<P> & rhs;
00294     
00295     Operator( const Vector<S,PV,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00296     
00297     template <int S0, typename P0, typename A0>
00298     void eval(Vector<S0, P0, A0> & res ) const {
00299         SizeMismatch<4,S>::test(4, lhs.size());
00300         res.template slice<0,3>()=lhs.template slice<0,3>() * rhs.get_rotation();
00301         res[3] = lhs[3];
00302         res[3] += lhs.template slice<0,3>() * rhs.get_translation();
00303     }
00304     int size() const { return 4; }
00305 };
00306 
00309 template<int S, typename PV, typename A, typename P> inline
00310 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const Vector<S,PV,A>& lhs, const SE3<P> & rhs){
00311     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSE3Mult<S,PV,A,P> >(lhs,rhs));
00312 }
00313 
00315 // operator *   //
00316 // SE3 * Matrix //
00318 
00319 namespace Internal {
00320 template <int R, int C, typename PM, typename A, typename P>
00321 struct SE3MMult;
00322 }
00323 
00324 template<int R, int Cols, typename PM, typename A, typename P>
00325 struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
00326     const SE3<P> & lhs;
00327     const Matrix<R,Cols,PM,A> & rhs;
00328     
00329     Operator(const SE3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
00330     
00331     template <int R0, int C0, typename P0, typename A0>
00332     void eval(Matrix<R0, C0, P0, A0> & res ) const {
00333         SizeMismatch<4,R>::test(4, rhs.num_rows());
00334         for(int i=0; i<rhs.num_cols(); ++i)
00335             res.T()[i] = lhs * rhs.T()[i];
00336     }
00337     int num_cols() const { return rhs.num_cols(); }
00338     int num_rows() const { return 4; }
00339 };
00340 
00343 template <int R, int Cols, typename PM, typename A, typename P> inline 
00344 Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
00345     return Matrix<4,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE3MMult<R, Cols, PM, A, P> >(lhs,rhs));
00346 }
00347 
00349 // operator *   //
00350 // Matrix * SE3 //
00352 
00353 namespace Internal {
00354 template <int Rows, int C, typename PM, typename A, typename P>
00355 struct MSE3Mult;
00356 }
00357 
00358 template<int Rows, int C, typename PM, typename A, typename P>
00359 struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
00360     const Matrix<Rows,C,PM,A> & lhs;
00361     const SE3<P> & rhs;
00362     
00363     Operator( const Matrix<Rows,C,PM,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00364     
00365     template <int R0, int C0, typename P0, typename A0>
00366     void eval(Matrix<R0, C0, P0, A0> & res ) const {
00367         SizeMismatch<4, C>::test(4, lhs.num_cols());
00368         for(int i=0; i<lhs.num_rows(); ++i)
00369             res[i] = lhs[i] * rhs;
00370     }
00371     int num_cols() const { return 4; }
00372     int num_rows() const { return lhs.num_rows(); }
00373 };
00374 
00377 template <int Rows, int C, typename PM, typename A, typename P> inline 
00378 Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE3<P> & rhs ){
00379     return Matrix<Rows,4,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE3Mult<Rows, C, PM, A, P> >(lhs,rhs));
00380 }
00381 
00382 template <typename Precision>
00383 template <int S, typename P, typename VA>
00384 inline SE3<Precision> SE3<Precision>::exp(const Vector<S, P, VA>& mu){
00385     SizeMismatch<6,S>::test(6, mu.size());
00386     static const Precision one_6th = 1.0/6.0;
00387     static const Precision one_20th = 1.0/20.0;
00388     
00389     SE3<Precision> result;
00390     
00391     const Vector<3,Precision> w = mu.template slice<3,3>();
00392     const Precision theta_sq = w*w;
00393     const Precision theta = sqrt(theta_sq);
00394     double A, B;
00395     
00396     const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
00397     if (theta_sq < 1e-8) {
00398         A = 1.0 - one_6th * theta_sq;
00399         B = 0.5;
00400         result.get_translation() = mu.template slice<0,3>() + 0.5 * cross;
00401     } else {
00402         double C;
00403         if (theta_sq < 1e-6) {
00404             C = one_6th*(1.0 - one_20th * theta_sq);
00405             A = 1.0 - theta_sq * C;
00406             B = 0.5 - 0.25 * one_6th * theta_sq;
00407         } else {
00408             const double inv_theta = 1.0/theta;
00409             A = sin(theta) * inv_theta;
00410             B = (1 - cos(theta)) * (inv_theta * inv_theta);
00411             C = (1 - A) * (inv_theta * inv_theta);
00412         }
00413         result.get_translation() = mu.template slice<0,3>() + B * cross + C * (w ^ cross);
00414     }
00415     rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
00416     return result;
00417 }
00418 
00419 template <typename Precision>
00420 inline Vector<6, Precision> SE3<Precision>::ln(const SE3<Precision>& se3) {
00421     Vector<3,Precision> rot = se3.get_rotation().ln();
00422     const Precision theta = sqrt(rot*rot);
00423 
00424     Precision shtot = 0.5;  
00425     if(theta > 0.00001) {
00426         shtot = sin(theta/2)/theta;
00427     }
00428     
00429     // now do the rotation
00430     const SO3<Precision> halfrotator = SO3<Precision>::exp(rot * -0.5);
00431     Vector<3, Precision> rottrans = halfrotator * se3.get_translation();
00432     
00433     if(theta > 0.001){
00434         rottrans -= rot * ((se3.get_translation() * rot) * (1-2*shtot) / (rot*rot));
00435     } else {
00436         rottrans -= rot * ((se3.get_translation() * rot)/24);
00437     }
00438     
00439     rottrans /= (2 * shtot);
00440     
00441     Vector<6, Precision> result;
00442     result.template slice<0,3>()=rottrans;
00443     result.template slice<3,3>()=rot;
00444     return result;
00445 }
00446 
00447 template <typename Precision>
00448 inline SE3<Precision> operator*(const SO3<Precision>& lhs, const SE3<Precision>& rhs){
00449     return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
00450 }
00451 
00452 #if 0 // should be moved to another header file
00453 
00454     template <class A> inline
00455     Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return pose.get_rotation()*x + pose.get_translation(); }
00456     
00457     template <class A> inline
00458     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A>& uvq)
00459     {
00460     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00461     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
00462     const double inv_z = 1.0/ DqT[2];
00463     return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
00464     }
00465 
00466     template <class A1, class A2> inline
00467     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
00468     {
00469     J_x = pose.get_rotation().get_matrix();
00470     return pose * x;
00471     }
00472 
00473 
00474     template <class A1, class A2> inline
00475     Vector<3> inverse_depth_point_jacobian(const SE3& pose, const double q, const FixedVector<3,A1>& DqT, const double inv_z, FixedMatrix<3,3,A2>& J_uvq)
00476     {
00477     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00478     const Vector<3>& T = pose.get_translation();
00479     const double u1 = DqT[0] * inv_z;
00480     J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
00481     J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
00482     J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
00483     
00484     const double v1 = DqT[1] * inv_z;
00485     J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
00486     J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
00487     J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
00488     
00489     const double q1 = q * inv_z;
00490     J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
00491     J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
00492     J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
00493 
00494     return makeVector(u1, v1, q1);
00495     }
00496     
00497 
00498     template <class A1, class A2> inline
00499     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
00500     {
00501     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00502     const Vector<3>& T = pose.get_translation();
00503     const double q = uvq[2];
00504     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00505     const double inv_z = 1.0 / DqT[2];
00506 
00507     return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00508     }
00509 
00510     template <class A1, class A2, class A3> inline
00511     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
00512     {
00513     J_x = pose.get_rotation().get_matrix();
00514     Identity(J_pose.template slice<0,0,3,3>());
00515     const Vector<3> cx = pose * x;
00516     J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
00517     J_pose[1][3] = -(J_pose[0][4] = cx[2]);
00518     J_pose[0][5] = -(J_pose[2][3] = cx[1]);
00519     J_pose[2][4] = -(J_pose[1][5] = cx[0]);
00520     return cx;
00521     }
00522 
00523     template <class A1, class A2, class A3> inline
00524     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq, FixedMatrix<3,6,A3>& J_pose)
00525     {
00526     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00527     const Vector<3>& T = pose.get_translation();
00528     const double q = uvq[2];
00529     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00530     const double inv_z = 1.0 / DqT[2];
00531     
00532     const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00533     const double q1 = uvq1[2];
00534 
00535     J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
00536     J_pose[0][0] = J_pose[1][1] = q1;
00537     J_pose[0][2] = -uvq1[0] * q1;
00538     J_pose[1][2] = -uvq1[1] * q1;
00539     J_pose[2][2] = -q1 * q1;
00540     
00541     J_pose[0][3] = -uvq1[1]*uvq1[0];
00542     J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
00543     J_pose[0][5] = -uvq1[1];
00544     
00545     J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
00546     J_pose[1][4] = uvq1[0] * uvq1[1];
00547     J_pose[1][5] = uvq1[0];
00548 
00549     J_pose[2][3] = -uvq1[1]*q1;
00550     J_pose[2][4] = uvq1[0]*q1;
00551     J_pose[2][5] = 0;
00552 
00553     return uvq1;
00554     }
00555 
00556     template <class A1, class A2, class A3> inline
00557     Vector<2> project_transformed_point(const SE3& pose, const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose)
00558     {
00559     const double z_inv = 1.0/in_frame[2];
00560     const double x_z_inv = in_frame[0]*z_inv;
00561     const double y_z_inv = in_frame[1]*z_inv;
00562     const double cross = x_z_inv * y_z_inv;
00563     J_pose[0][0] = J_pose[1][1] = z_inv;
00564     J_pose[0][1] = J_pose[1][0] = 0;
00565     J_pose[0][2] = -x_z_inv * z_inv;
00566     J_pose[1][2] = -y_z_inv * z_inv;
00567     J_pose[0][3] = -cross;
00568     J_pose[0][4] = 1 + x_z_inv*x_z_inv; 
00569     J_pose[0][5] = -y_z_inv;  
00570     J_pose[1][3] = -1 - y_z_inv*y_z_inv;
00571     J_pose[1][4] =  cross;
00572     J_pose[1][5] =  x_z_inv;    
00573     
00574     const TooN::Matrix<3>& R = pose.get_rotation().get_matrix();
00575     J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00576     J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00577     J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00578     J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00579     J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00580     J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00581     
00582     return makeVector(x_z_inv, y_z_inv);
00583     }
00584 
00585 
00586     template <class A1> inline
00587     Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x)
00588     {
00589     return project(transform(pose,x));
00590     }
00591 
00592     template <class A1> inline
00593     Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq)
00594     {
00595     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00596     const Vector<3>& T = pose.get_translation();
00597     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * T;
00598     return project(DqT);
00599     }
00600 
00601     template <class A1, class A2, class A3> inline
00602     Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose)
00603     {
00604     return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
00605     }
00606 
00607     template <class A1, class A2, class A3> inline
00608     Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose)
00609     {
00610     const Matrix<3>& R = pose.get_rotation().get_matrix();  
00611     const Vector<3>& T = pose.get_translation();
00612     const double q = uvq[2];
00613     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00614     const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, J_pose);
00615     J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * pose.get_translation();
00616     J_pose.template slice<0,0,2,3>() *= q;
00617     return uv;
00618     }
00619 
00620 #endif
00621 
00622 }
00623 
00624 #endif

Generated on Thu May 7 20:28:41 2009 for TooN by  doxygen 1.5.3