diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/body.cpp | 19 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/feature.cpp | 2 | ||||
| -rw-r--r-- | src/feature.h | 1 | ||||
| -rw-r--r-- | src/main.cpp | 55 | ||||
| -rw-r--r-- | src/main.h | 10 | ||||
| -rw-r--r-- | src/state.cpp | 25 | ||||
| -rw-r--r-- | src/state.h | 4 | ||||
| -rw-r--r-- | src/types.h | 2 |
9 files changed, 107 insertions, 13 deletions
diff --git a/src/body.cpp b/src/body.cpp index 819bd01..f909a39 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -366,15 +366,24 @@ Body::asVector ( const Matrix<double,STATESIZE,1> &m ) * Description: Returns the current quaternion estimate. *-------------------------------------------------------------------------------------- */ +#if STATESIZE==13 Quaterniond Body::qhat ( ) { -#if STATESIZE==13 Quaterniond qbw(X(9),X(6),X(7),X(8)); -#else - fprintf(stderr, "Quaternion is not being estimated, quitting.\n"); - exit(1); -#endif return qbw; } /* ----- end of method Body::qhat ----- */ +void +Body::qhat ( const Quaterniond &q ) +{ + X[6] = q.x(); + X[7] = q.y(); + X[8] = q.z(); + X[9] = q.w(); + return ; +} /* ----- end of method Body::qhat ----- */ + +#endif + + @@ -9,7 +9,6 @@ //#define DOCLAMP #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */ #define MINHEIGHT -0.3 /* */ -#define STATESIZE 13 using Eigen::Matrix; @@ -32,6 +31,7 @@ class Body #if STATESIZE==13 Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt); Quaterniond qhat(); + void qhat(const Quaterniond &q); #else Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt); #endif diff --git a/src/feature.cpp b/src/feature.cpp index 9c6bf84..85396e3 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -350,7 +350,7 @@ Feature::Fx ( double dt ) y2 = X[2]; Matrix<double,3,STATESIZE> F; F = Matrix<double,3,STATESIZE>::Zero(); - F.block<3,3>(0,3) << y0*y2,-y2, + F.block<3,3>(0,3) << y0*y2,-y2, 0., y1*y2, 0.,-y2, y2*y2, 0, 0; F *= dt; diff --git a/src/feature.h b/src/feature.h index 53eef2d..82a48d3 100644 --- a/src/feature.h +++ b/src/feature.h @@ -17,7 +17,6 @@ #define RANSAC_LI_THRESHOLD 4e-5 /* */ #define RANSAC_HI_THRESHOLD 5e-2 /* */ #define INITDEPTH -#define STATESIZE 13 /* */ using Eigen::Dynamic; using Eigen::Matrix; diff --git a/src/main.cpp b/src/main.cpp index 914aa26..11bf6dd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -50,11 +50,18 @@ aboveWater(const Quaterniond &q) * ===================================================================================== */ void +#if STATESIZE==13 +covCallback(const message &msg, State &mu) +#else covCallback(const message &msg, State &mu, const Quaterniond &q) +#endif { if (seencov && seenutm && seenpva) return; seencov=true; // Rotation from body to world +#if STATESIZE==13 + Quaterniond q = mu.qhat(); +#endif Matrix3d Rbw(q.toRotationMatrix()); // Rotation from ENU to NED Matrix3d Renuned; @@ -63,6 +70,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) pcov(2,2) *= 10; mu.position_covariance(pcov); mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw); +#if STATESIZE==13 + mu.quaternion_covariance(); +#endif } @@ -74,9 +84,16 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) * ===================================================================================== */ void +#if STATESIZE==13 +imgCallback(message &msg, State &mu, Camera &cam) +#else imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) +#endif { if (seenutm && seenpva && seencov) { +#if STATESIZE==13 + Quaterniond q = mu.qhat(); +#endif std::vector<measurement_t> z; #ifdef MEASURE_HEIGHT measurement_t height; @@ -112,7 +129,11 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) if (fclose(fin)==EOF) { err_sys("fclose"); } +#if STATESIZE==13 + mu.handle_measurements(z); +#else mu.handle_measurements(z,q); +#endif } return; @@ -126,7 +147,11 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) * ===================================================================================== */ void +#if STATESIZE==13 +imuCallback(const message &msg, State &mu, const timestamp dt) +#else imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt) +#endif { if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return; Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z); @@ -136,7 +161,11 @@ imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp ang -= ang_bias; double dtf = dt.secs+dt.nsecs*1e-9; +#if STATESIZE==13 + mu.motionModel(acc,ang,dtf); +#else mu.motionModel(acc,ang,q,dtf); +#endif } /* @@ -166,6 +195,9 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) if (!seenpva || !seencov || !seenutm) { Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up); mu.vel(R.transpose()*vw); +#if STATESIZE==13 + mu.qhat(q); +#endif seenpva=true; } return; @@ -179,8 +211,15 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) * ===================================================================================== */ void +#if STATESIZE==13 +utmCallback(const message &msg, State &mu) +#else utmCallback(const message &msg, State &mu, const Quaterniond &q) +#endif { +#if STATESIZE==13 + Quaterniond q = mu.qhat(); +#endif static int i=0; if ((!seenutm || !seencov) && seenpva) { seenutm=true; @@ -368,13 +407,21 @@ main(int argc, char **argv) switch ( msg.msg_type ) { case BESTUTM: +#if STATESIZE==13 + utmCallback(msg, mu); +#else utmCallback(msg, mu,qbw); +#endif printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); mu.unicsv(); break; case IMG: +#if STATESIZE==13 + imgCallback(msg, mu, cam); +#else imgCallback(msg, mu, cam, qbw); +#endif break; case INSPVAS: @@ -384,7 +431,11 @@ main(int argc, char **argv) case RAWIMUS: if (i++%DOWNSAMPLE==0) { dt = update_dt(msg.stamp, &t_old); +#if STATESIZE==13 + imuCallback(msg, mu, dt); +#else imuCallback(msg, mu, qbw, dt); +#endif if (seenutm && seencov && seenpva) { #ifdef ROS_PUBLISH geometry_msgs::PoseStamped msg; @@ -397,7 +448,11 @@ main(int argc, char **argv) break; case INSCOVS: +#if STATESIZE==13 + covCallback(msg, mu); +#else covCallback(msg, mu, qbw); +#endif break; default: @@ -40,7 +40,6 @@ #define DOWNSAMPLE 1 /* */ #define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */ #define MEASURE_HEIGHT - #if ROS_PUBLISH #include <ros/ros.h> #include <ros/geometry_msgs.h> @@ -55,11 +54,18 @@ double aboveWater(const Quaterniond &q); int parseLine(char *line, message *msg); timestamp update_dt(const timestamp t, timestamp *t_old); +#if STATESIZE==13 +void covCallback(const message &msg, State &mu); +void imgCallback(message &msg, State &mu, Camera &cam); +void imuCallback(const message &msg, State &mu, const timestamp dt); +void utmCallback(const message &msg, State &mu); +#else void covCallback(const message &msg, State &mu, const Quaterniond &q); void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q); void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt); -void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q); void utmCallback(const message &msg, State &mu, const Quaterniond &q); +#endif +void pvaCallback(const message &msg, Matrix<double,STATESIZE,1> &X, Quaterniond &q); #endif /* ----- #ifndef main_INC ----- */ diff --git a/src/state.cpp b/src/state.cpp index dfe41fa..e53ad29 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -464,7 +464,7 @@ void #if STATESIZE==13 State::addFeatures ( std::vector<measurement_t> &F, double z) #else -State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z); +State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z) #endif { #if STATESIZE==13 @@ -1008,3 +1008,26 @@ State::asVector ( const Matrix<double,Dynamic,1> &m ) return ; } /* ----- end of method State::asVector ----- */ +#if STATESIZE==13 +Quaterniond +State::qhat ( ) +{ + return body->qhat() ; +} /* ----- end of method State::qhat ----- */ + +void +State::qhat ( const Quaterniond &q ) +{ + body->qhat(q); + return ; +} /* ----- end of method State::qhat ----- */ + + +void +State::quaternion_covariance ( ) +{ + P.block<4,4>(6,6) = 1e-5*Matrix<double,4,4>::Identity(); + return ; +} /* ----- end of method State::quaternion_covariance ----- */ + +#endif diff --git a/src/state.h b/src/state.h index f330b67..b04ae9c 100644 --- a/src/state.h +++ b/src/state.h @@ -15,7 +15,6 @@ //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ //#define INLIERTEST /* */ -#define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -48,6 +47,7 @@ class State MatrixXd F(const Vector3d &w, double dt); MatrixXd H ( const std::vector<measurement_t> &z ); MatrixXd blockSI( const std::vector<measurement_t> &z ); + Quaterniond qhat(); #else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); @@ -74,6 +74,8 @@ class State const std::vector<measurement_t> &z); void Pkk1 ( const Vector3d &ang, const double dt); void ransacUpdate(std::vector<measurement_t> &z); + void quaternion_covariance(); + void qhat(const Quaterniond &q); #else void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, diff --git a/src/types.h b/src/types.h index c284d4a..3c1a962 100644 --- a/src/types.h +++ b/src/types.h @@ -4,7 +4,7 @@ #include <Eigen/Dense> #define MAXLINE 8192 #define MAXFILENAME 1024 - +#define STATESIZE 9 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ using Eigen::Matrix; using Eigen::Matrix3d; using Eigen::Quaterniond; |