00001
00002
00008 #ifndef SLAM_EULER_TOOLS_HPP
00009 #define SLAM_EULER_TOOLS_HPP
00010
00011 #include "kernel/jafarException.hpp"
00012
00013 #include "jmath/jblas.hpp"
00014
00015 namespace jafar {
00016 namespace slam {
00017
00022 namespace EulerTools {
00023
00024 enum H {h_0, h_1};
00025
00026 namespace details {
00027
00028 void toFrame(double x, double y, double z,
00029 double yaw, double pitch, double roll,
00030 double v_x, double v_y, double v_z, H h,
00031 double& vres_x, double& vres_y, double& vres_z);
00032
00033 template<class M_Jframe, class M_Jv>
00034 void toFrameJac(double x, double y, double z,
00035 double yaw, double pitch, double roll,
00036 double v_x, double v_y, double v_z, H h,
00037 M_Jframe& Jframe, M_Jv& Jv);
00038
00039 void fromFrame(double x, double y, double z,
00040 double yaw, double pitch, double roll,
00041 double v_x, double v_y, double v_z, H h,
00042 double& vres_x, double& vres_y, double& vres_z);
00043
00044 template<class M_Jframe, class M_Jv>
00045 void fromFrameJac(double x, double y, double z,
00046 double yaw, double pitch, double roll,
00047 double v_x, double v_y, double v_z, H h,
00048 M_Jframe& Jframe, M_Jv& Jv);
00049
00050 void composeFrame(double x2, double y2, double z2,
00051 double yaw2, double pitch2, double roll2,
00052 double x1, double y1, double z1,
00053 double yaw1, double pitch1, double roll1,
00054 double& x_res, double& y_res, double& z_res,
00055 double& yaw_res, double& pitch_res, double& roll_res);
00056
00057 void composeFrameJac(double x2, double y2, double z2,
00058 double yaw2, double pitch2, double roll2,
00059 double x1, double y1, double z1,
00060 double yaw1, double pitch1, double roll1,
00061 jblas::mat& Jframe1, jblas::mat& Jframe2);
00062
00063 void invFrame(double x, double y, double z,
00064 double yaw, double pitch, double roll,
00065 double& x_inv, double& y_inv, double& z_inv,
00066 double& yaw_inv, double& pitch_inv, double& roll_inv);
00067
00068 void lineToFrame(double x, double y, double z,
00069 double yaw, double pitch, double roll,
00070 double n1, double n2, double n3, double u1, double u2, double u3,
00071 double& n1Res, double& n2Res, double& n3Res, double& u1Res, double& u2Res, double& u3Res);
00072
00073
00074 void lineToFrameJac(double x, double y, double z,
00075 double yaw, double pitch, double roll,
00076 double n1, double n2, double n3, double u1, double u2, double u3,
00077 jblas::mat& Jframe, jblas::mat& Jl);
00078
00079 void lineFromFrame(double x, double y, double z,
00080 double yaw, double pitch, double roll,
00081 double n1, double n2, double n3, double u1, double u2, double u3,
00082 double& n1Res, double& n2Res, double& n3Res, double& u1Res, double& u2Res, double& u3Res);
00083
00084 void lineFromFrameJac(double x, double y, double z,
00085 double yaw, double pitch, double roll,
00086 double n1, double n2, double n3, double u1, double u2, double u3,
00087 jblas::mat& Jframe, jblas::mat& Jl);
00088
00089 void planeToFrame(double x, double y, double z,
00090 double yaw, double pitch, double roll,
00091 double px, double py, double pz, double pw,
00092 double& px_res, double& py_res, double& pz_res, double& pw_res);
00093
00094
00095 void planeToFrameJac(double x, double y, double z,
00096 double yaw, double pitch, double roll,
00097 double px, double py, double pz, double pw,
00098 jblas::mat& Jframe, jblas::mat& Jp);
00099
00100 void planeFromFrame(double x, double y, double z,
00101 double yaw, double pitch, double roll,
00102 double px, double py, double pz, double pw,
00103 double& px_res, double& py_res, double& pz_res, double& pw_res);
00104
00105
00106 void planeFromFrameJac(double x, double y, double z,
00107 double yaw, double pitch, double roll,
00108 double px, double py, double pz, double pw,
00109 jblas::mat& Jframe, jblas::mat& Jp);
00110
00111
00112 void odo3d(double x, double y, double z,
00113 double yaw, double pitch, double roll,
00114 double v, double w,
00115 double dt,
00116 double& x_res, double& y_res, double& z_res,
00117 double& yaw_res, double& pitch_res, double& roll_res);
00118
00119 void odo3dJac(double x, double y, double z,
00120 double yaw, double pitch, double roll,
00121 double v, double w,
00122 double dt,
00123 jblas::mat& Jx, jblas::mat& Jodo);
00124
00125 void odo3dInRefFrame(
00126 double yaw_robotToRef,
00127
00128 double x, double y, double z,
00129 double yaw, double pitch, double roll,
00130 double v, double w,
00131 double dt,
00132 double& x_res, double& y_res, double& z_res,
00133 double& yaw_res, double& pitch_res, double& roll_res);
00134
00135 void odo3dInRefFrameJac(
00136 double yaw_robotToRef,
00137
00138 double x, double y, double z,
00139 double yaw, double pitch, double roll,
00140 double v, double w,
00141 double dt,
00142 jblas::mat& Jx, jblas::mat& Jodo);
00143
00144 }
00145
00146 template<class VecBearings, class VecUnitVector>
00147 void bearingsToUnitVector(VecBearings const& b, VecUnitVector& u)
00148 {
00149 JFR_PRECOND(b.size() == 2,
00150 "EulerTools::directionToUnitVector: invalid size of b" << b);
00151 JFR_PRECOND(u.size() == 3,
00152 "EulerTools::directionToUnitVector: invalid size of u");
00153
00154 double t1 = cos(b(1));
00155 u(0) = t1*cos(b(0));
00156 u(1) = t1*sin(b(0));
00157 u(2) = -sin(b(1));
00158 }
00159
00160 template<class VecBearings>
00161 void bearingsToUnitVectorJac(VecBearings const& b, jblas::mat& J)
00162 {
00163 JFR_PRECOND(b.size() == 2,
00164 "EulerTools::bearingsToUnitVectorJac: invalid size of b" << b);
00165 JFR_PRECOND(J.size1()==3 && J.size2()==2,
00166 "EulerTools::bearingsToUnitVectorJac: size of J does not match");
00167
00168
00169 double t1 = cos(b(1));
00170 double t2 = sin(b(0));
00171 double t4 = sin(b(1));
00172 double t5 = cos(b(0));
00173 J(0,0) = -t1 * t2;
00174 J(0,1) = -t4 * t5;
00175 J(1,0) = t1 * t5;
00176 J(1,1) = -t4 * t2;
00177 J(2,0) = 0.0e0;
00178 J(2,1) = -t1;
00179
00180 }
00181
00182 template<class VecVector, class VecDirection>
00183 void vectorToBearings(VecVector const& v, VecDirection& b)
00184 {
00185 JFR_PRECOND(v.size() == 3,
00186 "EulerTools::vectorToBearings: invalid size of v");
00187 JFR_PRECOND(b.size() == 2,
00188 "EulerTools::vectorToBearings: invalid size of d");
00189
00190 b(0) = atan2(v(1),v(0));
00191 b(1) = -atan2(v(2),sqrt(v(0)*v(0) + v(1)*v(1)));
00192 }
00193
00194 template<class VecVector>
00195 void vectorToBearingsJac(VecVector const& v, jblas::mat& J)
00196 {
00197 JFR_PRECOND(v.size() == 3,
00198 "EulerTools::vectorToDirectionJac: invalid size of v");
00199 JFR_PRECOND(J.size1()==2 && J.size2()==3,
00200 "EulerTools::vectorToDirectionJac: size of J does not match");
00201 double x = v(0);
00202 double y = v(1);
00203 double z = v(2);
00204
00205
00206 double t1 = x * x;
00207 double t2 = 0.1e1 / t1;
00208 double t4 = y * y;
00209 double t7 = 0.1e1 / (0.1e1 + t4 * t2);
00210 double t11 = t1 + t4;
00211 double t12 = sqrt(t11);
00212 double t15 = z / t12 / t11;
00213 double t16 = z * z;
00214 double t20 = 0.1e1 / (0.1e1 + t16 / t11);
00215 J(0,0) = -y * t2 * t7;
00216 J(0,1) = 0.1e1 / x * t7;
00217 J(0,2) = 0.0e0;
00218 J(1,0) = t15 * x * t20;
00219 J(1,1) = t15 * y * t20;
00220 J(1,2) = -0.1e1 / t12 * t20;
00221
00222 }
00223
00225 template<class VecFrame1, class VecFrame2, class VecFrameRes>
00226 inline void composeFrame(const VecFrame1& frame1_,
00227 const VecFrame2& frame2_,
00228 VecFrameRes& frameRes)
00229 {
00230 JFR_PRECOND(frame1_.size() == 6, "EulerTools::composeFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00231 JFR_PRECOND(frame2_.size() == 6, "EulerTools::composeFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00232 JFR_PRECOND(frameRes.size() == 6, "EulerTools::composeFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00233 details::composeFrame(frame1_(0), frame1_(1), frame1_(2), frame1_(3), frame1_(4), frame1_(5),
00234 frame2_(0), frame2_(1), frame2_(2), frame2_(3), frame2_(4), frame2_(5),
00235 frameRes(0), frameRes(1), frameRes(2), frameRes(3), frameRes(4), frameRes(5));
00236 }
00237
00239 template<class VecFrame1, class VecFrame2>
00240 inline void composeFrameJac(const VecFrame1& frame1_, const VecFrame2& frame2_,
00241 jblas::mat& Jframe2, jblas::mat& Jframe1)
00242 {
00243 JFR_PRECOND(frame1_.size() == 6, "EulerTools::composeFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00244 JFR_PRECOND(frame2_.size() == 6, "EulerTools::composeFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00245 JFR_TRACE_BEGIN;
00246 details::composeFrameJac(frame1_(0), frame1_(1), frame1_(2), frame1_(3), frame1_(4), frame1_(5),
00247 frame2_(0), frame2_(1), frame2_(2), frame2_(3), frame2_(4), frame2_(5),
00248 Jframe2, Jframe1);
00249 JFR_TRACE_END("EulerTools::composeFrameJac()");
00250 }
00251
00253 template<class VecFrame, class VecFrameInv>
00254 inline void invFrame(VecFrame const& frame, VecFrameInv& frameInv)
00255 {
00256 JFR_PRECOND(frame.size() == 6, "EulerTools::invFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00257 JFR_PRECOND(frameInv.size() == 6, "EulerTools::invFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00258 details::invFrame(frame(0), frame(1), frame(2), frame(3), frame(4), frame(5),
00259 frameInv(0), frameInv(1), frameInv(2), frameInv(3), frameInv(4), frameInv(5));
00260 }
00261
00263 template<class VecFrame, class Vec, class VecRes>
00264 inline void toFrame(const VecFrame& frame_,
00265 const Vec& v_, H h,
00266 VecRes& vRes)
00267 {
00268 JFR_PRECOND(frame_.size() == 6, "EulerTools::toFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00269 JFR_PRECOND(v_.size() == 3, "EulerTools::toFrame: v_ must be a 3D vector");
00270 JFR_PRECOND(vRes.size() == 3, "EulerTools::toFrame: vres must be a 3D vector");
00271 details::toFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00272 v_(0), v_(1), v_(2), h,
00273 vRes(0), vRes(1), vRes(2));
00274 }
00275
00277 template<class VecFrame, class Vec, class M_Jframe, class M_Jv>
00278 inline void toFrameJac(const VecFrame& frame_,
00279 const Vec& v_, H h,
00280 M_Jframe& Jframe, M_Jv& Jv)
00281 {
00282 JFR_PRECOND(frame_.size() == 6, "EulerTools::toFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00283 JFR_PRECOND(v_.size() == 3, "EulerTools::toFrameJac: v_ must be a 3D vector");
00284 JFR_TRACE_BEGIN;
00285 details::toFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00286 v_(0), v_(1), v_(2), h,
00287 Jframe, Jv);
00288 JFR_TRACE_END("EulerTools::toFrameJac()");
00289 }
00290
00292 template<class VecFrame, class Vec, class VecRes>
00293 inline void fromFrame(const VecFrame& frame_,
00294 const Vec& v_, H h,
00295 VecRes& vRes) {
00296 JFR_PRECOND(frame_.size() == 6, "EulerTools::fromFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00297 JFR_PRECOND(v_.size() == 3, "EulerTools::fromFrame: v_ must be a 3D vector");
00298 JFR_PRECOND(vRes.size() == 3, "EulerTools::fromFrame: vres must be a 3D vector");
00299 details::fromFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00300 v_(0), v_(1), v_(2), h,
00301 vRes(0), vRes(1), vRes(2));
00302 }
00303
00305 template<class VecFrame, class Vec, class M_Jframe, class M_Jv>
00306 inline void fromFrameJac(const VecFrame& frame_,
00307 const Vec& v_, H h,
00308 M_Jframe& Jframe, M_Jv& Jv) {
00309 JFR_PRECOND(frame_.size() == 6, "EulerTools::fromFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00310 JFR_PRECOND(v_.size() == 3, "EulerTools::fromFrameJac: v_ must be a 3D vector");
00311 JFR_TRACE_BEGIN;
00312 details::fromFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00313 v_(0), v_(1), v_(2), h,
00314 Jframe, Jv);
00315 JFR_TRACE_END("EulerTools::fromFrameJac()");
00316 }
00317
00319 template<class VecFrame, class Vec, class VecRes>
00320 inline void lineToFrame(const VecFrame& frame_, const Vec& l_, VecRes& lRes) {
00321 JFR_PRECOND(frame_.size() == 6, "EulerTools::linetToFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00322 JFR_PRECOND(l_.size() == 6, "EulerTools::linetToFrame: l_ must be a 6-vector");
00323 JFR_PRECOND(lRes.size() == 6, "EulerTools::linetToFrame: lres must be a 6-vector");
00324 details::lineToFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00325 l_(0), l_(1), l_(2), l_(3), l_(4), l_(5),
00326 lRes(0), lRes(1), lRes(2), lRes(3), lRes(4), lRes(5));
00327 }
00328
00330 template<class VecFrame, class Vec>
00331 inline void lineToFrameJac(const VecFrame& frame_, const Vec& l_,
00332 jblas::mat& Jframe, jblas::mat& Jl) {
00333 JFR_PRECOND(frame_.size() == 6, "EulerTools::linetToFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00334 JFR_PRECOND(l_.size() == 6, "EulerTools::linetToFrameJac: l_ must be a 6-vector");
00335 JFR_TRACE_BEGIN;
00336 details::lineToFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00337 l_(0), l_(1), l_(2), l_(3), l_(4), l_(5),
00338 Jframe, Jl);
00339 JFR_TRACE_END("EulerTools::lineToFrameJac()");
00340 }
00341
00343 template<class VecFrame, class Vec, class VecRes>
00344 inline void lineFromFrame(const VecFrame& frame_, const Vec& l_, VecRes& lRes) {
00345 JFR_PRECOND(frame_.size() == 6, "EulerTools::lineFromFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00346 JFR_PRECOND(l_.size() == 6, "EulerTools::lineFromFrame: l_ must be a 6-vector");
00347 JFR_PRECOND(lRes.size() == 6, "EulerTools::lineFromFrame: lres must be a 6-vector");
00348 details::lineFromFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00349 l_(0), l_(1), l_(2), l_(3), l_(4), l_(5),
00350 lRes(0), lRes(1), lRes(2), lRes(3), lRes(4), lRes(5));
00351 }
00352
00354 template<class VecFrame, class Vec>
00355 inline void lineFromFrameJac(const VecFrame& frame_, const Vec& l_,
00356 jblas::mat& Jframe, jblas::mat& Jl) {
00357 JFR_PRECOND(frame_.size() == 6, "EulerTools::lineFromFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00358 JFR_PRECOND(l_.size() == 6, "EulerTools::lineFromFrameJac: l_ must be a 6-vector");
00359 JFR_TRACE_BEGIN;
00360 details::lineFromFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00361 l_(0), l_(1), l_(2), l_(3), l_(4), l_(5),
00362 Jframe, Jl);
00363 JFR_TRACE_END("EulerTools::lineFromFrameJac()");
00364 }
00365
00366
00368 template<class VecFrame, class Vec, class VecRes>
00369 inline void planeToFrame(const VecFrame& frame_, const Vec& p_, VecRes& pRes) {
00370 JFR_PRECOND(frame_.size() == 6, "EulerTools::planeToFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00371 JFR_PRECOND(p_.size() == 6, "EulerTools::planeToFrame: p_ must be a 6-vector");
00372 JFR_PRECOND(pRes.size() == 6, "EulerTools::planeToFrame: lres must be a 6-vector");
00373 details::planeToFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00374 p_(0), p_(1), p_(2), p_(3), p_(4), p_(5),
00375 pRes(0), pRes(1), pRes(2), pRes(3), pRes(4), pRes(5));
00376 }
00377
00379 template<class VecFrame, class Vec>
00380 inline void planeToFrameJac(const VecFrame& frame_, const Vec& p_,
00381 jblas::mat& Jframe, jblas::mat& Jp) {
00382 JFR_PRECOND(frame_.size() == 6, "EulerTools::planeToFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00383 JFR_PRECOND(p_.size() == 6, "EulerTools::planeToFrameJac: p_ must be a 6-vector");
00384 JFR_TRACE_BEGIN;
00385 details::planeToFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00386 p_(0), p_(1), p_(2), p_(3), p_(4), p_(5),
00387 Jframe, Jp);
00388 JFR_TRACE_END("EulerTools::planeToFrameJac()");
00389 }
00390
00392 template<class VecFrame, class Vec, class VecRes>
00393 inline void planeFromFrame(const VecFrame& frame_, const Vec& p_, VecRes& pRes) {
00394 JFR_PRECOND(frame_.size() == 6, "EulerTools::planeFromFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00395 JFR_PRECOND(p_.size() == 6, "EulerTools::planeFromFrame: p_ must be a 6-vector");
00396 JFR_PRECOND(pRes.size() == 6, "EulerTools::planeFromFrame: pres must be a 6-vector");
00397 details::planeFromFrame(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00398 p_(0), p_(1), p_(2), p_(3), p_(4), p_(5),
00399 pRes(0), pRes(1), pRes(2), pRes(3), pRes(4), pRes(5));
00400 }
00401
00403 template<class VecFrame, class Vec>
00404 inline void planeFromFrameJac(const VecFrame& frame_, const Vec& p_,
00405 jblas::mat& Jframe, jblas::mat& Jp) {
00406 JFR_PRECOND(frame_.size() == 6, "EulerTools::planeFromFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00407 JFR_PRECOND(p_.size() == 6, "EulerTools::planeFromFrameJac: p_ must be a 6-vector");
00408 JFR_TRACE_BEGIN;
00409 details::planeFromFrameJac(frame_(0), frame_(1), frame_(2), frame_(3), frame_(4), frame_(5),
00410 p_(0), p_(1), p_(2), p_(3), p_(4), p_(5),
00411 Jframe, Jp);
00412 JFR_TRACE_END("EulerTools::planeFromFrameJac()");
00413 }
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467 template<class VecOdo, class VecXRes>
00468 void odo3d(VecOdo const& u, double dt, VecXRes& xRes)
00469 {
00470 JFR_PRECOND(u.size() == 2,
00471 "EulerTools::odo3d: u command is [v,w]");
00472 JFR_PRECOND(xRes.size() == 6,
00473 "EulerTools::odo3d: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00474
00475 details::odo3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
00476 u(0), u(1),
00477 dt,
00478 xRes(0), xRes(1), xRes(2), xRes(3), xRes(4), xRes(5));
00479 }
00480
00481 template<class VecOdo>
00482 void odo3dJac(VecOdo const& u, double dt,
00483 jblas::mat& Ju)
00484 {
00485 JFR_PRECOND(u.size() == 2,
00486 "EulerTools::odo3dJac: u command is [v,w]");
00487
00488 jblas::mat Jx(6,2);
00489
00490 details::odo3dJac(0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
00491 u(0), u(1), dt,
00492 Jx, Ju);
00493 }
00494
00495
00496
00497
00498
00499 template<class VecX, class VecOdo, class VecXRes>
00500 void odo3d(VecX const& x, VecOdo const& u, double dt, VecXRes& xRes)
00501 {
00502 JFR_PRECOND(x.size() == 6,
00503 "EulerTools::odo3d: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00504 JFR_PRECOND(u.size() == 2,
00505 "EulerTools::odo3d: u command is [v,w]");
00506 JFR_PRECOND(xRes.size() == 6,
00507 "EulerTools::odo3d: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00508
00509 details::odo3d(x(0), x(1), x(2), x(3), x(4), x(5),
00510 u(0), u(1),
00511 dt,
00512 xRes(0), xRes(1), xRes(2), xRes(3), xRes(4), xRes(5));
00513 }
00514
00515 template<class VecX, class VecOdo>
00516 void odo3dJac(VecX const& x, VecOdo const& u, double dt,
00517 jblas::mat& Jx, jblas::mat& Ju)
00518 {
00519 JFR_PRECOND(x.size() == 6,
00520 "EulerTools::odo3dJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00521 JFR_PRECOND(u.size() == 2,
00522 "EulerTools::odo3dJac: u command is [v,w]");
00523
00524 details::odo3dJac(x(0), x(1), x(2), x(3), x(4), x(5),
00525 u(0), u(1), dt,
00526 Jx, Ju);
00527 }
00528
00529
00530
00531
00532
00533 template<class VecX, class VecOdo, class VecXRes>
00534 void odo3dInRefFrame(double yaw_robotToRef,
00535 VecX const& x, VecOdo const& u, double dt, VecXRes& xRes)
00536 {
00537
00538
00539 JFR_PRECOND(x.size() == 6,
00540 "EulerTools::odo3dInRefFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00541 JFR_PRECOND(u.size() == 2,
00542 "EulerTools::odo3dInRefFrame: u command is [v,w]");
00543 JFR_PRECOND(xRes.size() == 6,
00544 "EulerTools::odo3dInRefFrame: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00545
00546 details::odo3dInRefFrame(yaw_robotToRef,
00547 x(0), x(1), x(2), x(3), x(4), x(5),
00548 u(0), u(1),
00549 dt,
00550 xRes(0), xRes(1), xRes(2), xRes(3), xRes(4), xRes(5));
00551 }
00552
00553 template<class VecX, class VecOdo>
00554 void odo3dInRefFrameJac(double yaw_robotToRef,
00555 VecX const& x, VecOdo const& u, double dt,
00556 jblas::mat& Jx, jblas::mat& Ju)
00557 {
00558
00559
00560 JFR_PRECOND(x.size() == 6,
00561 "EulerTools::odo3dInRefFrameJac: 3D robot pose is [x,y,z,roll,pitch,yaw]");
00562 JFR_PRECOND(u.size() == 2,
00563 "EulerTools::odo3dInRefFrameJac: u command is [v,w]");
00564
00565 details::odo3dInRefFrameJac(yaw_robotToRef,
00566 x(0), x(1), x(2), x(3), x(4), x(5),
00567 u(0), u(1), dt,
00568 Jx, Ju);
00569 }
00570
00571 namespace details {
00572
00573 template<class M_Jframe, class M_Jv>
00574 void fromFrameJac(double x, double y, double z,
00575 double yaw, double pitch, double roll,
00576 double v_x, double v_y, double v_z, H h,
00577 M_Jframe& Jframe, M_Jv& Jv)
00578 {
00579 JFR_PRECOND(Jframe.size1() == 3 && Jframe.size2() == 6,
00580 "fromFrameJac: size of Jframe does not match");
00581 JFR_PRECOND(Jv.size1() == 3 && Jv.size2() == 3,
00582 "fromFrameJac: size of Jv does not match");
00583
00584 switch (h) {
00585 case h_1:
00586 {
00587 double t1 = sin(yaw);
00588 double t2 = cos(pitch);
00589 double t3 = t1 * t2;
00590 double t6 = sin(pitch);
00591 double t7 = t1 * t6;
00592 double t8 = sin(roll);
00593 double t10 = cos(yaw);
00594 double t11 = cos(roll);
00595 double t14 = -t7 * t8 - 0.10e1 * t10 * t11;
00596 double t19 = -t7 * t11 + 0.10e1 * t10 * t8;
00597 double t22 = t10 * t6;
00598 double t25 = t10 * t2;
00599 double t26 = t8 * v_y;
00600 double t28 = t11 * v_z;
00601 double t34 = t22 * t11 + 0.10e1 * t1 * t8;
00602 double t39 = -t22 * t8 + 0.10e1 * t1 * t11;
00603 double t66 = t2 * t11;
00604 double t69 = t2 * t8;
00605
00606 Jframe(0,0) = 0.10e1;
00607 Jframe(0,1) = 0.0e0;
00608 Jframe(0,2) = 0.0e0;
00609 Jframe(0,3) = -0.10e1 * t3 * v_x + t14 * v_y + t19 * v_z;
00610 Jframe(0,4) = -0.10e1 * t22 * v_x + t25 * t26 + t25 * t28;
00611 Jframe(0,5) = t34 * v_y + t39 * v_z;
00612 Jframe(1,0) = 0.0e0;
00613 Jframe(1,1) = 0.10e1;
00614 Jframe(1,2) = 0.0e0;
00615 Jframe(1,3) = 0.10e1 * t25 * v_x - t39 * v_y + t34 * v_z;
00616 Jframe(1,4) = -0.10e1 * t7 * v_x + t3 * t26 + t3 * t28;
00617 Jframe(1,5) = -t19 * v_y + t14 * v_z;
00618 Jframe(2,0) = 0.0e0;
00619 Jframe(2,1) = 0.0e0;
00620 Jframe(2,2) = 0.10e1;
00621 Jframe(2,3) = 0.0e0;
00622 Jframe(2,4) = -0.100e1 * t2 * v_x - 0.10e1 * t6 * t8 * v_y - 0.10e1 * t6 * t11 * v_z;
00623 Jframe(2,5) = 0.10e1 * t66 * v_y - 0.10e1 * t69 * v_z;
00624
00625 Jv(0,0) = 0.10e1 * t25;
00626 Jv(0,1) = -t39;
00627 Jv(0,2) = t34;
00628 Jv(1,0) = 0.10e1 * t3;
00629 Jv(1,1) = -t14;
00630 Jv(1,2) = -t19;
00631 Jv(2,0) = -0.100e1 * t6;
00632 Jv(2,1) = 0.10e1 * t69;
00633 Jv(2,2) = 0.10e1 * t66;
00634
00635 break;
00636 }
00637 case h_0:
00638 {
00639 double t1 = sin(yaw);
00640 double t2 = cos(pitch);
00641 double t3 = t1 * t2;
00642 double t6 = sin(pitch);
00643 double t7 = t1 * t6;
00644 double t8 = sin(roll);
00645 double t10 = cos(yaw);
00646 double t11 = cos(roll);
00647 double t14 = -t7 * t8 - 0.10e1 * t10 * t11;
00648 double t19 = -t7 * t11 + 0.10e1 * t10 * t8;
00649 double t22 = t10 * t6;
00650 double t25 = t10 * t2;
00651 double t26 = t8 * v_y;
00652 double t28 = t11 * v_z;
00653 double t34 = t22 * t11 + 0.10e1 * t1 * t8;
00654 double t39 = -t22 * t8 + 0.10e1 * t1 * t11;
00655 double t66 = t2 * t11;
00656 double t69 = t2 * t8;
00657
00658 Jframe(0,0) = 0.0e0;
00659 Jframe(0,1) = 0.0e0;
00660 Jframe(0,2) = 0.0e0;
00661 Jframe(0,3) = -0.10e1 * t3 * v_x + t14 * v_y + t19 * v_z;
00662 Jframe(0,4) = -0.10e1 * t22 * v_x + t25 * t26 + t25 * t28;
00663 Jframe(0,5) = t34 * v_y + t39 * v_z;
00664 Jframe(1,0) = 0.0e0;
00665 Jframe(1,1) = 0.0e0;
00666 Jframe(1,2) = 0.0e0;
00667 Jframe(1,3) = 0.10e1 * t25 * v_x - t39 * v_y + t34 * v_z;
00668 Jframe(1,4) = -0.10e1 * t7 * v_x + t3 * t26 + t3 * t28;
00669 Jframe(1,5) = -t19 * v_y + t14 * v_z;
00670 Jframe(2,0) = 0.0e0;
00671 Jframe(2,1) = 0.0e0;
00672 Jframe(2,2) = 0.0e0;
00673 Jframe(2,3) = 0.0e0;
00674 Jframe(2,4) = -0.100e1 * t2 * v_x - 0.10e1 * t6 * t8 * v_y - 0.10e1 * t6 * t11 * v_z;
00675 Jframe(2,5) = 0.10e1 * t66 * v_y - 0.10e1 * t69 * v_z;
00676
00677 Jv(0,0) = 0.10e1 * t25;
00678 Jv(0,1) = -t39;
00679 Jv(0,2) = t34;
00680 Jv(1,0) = 0.10e1 * t3;
00681 Jv(1,1) = -t14;
00682 Jv(1,2) = -t19;
00683 Jv(2,0) = -0.100e1 * t6;
00684 Jv(2,1) = 0.10e1 * t69;
00685 Jv(2,2) = 0.10e1 * t66;
00686
00687 break;
00688 }
00689 }
00690 }
00691
00692 template<class M_Jframe, class M_Jv>
00693 void toFrameJac(double x, double y, double z,
00694 double yaw, double pitch, double roll,
00695 double v_x, double v_y, double v_z, H h,
00696 M_Jframe& Jframe, M_Jv& Jv)
00697 {
00698 JFR_PRECOND(Jframe.size1() == 3 && Jframe.size2() == 6,
00699 "toFrameJac: size of Jframe does not match");
00700 JFR_PRECOND(Jv.size1() == 3 && Jv.size2() == 3,
00701 "toFrameJac: size of Jv does not match");
00702
00703 switch (h) {
00704 case h_1:
00705 {
00706 double t1 = cos(yaw);
00707 double t2 = cos(pitch);
00708 double t3 = t1 * t2;
00709 double t5 = sin(yaw);
00710 double t6 = t5 * t2;
00711 double t8 = sin(pitch);
00712 double t19 = t1 * t8;
00713 double t22 = t5 * t8;
00714 double t37 = sin(roll);
00715 double t38 = t19 * t37;
00716 double t40 = cos(roll);
00717 double t41 = t5 * t40;
00718 double t44 = t22 * t37;
00719 double t46 = t1 * t40;
00720 double t49 = t2 * t37;
00721 double t52 = -t44 - 0.10e1 * t46;
00722 double t55 = t38 - 0.10e1 * t41;
00723 double t66 = t8 * t37;
00724 double t78 = t19 * t40;
00725 double t79 = t5 * t37;
00726 double t81 = t78 + 0.10e1 * t79;
00727 double t83 = t22 * t40;
00728 double t84 = t1 * t37;
00729 double t86 = t83 - 0.10e1 * t84;
00730 double t88 = t2 * t40;
00731 double t117 = t8 * t40;
00732 Jframe(0,0) = -0.100e1 * t3;
00733 Jframe(0,1) = -0.100e1 * t6;
00734 Jframe(0,2) = 0.1000e1 * t8;
00735 Jframe(0,3) = -0.10e1 * t6 * v_x + 0.10e1 * t3 * v_y + 0.100e1 * t6 * x - 0.100e1 * t3 * y;
00736 Jframe(0,4) = -0.10e1 * t19 * v_x - 0.10e1 * t22 * v_y - 0.100e1 * t2 * v_z + 0.100e1 * t19 * x + 0.100e1 * t22 * y + 0.1000e1 * t2 * z;
00737 Jframe(0,5) = 0.0e0;
00738 Jframe(1,0) = -0.10e1 * t38 + 0.100e1 * t41;
00739 Jframe(1,1) = -0.10e1 * t44 - 0.100e1 * t46;
00740 Jframe(1,2) = -0.100e1 * t49;
00741 Jframe(1,3) = t52 * v_x + t55 * v_y - 0.10e1 * t52 * x - 0.10e1 * t55 * y;
00742 Jframe(1,4) = t3 * t37 * v_x + t6 * t37 * v_y - 0.10e1 * t66 * v_z - 0.10e1 * t3 * t37 * x - 0.10e1 * t6 * t37 * y + 0.100e1 * t66 * z;
00743 Jframe(1,5) = t81 * v_x + t86 * v_y + 0.10e1 * t88 * v_z - 0.10e1 * t81 * x - 0.10e1 * t86 * y - 0.100e1 * t88 * z;
00744 Jframe(2,0) = -0.10e1 * t78 - 0.100e1 * t79;
00745 Jframe(2,1) = -0.10e1 * t83 + 0.100e1 * t84;
00746 Jframe(2,2) = -0.100e1 * t88;
00747 Jframe(2,3) = -t86 * v_x + t81 * v_y + 0.10e1 * t86 * x - 0.10e1 * t81 * y;
00748 Jframe(2,4) = t3 * t40 * v_x + t6 * t40 * v_y - 0.10e1 * t117 * v_z - 0.10e1 * t3 * t40 * x - 0.10e1 * t6 * t40 * y + 0.100e1 * t117 * z;
00749 Jframe(2,5) = -t55 * v_x + t52 * v_y - 0.10e1 * t49 * v_z + 0.10e1 * t55 * x - 0.10e1 * t52 * y + 0.100e1 * t49 * z;
00750
00751 Jv(0,0) = 0.10e1 * t3;
00752 Jv(0,1) = 0.10e1 * t6;
00753 Jv(0,2) = -0.100e1 * t8;
00754 Jv(1,0) = t55;
00755 Jv(1,1) = -t52;
00756 Jv(1,2) = 0.10e1 * t49;
00757 Jv(2,0) = t81;
00758 Jv(2,1) = t86;
00759 Jv(2,2) = 0.10e1 * t88;
00760
00761 break;
00762 }
00763 case h_0:
00764 {
00765 double t1 = sin(yaw);
00766 double t2 = cos(pitch);
00767 double t3 = t1 * t2;
00768 double t6 = cos(yaw);
00769 double t7 = t6 * t2;
00770 double t11 = sin(pitch);
00771 double t12 = t6 * t11;
00772 double t15 = t1 * t11;
00773 double t24 = sin(roll);
00774 double t26 = cos(roll);
00775 double t29 = -t15 * t24 - 0.10e1 * t6 * t26;
00776 double t34 = t12 * t24 - 0.10e1 * t1 * t26;
00777 double t48 = t12 * t26 + 0.10e1 * t1 * t24;
00778 double t53 = t15 * t26 - 0.10e1 * t6 * t24;
00779 double t55 = t2 * t26;
00780 double t59 = t2 * t24;
00781
00782 Jframe(0,0) = 0.0e0;
00783 Jframe(0,1) = 0.0e0;
00784 Jframe(0,2) = 0.0e0;
00785 Jframe(0,3) = -0.10e1 * t3 * v_x + 0.10e1 * t7 * v_y;
00786 Jframe(0,4) = -0.10e1 * t12 * v_x - 0.10e1 * t15 * v_y - 0.100e1 * t2 * v_z;
00787 Jframe(0,5) = 0.0e0;
00788 Jframe(1,0) = 0.0e0;
00789 Jframe(1,1) = 0.0e0;
00790 Jframe(1,2) = 0.0e0;
00791 Jframe(1,3) = t29 * v_x + t34 * v_y;
00792 Jframe(1,4) = t7 * t24 * v_x + t3 * t24 * v_y - 0.10e1 * t11 * t24 * v_z;
00793 Jframe(1,5) = t48 * v_x + t53 * v_y + 0.10e1 * t55 * v_z;
00794 Jframe(2,0) = 0.0e0;
00795 Jframe(2,1) = 0.0e0;
00796 Jframe(2,2) = 0.0e0;
00797 Jframe(2,3) = -t53 * v_x + t48 * v_y;
00798 Jframe(2,4) = t7 * t26 * v_x + t3 * t26 * v_y - 0.10e1 * t11 * t26 * v_z;
00799 Jframe(2,5) = -t34 * v_x + t29 * v_y - 0.10e1 * t59 * v_z;
00800
00801 Jv(0,0) = 0.10e1 * t7;
00802 Jv(0,1) = 0.10e1 * t3;
00803 Jv(0,2) = -0.100e1 * t11;
00804 Jv(1,0) = t34;
00805 Jv(1,1) = -t29;
00806 Jv(1,2) = 0.10e1 * t59;
00807 Jv(2,0) = t48;
00808 Jv(2,1) = t53;
00809 Jv(2,2) = 0.10e1 * t55;
00810
00811 break;
00812 }
00813 }
00814 }
00815
00816 }
00817
00818 }
00819
00820 }
00821 }
00822
00823 #endif // SLAM_EULER_TOOLS_HPP
