diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/camera.h | 4 | ||||
| -rw-r--r-- | src/feature.cpp | 8 | ||||
| -rw-r--r-- | src/feature.h | 1 | ||||
| -rw-r--r-- | src/main.cpp | 6 | ||||
| -rw-r--r-- | src/main.h | 21 | ||||
| -rw-r--r-- | src/state.h | 3 |
6 files changed, 24 insertions, 19 deletions
diff --git a/src/camera.h b/src/camera.h index a08eb0c..ea006ec 100644 --- a/src/camera.h +++ b/src/camera.h @@ -5,8 +5,8 @@ #include <yaml-cpp/yaml.h> #define BINNING 0.5 // set the binning factor -#define YAWCORRECT 7.5 -//#define DOYAWCORRECT +#define YAWCORRECT 7.0 +#define DOYAWCORRECT using Eigen::Matrix; using Eigen::Vector4d; using Eigen::Vector3d; diff --git a/src/feature.cpp b/src/feature.cpp index caa56f0..a75fdf0 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -54,6 +54,8 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw, pib[1] = xib[2]; pib[2] = RHO_0; + X = pib; + X0 = X; xb0w = xbw; q0 = q; @@ -531,11 +533,11 @@ Feature::P0 ( measurement_type t ) double p0 = FEATURECOVX; double p1 = FEATURECOVY; double p2; - if (t==REFLECTION) { +#ifdef INITDEPTH p2 = FEATURECOVRHO; - } else { +#else p2 = FEATURECOVRHO_MONO; - } +#endif P << p0, 0., 0., 0., p1, 0., 0., 0., p2; diff --git a/src/feature.h b/src/feature.h index 2754f89..db4f05e 100644 --- a/src/feature.h +++ b/src/feature.h @@ -16,6 +16,7 @@ #define INLIER_THRESHOLD 5.9915 /* */ #define RANSAC_LI_THRESHOLD 4e-5 /* */ #define RANSAC_HI_THRESHOLD 5e-2 /* */ +#define INITDEPTH using Eigen::Dynamic; diff --git a/src/main.cpp b/src/main.cpp index c7ed177..914aa26 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -357,7 +357,7 @@ main(int argc, char **argv) ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100); #else /* ----- not ROS_PUBLISH ----- */ // Print unicsv header - printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); + printf("GPS,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); #endif /* ----- not ROS_PUBLISH ----- */ // Read sensors from file int i=0; @@ -369,6 +369,8 @@ main(int argc, char **argv) switch ( msg.msg_type ) { case BESTUTM: utmCallback(msg, mu,qbw); + printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); + mu.unicsv(); break; case IMG: @@ -388,7 +390,7 @@ main(int argc, char **argv) geometry_msgs::PoseStamped msg; ros::spinOnce(); #else - mu.unicsv(); + //mu.unicsv(); #endif } } @@ -22,23 +22,24 @@ #define ANGBIASY 6.984255690000021506e-03 #define ANGBIASZ -1.418145565750002614e-03 -#define ACCBIASX 0.95*-0.03713532 -#define ACCBIASY 0.65*0.01465135 -#define ACCBIASZ -1*-0.00709229 +//#define ACCBIASX 0.95*-0.03713532 +//#define ACCBIASY 0.65*0.01465135 +//#define ACCBIASZ -1*-0.00709229 //#define ACCBIASX 0.99*-0.03713532 //#define ACCBIASY 0.2*0.01465135 //#define ACCBIASZ -1*-0.00709229 -//#define ACCBIASX -0.03713532 -//#define ACCBIASY 0.01465135 -//#define ACCBIASZ -1*-0.00709229 +#define ACCBIASX -0.03713532 +#define ACCBIASY 0.01465135 +#define ACCBIASZ -1*-0.00709229 -#define CANOECENTER 0.65 /* center of gravity of canoe */ -#define CANOEHEIGHT -0.55 +#define CANOECENTER 0.88 /* center of gravity of canoe */ +#define CANOEHEIGHT -0.50 +//#define CANOEHEIGHT -0.75 #define DOWNSAMPLE 1 /* */ -//#define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */ -//#define MEASURE_HEIGHT +#define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */ +#define MEASURE_HEIGHT #if ROS_PUBLISH #include <ros/ros.h> diff --git a/src/state.h b/src/state.h index 19e807b..a7f7df5 100644 --- a/src/state.h +++ b/src/state.h @@ -13,8 +13,7 @@ #define MAXFEATURES 50 #define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once -//#define DORANSAC /* */ -//#define INITDEPTH /* */ +#define DORANSAC /* */ //#define INLIERTEST /* */ using Eigen::Dynamic; |