diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 67 |
1 files changed, 17 insertions, 50 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index 8f6fd12..9c6bf84 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -130,19 +130,10 @@ Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs, *-------------------------------------------------------------------------------------- */ Matrix<double,6,6> -#ifdef FULLSTATE -Feature::S ( const Matrix<double,13,13> &Pxx, const Matrix<double,13,3> &Pxy, +Feature::S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx, const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q) -#else -Feature::S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy, - const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q) -#endif { -#ifdef FULLSTATE - Matrix<double,6,13> hx; -#else - Matrix<double,6,9> hx; -#endif + Matrix<double,6,STATESIZE> hx; Matrix<double,6,3> hy; hx = Hx( pos, q); hy = Hy( pos, q); @@ -350,28 +341,18 @@ Feature::x2p ( const Vector3d &x ) * body. *-------------------------------------------------------------------------------------- */ -#ifdef FULLSTATE -Matrix<double,3,13> -#else -Matrix<double,3,9> -#endif +Matrix<double,3,STATESIZE> Feature::Fx ( double dt ) { double y0,y1,y2; y0 = X[0]; y1 = X[1]; y2 = X[2]; -#ifdef FULLSTATE - Matrix<double,3,13> F; - F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0,0,0,0,0, - 0.,0.,0., y1*y2, 0.,-y2, 0,0,0,0,0,0,0, - 0,0,0, y2*y2, 0, 0, 0,0,0,0,0,0,0; -#else - Matrix<double,3,9> F; - F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0, - 0.,0.,0., y1*y2, 0.,-y2, 0,0,0, - 0,0,0, y2*y2, 0, 0, 0,0,0; -#endif + Matrix<double,3,STATESIZE> F; + F = Matrix<double,3,STATESIZE>::Zero(); + F.block<3,3>(0,3) << y0*y2,-y2, + y1*y2, 0.,-y2, + y2*y2, 0, 0; F *= dt; return F; } /* ----- end of method Feature::Fx ----- */ @@ -408,11 +389,7 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt ) return F; } /* ----- end of method Feature::Fy ----- */ -#ifdef FULLSTATE -Matrix<double,6,13> -#else -Matrix<double,6,9> -#endif +Matrix<double,6,STATESIZE> Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) { double xbw1,xbw2,xbw3; @@ -444,13 +421,8 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) qb0w3=q0.z(); qb0w4=q0.w(); -#ifdef FULLSTATE - Matrix<double,Dynamic,13,0,6,13> H; - H = Matrix<double,6,13>::Zero(); -#else - Matrix<double,Dynamic,9,0,6,9> H; - H = Matrix<double,6,9>::Zero(); -#endif + Matrix<double,Dynamic,STATESIZE,0,6,STATESIZE> H; + H = Matrix<double,6,STATESIZE>::Zero(); // Matlab generated C code. double t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13,t14,t15,t16,t17,t18,t19,t20, @@ -705,7 +677,7 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) H(2,0) = -t47/(t24+t26+t28+t46-t12*(t30+t32+t11*(t22-qbw2*qbw4*2.0))-pib1*t12*t39)-t7*t60*t75; H(2,1) = t52*t76-t9*t60*t75; H(2,2) = -t50*t76-t11*t60*t75; -#ifdef FULLSTATE +#if STATESIZE==13 H(2,6) = -t76*(t12*t87+pib1*t12*t84+pib2*t12*t97)-t60*t75*t137; H(2,7) = t76*(t12*t84-pib1*t12*t87+pib2*t12*t101)-t60*t75*t146; H(2,8) = -t76*(-t12*t97+pib2*t12*t87+pib1*t12*t101)-t60*t75*t157; @@ -714,21 +686,21 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) H(3,0) = -t76*t113-t7*t60*t130; H(3,1) = t76*t114-t9*t60*t130; H(3,2) = t76*t115-t11*t60*t130; -#ifdef FULLSTATE +#if STATESIZE==13 H(3,6) = -t76*(-t12*t143+pib1*t12*t140+pib2*t12*t150)-t60*t130*t137; H(3,7) = t76*(t12*t140+pib1*t12*t143-pib2*t12*t154)-t60*t130*t146; H(3,8) = t76*(t12*t150+pib2*t12*t143+pib1*t12*t154)-t60*t130*t157; H(3,9) = -t76*(t12*t154+pib2*t12*t140-pib1*t12*t150)-t60*t130*t163; #endif H(4,2) = t181*(qbw1*qbw4*4.0+qbw2*qbw3*4.0)+t182*t185*t213; -#ifdef FULLSTATE +#if STATESIZE==13 H(4,6) = -t181*(t203+t204-qbw4*t167*2.0+t36*t188-t34*t191+t37*t195)+t182*t185*t220; H(4,7) = t181*(t197+t198-qbw1*t176*2.0-t37*t188+t36*t195-t34*t202)-t182*t185*t225; H(4,8) = t181*(t210+t211-qbw3*t171*2.0+t37*t191+t34*t195+t36*t202)+t182*t185*t229; H(4,9) = t181*(t207+t208+t209+t34*t188+t36*t191-t37*t202)-t182*t185*t232; #endif H(5,2) = -t181*(t14*2.0+t15*2.0-t16*2.0-t17*2.0)+t182*t213*t217; -#ifdef FULLSTATE +#if STATESIZE==13 H(5,6) = -t181*(t207+t208+t209+t40*t188+t42*t191+t44*t195)+t182*t217*t220; H(5,7) = -t181*(t210+t211-qbw3*t171*2.0+t44*t188-t40*t195-t42*t202)-t182*t217*t225; H(5,8) = t181*(t197+t198-t226+t44*t191-t42*t195+t40*t202)+t182*t217*t229; @@ -1080,13 +1052,8 @@ Feature::inFOV ( ) } /* ----- end of method Feature::inFOV ----- */ bool -#ifdef FULLSTATE -Feature::isInlier(const measurement_t &z, const Matrix<double,13,13> &Pxx, - const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy, -#else -Feature::isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx, - const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, -#endif +Feature::isInlier(const measurement_t &z, const Matrix<double,STATESIZE,STATESIZE> &Pxx, + const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q, double thr) { Matrix<double,Dynamic,Dynamic,0,6,6> s; |