summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/feature.cpp67
-rw-r--r--src/feature.h26
2 files changed, 24 insertions, 69 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;
diff --git a/src/feature.h b/src/feature.h
index 56e0835..53eef2d 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -17,8 +17,7 @@
#define RANSAC_LI_THRESHOLD 4e-5 /* */
#define RANSAC_HI_THRESHOLD 5e-2 /* */
#define INITDEPTH
-#define FULLSTATE
-
+#define STATESIZE 13 /* */
using Eigen::Dynamic;
using Eigen::Matrix;
@@ -59,25 +58,14 @@ class Feature
/* ==================== OPERATORS ======================================= */
bool sane();
-#ifdef FULLSTATE
- bool isInlier(const measurement_t &z, const Matrix<double,13,13> &Pxx,
- const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy,
- const Vector3d &pos, const Quaterniond &q, double thr);
- Matrix<double,3,13> Fx( double dt );
- Matrix<double,6,13> Hx( const Vector3d &pos, const Quaterniond &q);
- Matrix<double,6,6> 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
- bool isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx,
- const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy,
+ bool 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,3,9> Fx( double dt );
- Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
- Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx,
- const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy,
+ Matrix<double,3,STATESIZE> Fx( double dt );
+ Matrix<double,6,STATESIZE> Hx( const Vector3d &pos, const Quaterniond &q);
+ Matrix<double,6,6> 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);
-#endif
bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q);
bool inFOV();
Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,