libtoon: se3.h Source File

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 
00060         template <class IP, int S, typename P, typename A> 
00061         SE3(const Operator<Internal::Identity<IP> >&, const Vector<S, P, A>& T) : my_translation(T) {}
00062 
00064         inline SO3<Precision>& get_rotation(){return my_rotation;}
00066         inline const SO3<Precision>& get_rotation() const {return my_rotation;}
00067 
00069         inline Vector<3, Precision>& get_translation() {return my_translation;}
00071         inline const Vector<3, Precision>& get_translation() const {return my_translation;}
00072 
00076         template <int S, typename P, typename A>
00077         static inline SE3 exp(const Vector<S, P, A>& vect);
00078 
00079 
00082         static inline Vector<6, Precision> ln(const SE3& se3);
00084         inline Vector<6, Precision> ln() const { return SE3::ln(*this); }
00085 
00086         inline SE3 inverse() const {
00087                 const SO3<Precision> rinv = get_rotation().inverse();
00088                 return SE3(rinv, -(rinv*my_translation));
00089         }
00090 
00093         inline SE3& operator *=(const SE3& rhs) {
00094                 get_translation() += get_rotation() * rhs.get_translation();
00095                 get_rotation() *= rhs.get_rotation();
00096                 return *this;
00097         }
00098 
00101         inline SE3 operator *(const SE3& rhs) const { return SE3(get_rotation()*rhs.get_rotation(), get_translation() + get_rotation()*rhs.get_translation()); }
00102 
00103         inline SE3& left_multiply_by(const SE3& left) {
00104                 get_translation() = left.get_translation() + left.get_rotation() * get_translation();
00105                 get_rotation() = left.get_rotation() * get_rotation();
00106                 return *this;
00107         }
00108 
00109         static inline Matrix<4,4,Precision> generator(int i){
00110                 Matrix<4,4,Precision> result(Zeros);
00111                 if(i < 3){
00112                         result[i][3]=1;
00113                         return result;
00114                 }
00115                 result[(i+1)%3][(i+2)%3] = -1;
00116                 result[(i+2)%3][(i+1)%3] = 1;
00117                 return result;
00118         }
00119 
00121   template<typename Base>
00122   inline static Vector<4,Precision> generator_field(int i, const Vector<4, Precision, Base>& pos)
00123   {
00124     Vector<4, Precision> result(Zeros);
00125     if(i < 3){
00126       result[i]=pos[3];
00127       return result;
00128     }
00129     result[(i+1)%3] = - pos[(i+2)%3];
00130     result[(i+2)%3] = pos[(i+1)%3];
00131     return result;
00132   }
00133   
00139         template<int S, typename P2, typename Accessor>
00140         inline Vector<6, Precision> adjoint(const Vector<S,P2, Accessor>& vect)const;
00141 
00144         template<int S, typename P2, typename Accessor>
00145         inline Vector<6, Precision> trinvadjoint(const Vector<S,P2,Accessor>& vect)const;
00146         
00148         template <int R, int C, typename P2, typename Accessor>
00149         inline Matrix<6,6,Precision> adjoint(const Matrix<R,C,P2,Accessor>& M)const;
00150 
00152         template <int R, int C, typename P2, typename Accessor>
00153         inline Matrix<6,6,Precision> trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const;
00154 
00155 private:
00156         SO3<Precision> my_rotation;
00157         Vector<3, Precision> my_translation;
00158 };
00159 
00160 // transfers a vector in the Lie algebra
00161 // from one coord frame to another
00162 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
00163 template<typename Precision>
00164 template<int S, typename P2, typename Accessor>
00165 inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,P2, Accessor>& vect) const{
00166         SizeMismatch<6,S>::test(6, vect.size());
00167         Vector<6, Precision> result;
00168         result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00169         result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00170         result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
00171         return result;
00172 }
00173 
00174 // tansfers covectors between frames
00175 // (using the transpose of the inverse of the adjoint)
00176 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
00177 template<typename Precision>
00178 template<int S, typename P2, typename Accessor>
00179 inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const Vector<S,P2, Accessor>& vect) const{
00180         SizeMismatch<6,S>::test(6, vect.size());
00181         Vector<6, Precision> result;
00182         result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00183         result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00184         result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
00185         return result;
00186 }
00187 
00188 template<typename Precision>
00189 template<int R, int C, typename P2, typename Accessor>
00190 inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const Matrix<R,C,P2,Accessor>& M)const{
00191         SizeMismatch<6,R>::test(6, M.num_cols());
00192         SizeMismatch<6,C>::test(6, M.num_rows());
00193 
00194         Matrix<6,6,Precision> result;
00195         for(int i=0; i<6; i++){
00196                 result.T()[i] = adjoint(M.T()[i]);
00197         }
00198         for(int i=0; i<6; i++){
00199                 result[i] = adjoint(result[i]);
00200         }
00201         return result;
00202 }
00203 
00204 template<typename Precision>
00205 template<int R, int C, typename P2, typename Accessor>
00206 inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const{
00207         SizeMismatch<6,R>::test(6, M.num_cols());
00208         SizeMismatch<6,C>::test(6, M.num_rows());
00209 
00210         Matrix<6,6,Precision> result;
00211         for(int i=0; i<6; i++){
00212                 result.T()[i] = trinvadjoint(M.T()[i]);
00213         }
00214         for(int i=0; i<6; i++){
00215                 result[i] = trinvadjoint(result[i]);
00216         }
00217         return result;
00218 }
00219 
00222 template <typename Precision>
00223 inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
00224         for(int i=0; i<3; i++){
00225                 os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] << std::endl;
00226         }
00227         return os;
00228 }
00229 
00230 
00233 template <typename Precision>
00234 inline std::istream& operator>>(std::istream& is, SE3<Precision>& rhs){
00235         for(int i=0; i<3; i++){
00236                 is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
00237         }
00238         rhs.get_rotation().coerce();
00239         return is;
00240 }
00241 
00243 // operator *   //
00244 // SE3 * Vector //
00246 
00247 namespace Internal {
00248 template<int S, typename PV, typename A, typename P>
00249 struct SE3VMult;
00250 }
00251 
00252 template<int S, typename PV, typename A, typename P>
00253 struct Operator<Internal::SE3VMult<S,PV,A,P> > {
00254         const SE3<P> & lhs;
00255         const Vector<S,PV,A> & rhs;
00256         
00257         Operator(const SE3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
00258         
00259         template <int S0, typename P0, typename A0>
00260         void eval(Vector<S0, P0, A0> & res ) const {
00261                 SizeMismatch<4,S>::test(4, rhs.size());
00262                 res.template slice<0,3>()=lhs.get_rotation() * rhs.template slice<0,3>();
00263                 res.template slice<0,3>()+=lhs.get_translation() * rhs[3];
00264                 res[3] = rhs[3];
00265         }
00266         int size() const { return 4; }
00267 };
00268 
00271 template<int S, typename PV, typename A, typename P> inline
00272 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P> & lhs, const Vector<S,PV,A>& rhs){
00273         return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE3VMult<S,PV,A,P> >(lhs,rhs));
00274 }
00275 
00278 template <typename PV, typename A, typename P> inline
00279 Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>& lhs, const Vector<3,PV,A>& rhs){
00280         return lhs.get_translation() + lhs.get_rotation() * rhs;
00281 }
00282 
00284 // operator *   //
00285 // Vector * SE3 //
00287 
00288 namespace Internal {
00289 template<int S, typename PV, typename A, typename P>
00290 struct VSE3Mult;
00291 }
00292 
00293 template<int S, typename PV, typename A, typename P>
00294 struct Operator<Internal::VSE3Mult<S,PV,A,P> > {
00295         const Vector<S,PV,A> & lhs;
00296         const SE3<P> & rhs;
00297         
00298         Operator( const Vector<S,PV,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00299         
00300         template <int S0, typename P0, typename A0>
00301         void eval(Vector<S0, P0, A0> & res ) const {
00302                 SizeMismatch<4,S>::test(4, lhs.size());
00303                 res.template slice<0,3>()=lhs.template slice<0,3>() * rhs.get_rotation();
00304                 res[3] = lhs[3];
00305                 res[3] += lhs.template slice<0,3>() * rhs.get_translation();
00306         }
00307         int size() const { return 4; }
00308 };
00309 
00312 template<int S, typename PV, typename A, typename P> inline
00313 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const Vector<S,PV,A>& lhs, const SE3<P> & rhs){
00314         return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSE3Mult<S,PV,A,P> >(lhs,rhs));
00315 }
00316 
00318 // operator *   //
00319 // SE3 * Matrix //
00321 
00322 namespace Internal {
00323 template <int R, int C, typename PM, typename A, typename P>
00324 struct SE3MMult;
00325 }
00326 
00327 template<int R, int Cols, typename PM, typename A, typename P>
00328 struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
00329         const SE3<P> & lhs;
00330         const Matrix<R,Cols,PM,A> & rhs;
00331         
00332         Operator(const SE3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
00333         
00334         template <int R0, int C0, typename P0, typename A0>
00335         void eval(Matrix<R0, C0, P0, A0> & res ) const {
00336                 SizeMismatch<4,R>::test(4, rhs.num_rows());
00337                 for(int i=0; i<rhs.num_cols(); ++i)
00338                         res.T()[i] = lhs * rhs.T()[i];
00339         }
00340         int num_cols() const { return rhs.num_cols(); }
00341         int num_rows() const { return 4; }
00342 };
00343 
00346 template <int R, int Cols, typename PM, typename A, typename P> inline 
00347 Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
00348         return Matrix<4,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE3MMult<R, Cols, PM, A, P> >(lhs,rhs));
00349 }
00350 
00352 // operator *   //
00353 // Matrix * SE3 //
00355 
00356 namespace Internal {
00357 template <int Rows, int C, typename PM, typename A, typename P>
00358 struct MSE3Mult;
00359 }
00360 
00361 template<int Rows, int C, typename PM, typename A, typename P>
00362 struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
00363         const Matrix<Rows,C,PM,A> & lhs;
00364         const SE3<P> & rhs;
00365         
00366         Operator( const Matrix<Rows,C,PM,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00367         
00368         template <int R0, int C0, typename P0, typename A0>
00369         void eval(Matrix<R0, C0, P0, A0> & res ) const {
00370                 SizeMismatch<4, C>::test(4, lhs.num_cols());
00371                 for(int i=0; i<lhs.num_rows(); ++i)
00372                         res[i] = lhs[i] * rhs;
00373         }
00374         int num_cols() const { return 4; }
00375         int num_rows() const { return lhs.num_rows(); }
00376 };
00377 
00380 template <int Rows, int C, typename PM, typename A, typename P> inline 
00381 Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE3<P> & rhs ){
00382         return Matrix<Rows,4,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE3Mult<Rows, C, PM, A, P> >(lhs,rhs));
00383 }
00384 
00385 template <typename Precision>
00386 template <int S, typename P, typename VA>
00387 inline SE3<Precision> SE3<Precision>::exp(const Vector<S, P, VA>& mu){
00388         SizeMismatch<6,S>::test(6, mu.size());
00389         static const Precision one_6th = 1.0/6.0;
00390         static const Precision one_20th = 1.0/20.0;
00391         
00392         SE3<Precision> result;
00393         
00394         const Vector<3,Precision> w = mu.template slice<3,3>();
00395         const Precision theta_sq = w*w;
00396         const Precision theta = sqrt(theta_sq);
00397         Precision A, B;
00398         
00399         const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
00400         if (theta_sq < 1e-8) {
00401                 A = 1.0 - one_6th * theta_sq;
00402                 B = 0.5;
00403                 result.get_translation() = mu.template slice<0,3>() + 0.5 * cross;
00404         } else {
00405                 Precision C;
00406                 if (theta_sq < 1e-6) {
00407                         C = one_6th*(1.0 - one_20th * theta_sq);
00408                         A = 1.0 - theta_sq * C;
00409                         B = 0.5 - 0.25 * one_6th * theta_sq;
00410                 } else {
00411                         const Precision inv_theta = 1.0/theta;
00412                         A = sin(theta) * inv_theta;
00413                         B = (1 - cos(theta)) * (inv_theta * inv_theta);
00414                         C = (1 - A) * (inv_theta * inv_theta);
00415                 }
00416                 result.get_translation() = mu.template slice<0,3>() + B * cross + C * (w ^ cross);
00417         }
00418         rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
00419         return result;
00420 }
00421 
00422 template <typename Precision>
00423 inline Vector<6, Precision> SE3<Precision>::ln(const SE3<Precision>& se3) {
00424         Vector<3,Precision> rot = se3.get_rotation().ln();
00425         const Precision theta = sqrt(rot*rot);
00426 
00427         Precision shtot = 0.5;  
00428         if(theta > 0.00001) {
00429                 shtot = sin(theta/2)/theta;
00430         }
00431         
00432         // now do the rotation
00433         const SO3<Precision> halfrotator = SO3<Precision>::exp(rot * -0.5);
00434         Vector<3, Precision> rottrans = halfrotator * se3.get_translation();
00435         
00436         if(theta > 0.001){
00437                 rottrans -= rot * ((se3.get_translation() * rot) * (1-2*shtot) / (rot*rot));
00438         } else {
00439                 rottrans -= rot * ((se3.get_translation() * rot)/24);
00440         }
00441         
00442         rottrans /= (2 * shtot);
00443         
00444         Vector<6, Precision> result;
00445         result.template slice<0,3>()=rottrans;
00446         result.template slice<3,3>()=rot;
00447         return result;
00448 }
00449 
00450 template <typename Precision>
00451 inline SE3<Precision> operator*(const SO3<Precision>& lhs, const SE3<Precision>& rhs){
00452         return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
00453 }
00454 
00455 #if 0 // should be moved to another header file
00456 
00457     template <class A> inline
00458     Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return pose.get_rotation()*x + pose.get_translation(); }
00459     
00460     template <class A> inline
00461     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A>& uvq)
00462     {
00463         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00464         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
00465         const double inv_z = 1.0/ DqT[2];
00466         return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
00467     }
00468 
00469     template <class A1, class A2> inline
00470     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
00471     {
00472         J_x = pose.get_rotation().get_matrix();
00473         return pose * x;
00474     }
00475 
00476 
00477     template <class A1, class A2> inline
00478     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)
00479     {
00480         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00481         const Vector<3>& T = pose.get_translation();
00482         const double u1 = DqT[0] * inv_z;
00483         J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
00484         J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
00485         J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
00486         
00487         const double v1 = DqT[1] * inv_z;
00488         J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
00489         J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
00490         J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
00491         
00492         const double q1 = q * inv_z;
00493         J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
00494         J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
00495         J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
00496 
00497         return makeVector(u1, v1, q1);
00498     }
00499     
00500 
00501     template <class A1, class A2> inline
00502     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
00503     {
00504         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00505         const Vector<3>& T = pose.get_translation();
00506         const double q = uvq[2];
00507         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00508         const double inv_z = 1.0 / DqT[2];
00509 
00510         return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00511     }
00512 
00513     template <class A1, class A2, class A3> inline
00514     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
00515     {
00516         J_x = pose.get_rotation().get_matrix();
00517         Identity(J_pose.template slice<0,0,3,3>());
00518         const Vector<3> cx = pose * x;
00519         J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
00520         J_pose[1][3] = -(J_pose[0][4] = cx[2]);
00521         J_pose[0][5] = -(J_pose[2][3] = cx[1]);
00522         J_pose[2][4] = -(J_pose[1][5] = cx[0]);
00523         return cx;
00524     }
00525 
00526     template <class A1, class A2, class A3> inline
00527     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)
00528     {
00529         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00530         const Vector<3>& T = pose.get_translation();
00531         const double q = uvq[2];
00532         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00533         const double inv_z = 1.0 / DqT[2];
00534         
00535         const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00536         const double q1 = uvq1[2];
00537 
00538         J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
00539         J_pose[0][0] = J_pose[1][1] = q1;
00540         J_pose[0][2] = -uvq1[0] * q1;
00541         J_pose[1][2] = -uvq1[1] * q1;
00542         J_pose[2][2] = -q1 * q1;
00543         
00544         J_pose[0][3] = -uvq1[1]*uvq1[0];
00545         J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
00546         J_pose[0][5] = -uvq1[1];
00547         
00548         J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
00549         J_pose[1][4] = uvq1[0] * uvq1[1];
00550         J_pose[1][5] = uvq1[0];
00551 
00552         J_pose[2][3] = -uvq1[1]*q1;
00553         J_pose[2][4] = uvq1[0]*q1;
00554         J_pose[2][5] = 0;
00555 
00556         return uvq1;
00557     }
00558 
00559     template <class A1, class A2, class A3> inline
00560     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)
00561     {
00562         const double z_inv = 1.0/in_frame[2];
00563         const double x_z_inv = in_frame[0]*z_inv;
00564         const double y_z_inv = in_frame[1]*z_inv;
00565         const double cross = x_z_inv * y_z_inv;
00566         J_pose[0][0] = J_pose[1][1] = z_inv;
00567         J_pose[0][1] = J_pose[1][0] = 0;
00568         J_pose[0][2] = -x_z_inv * z_inv;
00569         J_pose[1][2] = -y_z_inv * z_inv;
00570         J_pose[0][3] = -cross;
00571         J_pose[0][4] = 1 + x_z_inv*x_z_inv; 
00572         J_pose[0][5] = -y_z_inv;  
00573         J_pose[1][3] = -1 - y_z_inv*y_z_inv;
00574         J_pose[1][4] =  cross;
00575         J_pose[1][5] =  x_z_inv;    
00576         
00577         const TooN::Matrix<3>& R = pose.get_rotation().get_matrix();
00578         J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00579         J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00580         J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00581         J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00582         J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00583         J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00584         
00585         return makeVector(x_z_inv, y_z_inv);
00586     }
00587 
00588 
00589     template <class A1> inline
00590     Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x)
00591     {
00592         return project(transform(pose,x));
00593     }
00594 
00595     template <class A1> inline
00596     Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq)
00597     {
00598         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00599         const Vector<3>& T = pose.get_translation();
00600         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * T;
00601         return project(DqT);
00602     }
00603 
00604     template <class A1, class A2, class A3> inline
00605     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)
00606     {
00607         return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
00608     }
00609 
00610     template <class A1, class A2, class A3> inline
00611     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)
00612     {
00613         const Matrix<3>& R = pose.get_rotation().get_matrix();  
00614         const Vector<3>& T = pose.get_translation();
00615         const double q = uvq[2];
00616         const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00617         const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, J_pose);
00618         J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * pose.get_translation();
00619         J_pose.template slice<0,0,2,3>() *= q;
00620         return uv;
00621     }
00622 
00623 #endif
00624 
00625 }
00626 
00627 #endif