TooN: se3.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

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
    00038 /// Represent a three-dimensional Euclidean transformation (a rotation and a translation).
    00039 /// This can be represented by a \f$3\times\f$4 matrix operating on a homogeneous co-ordinate,
    00040 /// so that a vector \f$\underline{x}\f$ is transformed to a new location \f$\underline{x}'\f$
    00041 /// by
    00042 /// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\ \begin{bmatrix}x'\\y'\\z'\end{bmatrix} &= \begin{pmatrix}r_{11} & r_{12} & r_{13} & t_1\\r_{21} & r_{22} & r_{23} & t_2\\r_{31} & r_{32} & r_{33} & t_3\end{pmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\end{aligned}\f]
    00043 ///
    00044 /// This transformation is a member of the Special Euclidean Lie group SE3. These can be parameterised
    00045 /// six numbers (in the space of the Lie Algebra). In this class, the first three parameters are a
    00046 /// translation vector while the second three are a rotation vector, whose direction is the axis of rotation
    00047 /// and length the amount of rotation (in radians), as for SO3
    00048 /// @ingroup gTransforms
    00049 template <typename Precision = DefaultPrecision>
    00050 class SE3 {
    00051 public:
    00052     /// Default constructor. Initialises the the rotation to zero (the identity) and the translation to zero
    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
    00063     /// Returns the rotation part of the transformation as a SO3
    00064     inline SO3<Precision>& get_rotation(){return my_rotation;}
    00065     /// @overload
    00066     inline const SO3<Precision>& get_rotation() const {return my_rotation;}
    00067
    00068     /// Returns the translation part of the transformation as a Vector
    00069     inline Vector<3, Precision>& get_translation() {return my_translation;}
    00070     /// @overload
    00071     inline const Vector<3, Precision>& get_translation() const {return my_translation;}
    00072
    00073     /// Exponentiate a Vector in the Lie Algebra to generate a new SE3.
    00074     /// See the Detailed Description for details of this vector.
    00075     /// @param vect The Vector to exponentiate
    00076     template <int S, typename P, typename A>
    00077     static inline SE3 exp(const Vector<S, P, A>& vect);
    00078
    00079
    00080     /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
    00081     /// See the Detailed Description for details of this vector.
    00082     static inline Vector<6, Precision> ln(const SE3& se3);
    00083     /// @overload
    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
    00091     /// Right-multiply by another SE3 (concatenate the two transformations)
    00092     /// @param rhs The multipier
    00093     template<typename P>
    00094     inline SE3& operator *=(const SE3<P> & rhs) {
    00095         get_translation() += get_rotation() * rhs.get_translation();
    00096         get_rotation() *= rhs.get_rotation();
    00097         return *this;
    00098     }
    00099
    00100     /// Right-multiply by another SE3 (concatenate the two transformations)
    00101     /// @param rhs The multipier
    00102     template<typename P>
    00103     inline SE3<typename Internal::MultiplyType<Precision, P>::type> operator *(const SE3<P>& rhs) const {
    00104         return SE3<typename Internal::MultiplyType<Precision, P>::type>(get_rotation()*rhs.get_rotation(), get_translation() + get_rotation()*rhs.get_translation());
    00105     }
    00106
    00107     inline SE3& left_multiply_by(const SE3& left) {
    00108         get_translation() = left.get_translation() + left.get_rotation() * get_translation();
    00109         get_rotation() = left.get_rotation() * get_rotation();
    00110         return *this;
    00111     }
    00112
    00113     static inline Matrix<4,4,Precision> generator(int i){
    00114         Matrix<4,4,Precision> result(Zeros);
    00115         if(i < 3){
    00116             result[i][3]=1;
    00117             return result;
    00118         }
    00119         result[(i+1)%3][(i+2)%3] = -1;
    00120         result[(i+2)%3][(i+1)%3] = 1;
    00121         return result;
    00122     }
    00123
    00124   /// Returns the i-th generator times pos
    00125   template<typename Base>
    00126   inline static Vector<4,Precision> generator_field(int i, const Vector<4, Precision, Base>& pos)
    00127   {
    00128     Vector<4, Precision> result(Zeros);
    00129     if(i < 3){
    00130       result[i]=pos[3];
    00131       return result;
    00132     }
    00133     result[(i+1)%3] = - pos[(i+2)%3];
    00134     result[(i+2)%3] = pos[(i+1)%3];
    00135     return result;
    00136   }
    00137
    00138     /// Transfer a matrix in the Lie Algebra from one
    00139     /// co-ordinate frame to another. This is the operation such that for a matrix
    00140     /// \f$ B \f$,
    00141     /// \f$ e^{\text{Adj}(v)} = Be^{v}B^{-1} \f$
    00142     /// @param M The Matrix to transfer
    00143     template<int S, typename P2, typename Accessor>
    00144     inline Vector<6, Precision> adjoint(const Vector<S,P2, Accessor>& vect)const;
    00145
    00146     /// Transfer covectors between frames (using the transpose of the inverse of the adjoint)
    00147     /// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
    00148     template<int S, typename P2, typename Accessor>
    00149     inline Vector<6, Precision> trinvadjoint(const Vector<S,P2,Accessor>& vect)const;
    00150
    00151     ///@overload
    00152     template <int R, int C, typename P2, typename Accessor>
    00153     inline Matrix<6,6,Precision> adjoint(const Matrix<R,C,P2,Accessor>& M)const;
    00154
    00155     ///@overload
    00156     template <int R, int C, typename P2, typename Accessor>
    00157     inline Matrix<6,6,Precision> trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const;
    00158
    00159 private:
    00160     SO3<Precision> my_rotation;
    00161     Vector<3, Precision> my_translation;
    00162 };
    00163
    00164 // transfers a vector in the Lie algebra
    00165 // from one coord frame to another
    00166 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
    00167 template<typename Precision>
    00168 template<int S, typename P2, typename Accessor>
    00169 inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,P2, Accessor>& vect) const{
    00170     SizeMismatch<6,S>::test(6, vect.size());
    00171     Vector<6, Precision> result;
    00172     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
    00173     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
    00174     result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
    00175     return result;
    00176 }
    00177
    00178 // tansfers covectors between frames
    00179 // (using the transpose of the inverse of the adjoint)
    00180 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
    00181 template<typename Precision>
    00182 template<int S, typename P2, typename Accessor>
    00183 inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const Vector<S,P2, Accessor>& vect) const{
    00184     SizeMismatch<6,S>::test(6, vect.size());
    00185     Vector<6, Precision> result;
    00186     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
    00187     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
    00188     result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
    00189     return result;
    00190 }
    00191
    00192 template<typename Precision>
    00193 template<int R, int C, typename P2, typename Accessor>
    00194 inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const Matrix<R,C,P2,Accessor>& M)const{
    00195     SizeMismatch<6,R>::test(6, M.num_cols());
    00196     SizeMismatch<6,C>::test(6, M.num_rows());
    00197
    00198     Matrix<6,6,Precision> result;
    00199     for(int i=0; i<6; i++){
    00200         result.T()[i] = adjoint(M.T()[i]);
    00201     }
    00202     for(int i=0; i<6; i++){
    00203         result[i] = adjoint(result[i]);
    00204     }
    00205     return result;
    00206 }
    00207
    00208 template<typename Precision>
    00209 template<int R, int C, typename P2, typename Accessor>
    00210 inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const{
    00211     SizeMismatch<6,R>::test(6, M.num_cols());
    00212     SizeMismatch<6,C>::test(6, M.num_rows());
    00213
    00214     Matrix<6,6,Precision> result;
    00215     for(int i=0; i<6; i++){
    00216         result.T()[i] = trinvadjoint(M.T()[i]);
    00217     }
    00218     for(int i=0; i<6; i++){
    00219         result[i] = trinvadjoint(result[i]);
    00220     }
    00221     return result;
    00222 }
    00223
    00224 /// Write an SE3 to a stream
    00225 /// @relates SE3
    00226 template <typename Precision>
    00227 inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
    00228     std::streamsize fw = os.width();
    00229     for(int i=0; i<3; i++){
    00230         os.width(fw);
    00231         os << rhs.get_rotation().get_matrix()[i];
    00232         os.width(fw);
    00233         os << rhs.get_translation()[i] << '\n';
    00234     }
    00235     return os;
    00236 }
    00237
    00238
    00239 /// Reads an SE3 from a stream
    00240 /// @relates SE3
    00241 template <typename Precision>
    00242 inline std::istream& operator>>(std::istream& is, SE3<Precision>& rhs){
    00243     for(int i=0; i<3; i++){
    00244         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
    00245     }
    00246     rhs.get_rotation().coerce();
    00247     return is;
    00248 }
    00249
    00250 //////////////////
    00251 // operator *   //
    00252 // SE3 * Vector //
    00253 //////////////////
    00254
    00255 namespace Internal {
    00256 template<int S, typename PV, typename A, typename P>
    00257 struct SE3VMult;
    00258 }
    00259
    00260 template<int S, typename PV, typename A, typename P>
    00261 struct Operator<Internal::SE3VMult<S,PV,A,P> > {
    00262     const SE3<P> & lhs;
    00263     const Vector<S,PV,A> & rhs;
    00264
    00265     Operator(const SE3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
    00266
    00267     template <int S0, typename P0, typename A0>
    00268     void eval(Vector<S0, P0, A0> & res ) const {
    00269         SizeMismatch<4,S>::test(4, rhs.size());
    00270         res.template slice<0,3>()=lhs.get_rotation() * rhs.template slice<0,3>();
    00271         res.template slice<0,3>()+=TooN::operator*(lhs.get_translation(),rhs[3]);
    00272         res[3] = rhs[3];
    00273     }
    00274     int size() const { return 4; }
    00275 };
    00276
    00277 /// Right-multiply by a Vector
    00278 /// @relates SE3
    00279 template<int S, typename PV, typename A, typename P> inline
    00280 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P> & lhs, const Vector<S,PV,A>& rhs){
    00281     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE3VMult<S,PV,A,P> >(lhs,rhs));
    00282 }
    00283
    00284 /// Right-multiply by a Vector
    00285 /// @relates SE3
    00286 template <typename PV, typename A, typename P> inline
    00287 Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>& lhs, const Vector<3,PV,A>& rhs){
    00288     return lhs.get_translation() + lhs.get_rotation() * rhs;
    00289 }
    00290
    00291 //////////////////
    00292 // operator *   //
    00293 // Vector * SE3 //
    00294 //////////////////
    00295
    00296 namespace Internal {
    00297 template<int S, typename PV, typename A, typename P>
    00298 struct VSE3Mult;
    00299 }
    00300
    00301 template<int S, typename PV, typename A, typename P>
    00302 struct Operator<Internal::VSE3Mult<S,PV,A,P> > {
    00303     const Vector<S,PV,A> & lhs;
    00304     const SE3<P> & rhs;
    00305
    00306     Operator( const Vector<S,PV,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
    00307
    00308     template <int S0, typename P0, typename A0>
    00309     void eval(Vector<S0, P0, A0> & res ) const {
    00310         SizeMismatch<4,S>::test(4, lhs.size());
    00311         res.template slice<0,3>()=lhs.template slice<0,3>() * rhs.get_rotation();
    00312         res[3] = lhs[3];
    00313         res[3] += lhs.template slice<0,3>() * rhs.get_translation();
    00314     }
    00315     int size() const { return 4; }
    00316 };
    00317
    00318 /// Left-multiply by a Vector
    00319 /// @relates SE3
    00320 template<int S, typename PV, typename A, typename P> inline
    00321 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const Vector<S,PV,A>& lhs, const SE3<P> & rhs){
    00322     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSE3Mult<S,PV,A,P> >(lhs,rhs));
    00323 }
    00324
    00325 //////////////////
    00326 // operator *   //
    00327 // SE3 * Matrix //
    00328 //////////////////
    00329
    00330 namespace Internal {
    00331 template <int R, int C, typename PM, typename A, typename P>
    00332 struct SE3MMult;
    00333 }
    00334
    00335 template<int R, int Cols, typename PM, typename A, typename P>
    00336 struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
    00337     const SE3<P> & lhs;
    00338     const Matrix<R,Cols,PM,A> & rhs;
    00339
    00340     Operator(const SE3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
    00341
    00342     template <int R0, int C0, typename P0, typename A0>
    00343     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00344         SizeMismatch<4,R>::test(4, rhs.num_rows());
    00345         for(int i=0; i<rhs.num_cols(); ++i)
    00346             res.T()[i] = lhs * rhs.T()[i];
    00347     }
    00348     int num_cols() const { return rhs.num_cols(); }
    00349     int num_rows() const { return 4; }
    00350 };
    00351
    00352 /// Right-multiply by a Matrix
    00353 /// @relates SE3
    00354 template <int R, int Cols, typename PM, typename A, typename P> inline
    00355 Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
    00356     return Matrix<4,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE3MMult<R, Cols, PM, A, P> >(lhs,rhs));
    00357 }
    00358
    00359 //////////////////
    00360 // operator *   //
    00361 // Matrix * SE3 //
    00362 //////////////////
    00363
    00364 namespace Internal {
    00365 template <int Rows, int C, typename PM, typename A, typename P>
    00366 struct MSE3Mult;
    00367 }
    00368
    00369 template<int Rows, int C, typename PM, typename A, typename P>
    00370 struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
    00371     const Matrix<Rows,C,PM,A> & lhs;
    00372     const SE3<P> & rhs;
    00373
    00374     Operator( const Matrix<Rows,C,PM,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
    00375
    00376     template <int R0, int C0, typename P0, typename A0>
    00377     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00378         SizeMismatch<4, C>::test(4, lhs.num_cols());
    00379         for(int i=0; i<lhs.num_rows(); ++i)
    00380             res[i] = lhs[i] * rhs;
    00381     }
    00382     int num_cols() const { return 4; }
    00383     int num_rows() const { return lhs.num_rows(); }
    00384 };
    00385
    00386 /// Left-multiply by a Matrix
    00387 /// @relates SE3
    00388 template <int Rows, int C, typename PM, typename A, typename P> inline
    00389 Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE3<P> & rhs ){
    00390     return Matrix<Rows,4,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE3Mult<Rows, C, PM, A, P> >(lhs,rhs));
    00391 }
    00392
    00393 template <typename Precision>
    00394 template <int S, typename P, typename VA>
    00395 inline SE3<Precision> SE3<Precision>::exp(const Vector<S, P, VA>& mu){
    00396     SizeMismatch<6,S>::test(6, mu.size());
    00397     static const Precision one_6th = 1.0/6.0;
    00398     static const Precision one_20th = 1.0/20.0;
    00399     using std::sqrt;
    00400
    00401     SE3<Precision> result;
    00402
    00403     const Vector<3,Precision> w = mu.template slice<3,3>();
    00404     const Precision theta_sq = w*w;
    00405     const Precision theta = sqrt(theta_sq);
    00406     Precision A, B;
    00407
    00408     const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
    00409     if (theta_sq < 1e-8) {
    00410         A = 1.0 - one_6th * theta_sq;
    00411         B = 0.5;
    00412         result.get_translation() = mu.template slice<0,3>() + 0.5 * cross;
    00413     } else {
    00414         Precision C;
    00415         if (theta_sq < 1e-6) {
    00416             C = one_6th*(1.0 - one_20th * theta_sq);
    00417             A = 1.0 - theta_sq * C;
    00418             B = 0.5 - 0.25 * one_6th * theta_sq;
    00419         } else {
    00420             const Precision inv_theta = 1.0/theta;
    00421             A = sin(theta) * inv_theta;
    00422             B = (1 - cos(theta)) * (inv_theta * inv_theta);
    00423             C = (1 - A) * (inv_theta * inv_theta);
    00424         }
    00425         result.get_translation() = mu.template slice<0,3>() + TooN::operator*(B, cross) + TooN::operator*(C, (w ^ cross));
    00426     }
    00427     rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
    00428     return result;
    00429 }
    00430
    00431 template <typename Precision>
    00432 inline Vector<6, Precision> SE3<Precision>::ln(const SE3<Precision>& se3) {
    00433     using std::sqrt;
    00434     Vector<3,Precision> rot = se3.get_rotation().ln();
    00435     const Precision theta = sqrt(rot*rot);
    00436
    00437     Precision shtot = 0.5;
    00438     if(theta > 0.00001) {
    00439         shtot = sin(theta/2)/theta;
    00440     }
    00441
    00442     // now do the rotation
    00443     const SO3<Precision> halfrotator = SO3<Precision>::exp(rot * -0.5);
    00444     Vector<3, Precision> rottrans = halfrotator * se3.get_translation();
    00445
    00446     if(theta > 0.001){
    00447         rottrans -= TooN::operator*(rot, ((se3.get_translation() * rot) * (1-2*shtot) / (rot*rot)));
    00448     } else {
    00449         rottrans -= TooN::operator*(rot, ((se3.get_translation() * rot)/24));
    00450     }
    00451
    00452     rottrans /= (2 * shtot);
    00453
    00454     Vector<6, Precision> result;
    00455     result.template slice<0,3>()=rottrans;
    00456     result.template slice<3,3>()=rot;
    00457     return result;
    00458 }
    00459
    00460 template <typename Precision>
    00461 inline SE3<Precision> operator*(const SO3<Precision>& lhs, const SE3<Precision>& rhs){
    00462     return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
    00463 }
    00464
    00465 #if 0 // should be moved to another header file
    00466
    00467     template <class A> inline
    00468     Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return pose.get_rotation()*x + pose.get_translation(); }
    00469
    00470     template <class A> inline
    00471     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A>& uvq)
    00472     {
    00473     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00474     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
    00475     const double inv_z = 1.0/ DqT[2];
    00476     return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
    00477     }
    00478
    00479     template <class A1, class A2> inline
    00480     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
    00481     {
    00482     J_x = pose.get_rotation().get_matrix();
    00483     return pose * x;
    00484     }
    00485
    00486
    00487     template <class A1, class A2> inline
    00488     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)
    00489     {
    00490     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00491     const Vector<3>& T = pose.get_translation();
    00492     const double u1 = DqT[0] * inv_z;
    00493     J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
    00494     J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
    00495     J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
    00496
    00497     const double v1 = DqT[1] * inv_z;
    00498     J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
    00499     J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
    00500     J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
    00501
    00502     const double q1 = q * inv_z;
    00503     J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
    00504     J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
    00505     J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
    00506
    00507     return makeVector(u1, v1, q1);
    00508     }
    00509
    00510
    00511     template <class A1, class A2> inline
    00512     Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
    00513     {
    00514     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00515     const Vector<3>& T = pose.get_translation();
    00516     const double q = uvq[2];
    00517     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
    00518     const double inv_z = 1.0 / DqT[2];
    00519
    00520     return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
    00521     }
    00522
    00523     template <class A1, class A2, class A3> inline
    00524     Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
    00525     {
    00526     J_x = pose.get_rotation().get_matrix();
    00527     Identity(J_pose.template slice<0,0,3,3>());
    00528     const Vector<3> cx = pose * x;
    00529     J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
    00530     J_pose[1][3] = -(J_pose[0][4] = cx[2]);
    00531     J_pose[0][5] = -(J_pose[2][3] = cx[1]);
    00532     J_pose[2][4] = -(J_pose[1][5] = cx[0]);
    00533     return cx;
    00534     }
    00535
    00536     template <class A1, class A2, class A3> inline
    00537     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)
    00538     {
    00539     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00540     const Vector<3>& T = pose.get_translation();
    00541     const double q = uvq[2];
    00542     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
    00543     const double inv_z = 1.0 / DqT[2];
    00544
    00545     const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
    00546     const double q1 = uvq1[2];
    00547
    00548     J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
    00549     J_pose[0][0] = J_pose[1][1] = q1;
    00550     J_pose[0][2] = -uvq1[0] * q1;
    00551     J_pose[1][2] = -uvq1[1] * q1;
    00552     J_pose[2][2] = -q1 * q1;
    00553
    00554     J_pose[0][3] = -uvq1[1]*uvq1[0];
    00555     J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
    00556     J_pose[0][5] = -uvq1[1];
    00557
    00558     J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
    00559     J_pose[1][4] = uvq1[0] * uvq1[1];
    00560     J_pose[1][5] = uvq1[0];
    00561
    00562     J_pose[2][3] = -uvq1[1]*q1;
    00563     J_pose[2][4] = uvq1[0]*q1;
    00564     J_pose[2][5] = 0;
    00565
    00566     return uvq1;
    00567     }
    00568
    00569     template <class A1, class A2, class A3> inline
    00570     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)
    00571     {
    00572     const double z_inv = 1.0/in_frame[2];
    00573     const double x_z_inv = in_frame[0]*z_inv;
    00574     const double y_z_inv = in_frame[1]*z_inv;
    00575     const double cross = x_z_inv * y_z_inv;
    00576     J_pose[0][0] = J_pose[1][1] = z_inv;
    00577     J_pose[0][1] = J_pose[1][0] = 0;
    00578     J_pose[0][2] = -x_z_inv * z_inv;
    00579     J_pose[1][2] = -y_z_inv * z_inv;
    00580     J_pose[0][3] = -cross;
    00581     J_pose[0][4] = 1 + x_z_inv*x_z_inv;
    00582     J_pose[0][5] = -y_z_inv;
    00583     J_pose[1][3] = -1 - y_z_inv*y_z_inv;
    00584     J_pose[1][4] =  cross;
    00585     J_pose[1][5] =  x_z_inv;
    00586
    00587     const TooN::Matrix<3>& R = pose.get_rotation().get_matrix();
    00588     J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
    00589     J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
    00590     J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
    00591     J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
    00592     J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
    00593     J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
    00594
    00595     return makeVector(x_z_inv, y_z_inv);
    00596     }
    00597
    00598
    00599     template <class A1> inline
    00600     Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x)
    00601     {
    00602     return project(transform(pose,x));
    00603     }
    00604
    00605     template <class A1> inline
    00606     Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq)
    00607     {
    00608     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00609     const Vector<3>& T = pose.get_translation();
    00610     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * T;
    00611     return project(DqT);
    00612     }
    00613
    00614     template <class A1, class A2, class A3> inline
    00615     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)
    00616     {
    00617     return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
    00618     }
    00619
    00620     template <class A1, class A2, class A3> inline
    00621     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)
    00622     {
    00623     const Matrix<3>& R = pose.get_rotation().get_matrix();
    00624     const Vector<3>& T = pose.get_translation();
    00625     const double q = uvq[2];
    00626     const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
    00627     const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, J_pose);
    00628     J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * pose.get_translation();
    00629     J_pose.template slice<0,0,2,3>() *= q;
    00630     return uv;
    00631     }
    00632
    00633 #endif
    00634
    00635 }
    00636
    00637 #endif