00001
00002
00003 #ifndef GEOM_T3D_EULER_HPP
00004 #define GEOM_T3D_EULER_HPP
00005
00006 #include <iostream>
00007
00008 #include "kernel/jafarMacro.hpp"
00009
00010 #include "jmath/jblas.hpp"
00011
00012 #include "geom/t3d.hpp"
00013
00014 namespace jafar {
00015 namespace geom {
00016
00025 class T3DEuler : public T3D {
00026
00027 public:
00028
00029 T3DEuler(bool hasCov_=false);
00030 T3DEuler(const jblas::vec& x_, bool hasCov_=false);
00031 T3DEuler(const jblas::vec& x_, const jblas::vec& xStdDev_);
00032 T3DEuler(const jblas::vec& x_, const jblas::sym_mat& xCov_);
00033 T3DEuler(const T3D& t_);
00034 T3DEuler(const jblas::mat& M_);
00035 T3DEuler& operator=(const T3DEuler& t_);
00036
00037 ~T3DEuler();
00038
00039 Type type() const {return EULER;}
00040
00041 void setValue(double x_, double y_, double z_,
00042 double yaw_, double pitch_, double roll_);
00043
00044 double translationNorm() const;
00045
00048 void setOdo2D(const jblas::vec& u, double dt);
00049
00053 void setOdo2D(const jblas::vec& u, double dt, const jblas::sym_mat& uCov);
00054
00055 protected:
00056
00057 void updateM() const;
00058
00059 void updateX();
00060
00061 friend class T3D;
00062 friend class test_T3DEuler;
00063 };
00064
00065 namespace t3d {
00066
00071 void invJacEuler(const jblas::vec& v, jblas::mat& J);
00072
00077 void composeJacEuler(jblas::vec const& v1_, jblas::vec const& v2_,
00078 jblas::mat& J1, jblas::mat& J2);
00079
00084 template<class VecT, class VecPt, class MatJT, class MatJPt>
00085 void pointFromFrameJacEuler(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
00086 JFR_PRECOND(t.size() == 6, "t3d::pointFromFrameJacEuler: t3d is [x,y,z,roll,pitch,yaw]");
00087 JFR_PRECOND(pt.size() == 3, "t3d::pointFromFrameJacEuler: pt is [x,y,z]");
00088 JFR_PRECOND(JT.size1() == 3 && JT.size2() == 6,
00089 "t3d::pointFromFrameJacEuler: invalid size");
00090 JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
00091 "t3d::pointFromFrameJacEuler: invalid size");
00092
00093
00094
00095
00096 double const& yaw = t(3);
00097 double const& pitch = t(4);
00098 double const& roll = t(5);
00099 double const& pt_x = pt(0);
00100 double const& pt_y = pt(1);
00101 double const& pt_z = pt(2);
00102
00103
00104
00105 double t1 = sin(yaw);
00106 double t2 = cos(pitch);
00107 double t3 = t1 * t2;
00108 double t6 = sin(pitch);
00109 double t7 = t1 * t6;
00110 double t8 = sin(roll);
00111 double t10 = cos(yaw);
00112 double t11 = cos(roll);
00113 double t14 = -t7 * t8 - 0.10e1 * t10 * t11;
00114 double t19 = -t7 * t11 + 0.10e1 * t10 * t8;
00115 double t22 = t10 * t6;
00116 double t25 = t10 * t2;
00117 double t26 = t8 * pt_y;
00118 double t28 = t11 * pt_z;
00119 double t34 = t22 * t11 + 0.10e1 * t1 * t8;
00120 double t39 = -t22 * t8 + 0.10e1 * t1 * t11;
00121 double t66 = t2 * t11;
00122 double t69 = t2 * t8;
00123
00124 JT(0,0) = 0.10e1;
00125 JT(0,1) = 0.0e0;
00126 JT(0,2) = 0.0e0;
00127 JT(0,3) = -0.10e1 * t3 * pt_x + t14 * pt_y + t19 * pt_z;
00128 JT(0,4) = -0.10e1 * t22 * pt_x + t25 * t26 + t25 * t28;
00129 JT(0,5) = t34 * pt_y + t39 * pt_z;
00130 JT(1,0) = 0.0e0;
00131 JT(1,1) = 0.10e1;
00132 JT(1,2) = 0.0e0;
00133 JT(1,3) = 0.10e1 * t25 * pt_x - t39 * pt_y + t34 * pt_z;
00134 JT(1,4) = -0.10e1 * t7 * pt_x + t3 * t26 + t3 * t28;
00135 JT(1,5) = -t19 * pt_y + t14 * pt_z;
00136 JT(2,0) = 0.0e0;
00137 JT(2,1) = 0.0e0;
00138 JT(2,2) = 0.10e1;
00139 JT(2,3) = 0.0e0;
00140 JT(2,4) = -0.100e1 * t2 * pt_x - 0.10e1 * t6 * t8 * pt_y - 0.10e1 * t6 * t11 * pt_z;
00141 JT(2,5) = 0.10e1 * t66 * pt_y - 0.10e1 * t69 * pt_z;
00142
00143 JPt(0,0) = 0.10e1 * t25;
00144 JPt(0,1) = -t39;
00145 JPt(0,2) = t34;
00146 JPt(1,0) = 0.10e1 * t3;
00147 JPt(1,1) = -t14;
00148 JPt(1,2) = -t19;
00149 JPt(2,0) = -0.100e1 * t6;
00150 JPt(2,1) = 0.10e1 * t69;
00151 JPt(2,2) = 0.10e1 * t66;
00152
00153
00154 }
00155
00160 template<class VecT, class VecPt, class MatJT, class MatJPt>
00161 void pointToFrameJacEuler(VecT const& t, VecPt const& pt, MatJT& JT, MatJPt& JPt) {
00162 JFR_PRECOND(t.size() == 6, "t3d::pointToFrameJacEuler: t3d is [x,y,z,roll,pitch,yaw]");
00163 JFR_PRECOND(pt.size() == 3, "t3d::pointToFrameJacEuler: pt is [x,y,z]");
00164 JFR_PRECOND(JT.size1() == 3 && JT.size2() == 6,
00165 "t3d::pointToFrameJacEuler: invalid size");
00166 JFR_PRECOND(JPt.size1() == 3 && JPt.size2() == 3,
00167 "t3d::pointToFrameJacEuler: invalid size");
00168
00169 double const& x = t(0);
00170 double const& y = t(1);
00171 double const& z = t(2);
00172 double const& yaw = t(3);
00173 double const& pitch = t(4);
00174 double const& roll = t(5);
00175 double const& pt_x = pt(0);
00176 double const& pt_y = pt(1);
00177 double const& pt_z = pt(2);
00178
00179
00180
00181 double t1 = cos(yaw);
00182 double t2 = cos(pitch);
00183 double t3 = t1 * t2;
00184 double t5 = sin(yaw);
00185 double t6 = t5 * t2;
00186 double t8 = sin(pitch);
00187 double t19 = t1 * t8;
00188 double t22 = t5 * t8;
00189 double t37 = sin(roll);
00190 double t38 = t19 * t37;
00191 double t40 = cos(roll);
00192 double t41 = t5 * t40;
00193 double t44 = t22 * t37;
00194 double t46 = t1 * t40;
00195 double t49 = t2 * t37;
00196 double t52 = -t44 - 0.10e1 * t46;
00197 double t55 = t38 - 0.10e1 * t41;
00198 double t66 = t8 * t37;
00199 double t78 = t19 * t40;
00200 double t79 = t5 * t37;
00201 double t81 = t78 + 0.10e1 * t79;
00202 double t83 = t22 * t40;
00203 double t84 = t1 * t37;
00204 double t86 = t83 - 0.10e1 * t84;
00205 double t88 = t2 * t40;
00206 double t117 = t8 * t40;
00207
00208 JT(0,0) = -0.100e1 * t3;
00209 JT(0,1) = -0.100e1 * t6;
00210 JT(0,2) = 0.1000e1 * t8;
00211 JT(0,3) = -0.10e1 * t6 * pt_x + 0.10e1 * t3 * pt_y + 0.100e1 * t6 * x - 0.100e1 * t3 * y;
00212 JT(0,4) = -0.10e1 * t19 * pt_x - 0.10e1 * t22 * pt_y - 0.100e1 * t2 * pt_z + 0.100e1 * t19 * x + 0.100e1 * t22 * y + 0.1000e1 * t2 * z;
00213 JT(0,5) = 0.0e0;
00214 JT(1,0) = -0.10e1 * t38 + 0.100e1 * t41;
00215 JT(1,1) = -0.10e1 * t44 - 0.100e1 * t46;
00216 JT(1,2) = -0.100e1 * t49;
00217 JT(1,3) = t52 * pt_x + t55 * pt_y - 0.10e1 * t52 * x - 0.10e1 * t55 * y;
00218 JT(1,4) = t3 * t37 * pt_x + t6 * t37 * pt_y - 0.10e1 * t66 * pt_z - 0.10e1 * t3 * t37 * x - 0.10e1 * t6 * t37 * y + 0.100e1 * t66 * z;
00219 JT(1,5) = t81 * pt_x + t86 * pt_y + 0.10e1 * t88 * pt_z - 0.10e1 * t81 * x - 0.10e1 * t86 * y - 0.100e1 * t88 * z;
00220 JT(2,0) = -0.10e1 * t78 - 0.100e1 * t79;
00221 JT(2,1) = -0.10e1 * t83 + 0.100e1 * t84;
00222 JT(2,2) = -0.100e1 * t88;
00223 JT(2,3) = -t86 * pt_x + t81 * pt_y + 0.10e1 * t86 * x - 0.10e1 * t81 * y;
00224 JT(2,4) = t3 * t40 * pt_x + t6 * t40 * pt_y - 0.10e1 * t117 * pt_z - 0.10e1 * t3 * t40 * x - 0.10e1 * t6 * t40 * y + 0.100e1 * t117 * z;
00225 JT(2,5) = -t55 * pt_x + t52 * pt_y - 0.10e1 * t49 * pt_z + 0.10e1 * t55 * x - 0.10e1 * t52 * y + 0.100e1 * t49 * z;
00226
00227 JPt(0,0) = 0.10e1 * t3;
00228 JPt(0,1) = 0.10e1 * t6;
00229 JPt(0,2) = -0.100e1 * t8;
00230 JPt(1,0) = t55;
00231 JPt(1,1) = -t52;
00232 JPt(1,2) = 0.10e1 * t49;
00233 JPt(2,0) = t81;
00234 JPt(2,1) = t86;
00235 JPt(2,2) = 0.10e1 * t88;
00236
00237
00238 }
00239
00240
00241
00242
00243 void odo3d(jblas::vec const& u, double dt, jblas::vec& xRes);
00244 void odo3dJac(jblas::vec const& u, double dt, jblas::mat& Ju);
00245
00246 }
00247
00248 }
00249 }
00250
00251 #endif // GEOM_T3D_EULER_HPP
