diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/feature.cpp | 68 | ||||
-rw-r--r-- | src/feature.h | 4 |
2 files changed, 70 insertions, 2 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index ad554a1..45e24e7 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -19,12 +19,78 @@ #include "feature.h" Matrix<double,6,1> -Feature::h ( ) +Feature::h ( const Vector3d &x, const Quaterniond &q ) { Matrix<double,6,1> h; + Vector3d xib, xib0; + Vector3d pib0; + + xib = p2x(X); + + // Predict initial view + xib0 = q0._transformVector(q._transformVector(xib) + x - xb0w); + pib0 = x2p(xib0); + + // Predict reflection view + Vector3d xpb, ppb; + Quaterniond qxpb, qrb; + const Quaterniond qrw(0,0,0,-1); // the reflecting plane in world frame + Quaterniond qxib(0,xib[0],xib[1],xib[2]); + + // Compute reflection plane in body frame + qrb = q.conjugate()*qrw; + // Reflect xib about qrb + qxpb = qrb*qxib*qrb; + xpb = qxpb.vec(); + ppb = x2p(xpb); + + h[0] = X[0]; + h[1] = X[1]; + h[2] = pib0[0]; + h[3] = pib0[1]; + h[4] = ppb[0]; + h[5] = ppb[1]; + return h; } /* ----- end of method Feature::h ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: p2x + * Description: Returns x, the point p in inverse depth frame in the Cartesian + * frame. + *-------------------------------------------------------------------------------------- + */ +Vector3d +Feature::p2x ( const Vector3d &p ) +{ + Vector3d x; + x[0] = 1./p[2]; + x[1] = p[0]/p[2]; + x[2] = p[1]/p[2]; + return x; +} /* ----- end of method Feature::p2x ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: x2p + * Description: Returns p, the point x in the Cartesian body frame in the + * inverse depth frame. + *-------------------------------------------------------------------------------------- + */ +Vector3d +Feature::x2p ( const Vector3d &x ) +{ + Vector3d p; + p[0] = x[1]/x[0]; + p[1] = x[2]/x[0]; + p[2] = 1./x[0]; + return p; +} /* ----- end of method Feature::x2p ----- */ + + Matrix<double,3,9> Feature::Fx ( ) diff --git a/src/feature.h b/src/feature.h index 077a617..2982276 100644 --- a/src/feature.h +++ b/src/feature.h @@ -28,7 +28,7 @@ class Feature Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang); Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); - Matrix<double,6,1> h(); + Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); /* void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); @@ -46,6 +46,8 @@ class Feature private: /* ==================== METHODS ======================================= */ + Vector3d p2x(const Vector3d &p); + Vector3d x2p(const Vector3d &x); /* ==================== DATA MEMBERS ======================================= */ Vector3d X; |