diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/body.cpp | 23 | ||||
| -rw-r--r-- | src/body.h | 6 | ||||
| -rw-r--r-- | src/camera.h | 3 | ||||
| -rw-r--r-- | src/main.cpp | 4 | ||||
| -rw-r--r-- | src/main.h | 1 | ||||
| -rw-r--r-- | src/state.cpp | 11 | ||||
| -rw-r--r-- | src/state.h | 4 |
7 files changed, 40 insertions, 12 deletions
diff --git a/src/body.cpp b/src/body.cpp index 1e1f57d..1b07361 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -17,6 +17,19 @@ */ #include "body.h" +void +Body::clamp() +{ +#ifdef DOCLAMP + // Constrain the height + if (X[2]<MAXHEIGHT) { + X[2]=MAXHEIGHT; + } else if (X[2]>MINHEIGHT) { + X[2]=MINHEIGHT; + } +#endif /* ----- DOCLAMP ----- */ +} + /* *-------------------------------------------------------------------------------------- * Class: Body @@ -28,12 +41,7 @@ void Body::dx ( const Matrix<double,9,1> &del ) { X += del; - // Constrain the height - if (X[2]<-1.5) { - X[2]=-1.; - } else if (X[2]>-0.3) { - X[2]=-0.3; - } + clamp(); return ; } /* ----- end of method Body::dx ----- */ @@ -195,8 +203,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; - if (X[2]>-0.3) X[2]=-0.3; - if (X[2]<-1.3) X[2]=-1.3; + clamp(); return ; } /* ----- end of method Body::motionModel ----- */ @@ -6,6 +6,11 @@ #define R_HEIGHT 2.5e-3 /* measurement noise of height measurement */ #define IMU_NOISE 800e-6 /* IMU process noise */ #define IMU_RANDOMWALK 50e-6 /* Bias process noise */ +//#define DOCLAMP +#define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */ +#define MINHEIGHT -0.3 /* */ + + using Eigen::Matrix; using Eigen::Vector3d; using Eigen::Quaterniond; @@ -30,6 +35,7 @@ class Body /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); void asVector(const Matrix<double,9,1> &m); + void clamp(); void pos( const UTM &utm); void dx( const Matrix<double,9,1> &del); void vel(const Matrix<double,3,1> &v); diff --git a/src/camera.h b/src/camera.h index 599fa23..7dd83e7 100644 --- a/src/camera.h +++ b/src/camera.h @@ -5,7 +5,8 @@ #include <yaml-cpp/yaml.h> #define BINNING 0.5 // set the binning factor -#define YAWCORRECT 7.5 +//#define YAWCORRECT 7.5 +#define YAWCORRECT 0.0 using Eigen::Matrix; using Eigen::Vector4d; using Eigen::Vector3d; diff --git a/src/main.cpp b/src/main.cpp index 246f73d..1de5b29 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,8 +31,12 @@ double aboveWater(const Quaterniond &q) { Vector3d tip; +#ifdef DOHEIGHT tip << CANOECENTER*3.43, 0, CANOEHEIGHT; tip = q._transformVector(tip); +#else /* ----- not DOHEIGHT ----- */ + tip[2] = CANOEHEIGHT; +#endif /* ----- not DOHEIGHT ----- */ return tip[2]; } @@ -37,6 +37,7 @@ #define CANOECENTER 0.65 /* center of gravity of canoe */ #define CANOEHEIGHT -0.55 #define DOWNSAMPLE 1 /* */ +#define DOHEIGHT /* */ #if ROS_PUBLISH #include <ros/ros.h> diff --git a/src/state.cpp b/src/state.cpp index a9a55ae..472ee66 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -290,7 +290,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni } else if (exists(i->id)) { Feature *ft; ft = featureById(i->id); +#ifdef INLIERTEST if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD)) +#endif { featuresToUpdate.push_back(*i); } @@ -319,13 +321,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni } } if (featuresToUpdate.size()>1) { +#ifdef DORANSAC ransacUpdate(featuresToUpdate,q); - } else if (featuresToUpdate.size()>1) { +#else /* ----- not DORANSAC ----- */ MatrixXd h; h = H(q,featuresToUpdate); MatrixXd S; S = h*P*h.transpose() + R(featuresToUpdate); kalmanUpdate(h,S,featuresToUpdate,q); +#endif /* ----- not DORANSAC ----- */ } addFeatures( featuresToAdd, q, zmeas); @@ -405,8 +409,11 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double // Create feature Feature *f; if (i->z_type==REFLECTION) { +#ifdef INITDEPTH f = new Feature(i->id, i->source, i->reflection, pos, q, z); - //f = new Feature(i->id, i->source, pos, q); +#else /* ----- not INITDEPTH ----- */ + f = new Feature(i->id, i->source, pos, q); +#endif /* ----- not INITDEPTH ----- */ } else { f = new Feature(i->id, i->source, pos, q); } diff --git a/src/state.h b/src/state.h index 2383706..19e807b 100644 --- a/src/state.h +++ b/src/state.h @@ -13,7 +13,9 @@ #define MAXFEATURES 50 #define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once -#define DORANSAC /* */ +//#define DORANSAC /* */ +//#define INITDEPTH /* */ +//#define INLIERTEST /* */ using Eigen::Dynamic; using Eigen::Matrix; |