Jafar: modules/geom/include/geom/t3dQuaternion.hpp Source File

00001 /* $Id$ */
00002 
00003 #ifndef GEOM_T3D_QUATERNION_HPP
00004 #define GEOM_T3D_QUATERNION_HPP
00005 
00006 #include <iostream>
00007 
00008 #include "jmath/jblas.hpp"
00009 
00010 #include "geom/t3d.hpp"
00011 
00012 namespace jafar {
00013   namespace geom {
00014 
00023     class T3DQuaternion  : public T3D {
00024       
00025     public:
00026 
00027       T3DQuaternion(bool hasCov_=false);
00028       T3DQuaternion(const jblas::vec& x_, bool hasCov_=false);
00029       T3DQuaternion(const jblas::vec& x_, const jblas::sym_mat& xCov_);
00030       T3DQuaternion(const T3DQuaternion& t_);
00031 
00032       T3DQuaternion& operator=(const T3DQuaternion& t_);
00033 
00034       ~T3DQuaternion();
00035 
00036       Type type() const {return QUATERNION;}
00037 
00038       double translationNorm() const;
00039 
00040       void setValue(double x_, double y_, double z_, 
00041                     double qw_, double qx_, double qy_, double qz_);
00042       
00043       void normalize();
00044 
00045     protected:
00046 
00047       void updateM() const;
00048 
00049       void updateX();
00050 
00051       friend class T3D;
00052 
00053     };
00054 
00055     namespace t3d {
00056 
00064       template<class VecT, class VecPt, class MatJT, class MatJPt>
00065       void pointFromFrameJacQuaternion(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
00066   JFR_PRECOND(t.size() == 7, "t3d::pointFromFrameJacQuaternion: t3d is [x, y, z, qw,qx,qy,qz]");
00067         JFR_PRECOND(pt.size() == 3, "t3d::pointFromFrameJacQuaternion: pt is [x,y,z]");
00068   JFR_PRECOND(JT.size1() == 3 && JT.size2() == 7,
00069         "t3d::pointFromFrameJacQuaternion: JT invalid size");
00070   JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
00071         "t3d::pointFromFrameJacQuaternion: JPt invalid size");
00072 
00073         double const& q_w = t(3);
00074   double const& q_x = t(4);
00075   double const& q_y = t(5);
00076   double const& q_z = t(6);
00077 
00078   double const& pt_x = pt(0);
00079   double const& pt_y = pt(1);
00080   double const& pt_z = pt(2);
00081 
00082         // compute Jacobian with respect to quaternion parameters
00084         jblas::mat_range jt (JT, ublas::range(0,3), ublas::range(0,3));
00085         jt = jblas::identity_mat(3);
00087         JT(0,3) =  2 * (q_w * pt_x - q_z * pt_y + q_y * pt_z);
00088         JT(0,4) =  2 * (q_x * pt_x + q_y * pt_y + q_z * pt_z);
00089         JT(0,5) = -2 * (q_y * pt_x - q_x * pt_y - q_w * pt_z);
00090         JT(0,6) = -2 * (q_z * pt_x + q_w * pt_y - q_x * pt_z);
00091         JT(1,3) = -JT(0,6);
00092         JT(1,4) = -JT(0,5);
00093         JT(1,5) =  JT(0,4);
00094         JT(1,6) =  JT(0,3);
00095         JT(2,3) =  JT(0,5);
00096         JT(2,4) = -JT(0,6);
00097         JT(2,5) = -JT(0,3);
00098         JT(2,6) =  JT(0,4);
00099 
00100         // compute Jacobian with respect to point coordinates
00101         JPt(0,0) = 1 - 2*(q_y * q_y) - 2*(q_z * q_z);
00102         JPt(0,1) = (q_x * q_y) - (q_w * q_z);
00103         JPt(0,2) = (q_w * q_y) + (q_x * q_z);
00104         JPt(1,0) = (q_x * q_y) + (q_w * q_z);
00105         JPt(1,1) = 1 - 2*(q_x * q_x) - 2*(q_z * q_z);
00106         JPt(1,2) = (q_y * q_z) - (q_w * q_x);
00107         JPt(2,0) = (q_x * q_z) - (q_w * q_y);
00108         JPt(2,1) = (q_w * q_x) + (q_y * q_z) ;
00109         JPt(2,2) = 1 - 2*(q_x * q_x) - 2*(q_y * q_y);
00110       }
00111 
00119       template<class VecT, class VecPt, class MatJT, class MatJPt>
00120       void pointToFrameJacQuaternion(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
00121   JFR_PRECOND(t.size() == 7, "t3d::pointToFrameJacQuaternion: t3d is [x,y,z,qw,qx,qy,qz]");
00122         JFR_PRECOND(pt.size() == 3, "t3d::pointToFrameJacQuaternion: pt is [x,y,z]");
00123   JFR_PRECOND(JT.size1() == 3 && JT.size2() == 7,
00124         "t3d::pointToFrameJacQuaternion: JT invalid size");
00125   JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
00126         "t3d::pointToFrameJacQuaternion: JPt invalid size");
00127         
00128         double const& x = t(0);
00129         double const& y = t(1);
00130         double const& z = t(2);
00131         double const& q_w = t(3);
00132   double const& q_x = t(4);
00133   double const& q_y = t(5);
00134   double const& q_z = t(6);
00135 
00136   double const& pt_x = pt(0);
00137   double const& pt_y = pt(1);
00138   double const& pt_z = pt(2);
00139 
00140         // compute Jacobian with respect to point coordinates
00141         JPt(0,0) = 1 - 2*(q_y * q_y) - 2*(q_z * q_z);
00142         JPt(0,1) = (q_x * q_y) + (q_w * q_z);
00143         JPt(0,2) = (q_x * q_z) - (q_w * q_y);
00144         JPt(1,0) = (q_x * q_y) - (q_w * q_z);
00145         JPt(1,1) = 1 - 2*(q_x * q_x) - 2*(q_z * q_z);
00146         JPt(1,2) = (q_w * q_x) + (q_y * q_z) ;
00147         JPt(2,0) = (q_w * q_y) + (q_x * q_z);
00148         JPt(2,1) = (q_y * q_z) - (q_w * q_x);
00149         JPt(2,2) = 1 - 2*(q_x * q_x) - 2*(q_y * q_y); // OK Joan Sola 6/8/08
00150 
00151         // compute Jacobian with respect to T3D parameters
00153         jblas::mat_range jt (JT, ublas::range(0,3), ublas::range(0,3));
00154         jt = -1 * JPt;
00156         JT(0,3) =  2 * ((q_w * (pt_x - x)) + (q_z * (pt_y - y)) - (q_y * (pt_z - z)));
00157         JT(0,4) =  2 * ((q_x * (pt_x - x)) + (q_y * (pt_y - y)) + (q_z * (pt_z - z)));
00158         JT(0,5) = -2 * ((q_y * (pt_x - x)) - (q_x * (pt_y - y)) + (q_w * (pt_z - z)));
00159         JT(0,6) = -2 * ((q_z * (pt_x - x)) - (q_w * (pt_y - y)) - (q_x * (pt_z - z)));
00160 
00161         JT(1,3) =  JT(0,6);
00162         JT(1,4) = -JT(0,5);
00163         JT(1,5) =  JT(0,4);
00164         JT(1,6) = -JT(0,3);
00165         JT(2,3) = -JT(0,5);
00166         JT(2,4) = -JT(0,6);
00167         JT(2,5) =  JT(0,3);
00168         JT(2,6) =  JT(0,4); // OK Joan sola 6/8/08
00169       }
00170     } //namespace t3d
00171 
00172   } // namespace geom
00173 } // namespace jafar
00174 
00175 #endif // GEOM_T3D_QUATERNION_HPP