diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 24 |
1 files changed, 24 insertions, 0 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index a75fdf0..c9cdda1 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -130,10 +130,19 @@ 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, + 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,3> hy; hx = Hx( pos, q); hy = Hy( pos, q); @@ -341,17 +350,28 @@ Feature::x2p ( const Vector3d &x ) * body. *-------------------------------------------------------------------------------------- */ +#ifdef FULLSTATE +Matrix<double,3,13> +#else Matrix<double,3,9> +#endif 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 F *= dt; return F; } /* ----- end of method Feature::Fx ----- */ @@ -388,7 +408,11 @@ 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 Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) { double xbw1,xbw2,xbw3; |