diff options
| -rw-r--r-- | src/body.cpp | 9 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/feature.cpp | 11 | ||||
| -rw-r--r-- | src/feature.h | 3 | ||||
| -rw-r--r-- | src/main.cpp | 13 | ||||
| -rw-r--r-- | src/state.cpp | 9 | ||||
| -rw-r--r-- | src/state.h | 2 | ||||
| -rw-r--r-- | src/types.h | 1 |
8 files changed, 32 insertions, 18 deletions
diff --git a/src/body.cpp b/src/body.cpp index 3000eeb..e4024ae 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -343,16 +343,19 @@ Body::skewSymmetric ( const Vector3d &x ) } /* ----- end of method Body::skewSymmetric ----- */ void -Body::unicsv ( ) +Body::unicsv ( const double time ) { +#ifdef FEATUREMAP + printf("0,"); +#endif #if STATESIZE==13 - printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c, + printf("%f,%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", time,utm_i, utm_c, X[0], X[1], -X[2], X[3], X[4], X[5], X[6], X[7], X[8],X[9], X[10],X[11],X[12]); #else - printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c, + printf("%f,%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", time,utm_i, utm_c, X[0], X[1], -X[2], X[3], X[4], X[5], X[6], X[7], X[8]); @@ -74,7 +74,7 @@ class Body Matrix<double,3,3> skewSymmetric(const Vector3d &x); Matrix<double,4,4> omega(const Vector3d &x); Matrix<double,4,3> qomega(const Quaterniond &q); - void unicsv(); + void unicsv(const double time); protected: /* ==================== METHODS ======================================= */ diff --git a/src/feature.cpp b/src/feature.cpp index 4db6a29..94183ab 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -993,7 +993,7 @@ Feature::P0 ( measurement_type t ) *-------------------------------------------------------------------------------------- */ UTM -Feature::utm ( Vector3d &xbw, Quaterniond &q ) +Feature::utm ( const Vector3d &xbw, const Quaterniond &q ) { UTM utm; Vector3d xib,xiw; @@ -1159,3 +1159,12 @@ Feature::since ( int N ) const return (lastseen<N) ; } /* ----- end of method Feature::since ----- */ +void +Feature::unicsv(const double time, const Vector3d &xbw, const Quaterniond &qbw) +{ + UTM feature_utm = utm(xbw,qbw); + printf("%d,%f,%d,%c,%f,%f,%f,0,0,0,0,0,0,0\n", id(),time,feature_utm.zone_i, + feature_utm.zone_c, feature_utm.northing, feature_utm.easting, + feature_utm.up); +} + diff --git a/src/feature.h b/src/feature.h index 7cf38aa..9eb7eaa 100644 --- a/src/feature.h +++ b/src/feature.h @@ -52,10 +52,11 @@ class Feature int id(); Matrix<double,3,3> P0(measurement_type t); Vector3d asVector(); - UTM utm(Vector3d &xbw, Quaterniond &q); + UTM utm(const Vector3d &xbw, const Quaterniond &q); Mat patch(); Mat warpedPatch(const Camera &cam, const Quaterniond &q1); Mat reflectedPatch(const Camera &cam, const Quaterniond &q1); + void unicsv(const double time, const Vector3d &xbw, const Quaterniond &qbw) ; /* ==================== MUTATORS ======================================= */ void asVector(const Vector3d &m); diff --git a/src/main.cpp b/src/main.cpp index ce99968..f1bcd30 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -463,18 +463,14 @@ main(int argc, char **argv) // Open camera YAML Camera cam(argv[1]); -#ifdef ROS_PUBLISH - // Create ROS publisher - ros::init(argc, argv, "refslam"); - ros::NodeHandle n; - ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100); -#else /* ----- not ROS_PUBLISH ----- */ // Print unicsv header +#ifdef FEATUREMAP + printf("Name,"); +#endif #if STATESIZE==13 printf("GPS-stamp,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); #else printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); -#endif #endif /* ----- not ROS_PUBLISH ----- */ // read params file FILE *pfin; @@ -531,8 +527,7 @@ main(int argc, char **argv) #endif //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); if (seenutm && seenpva && seencov) { - printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); - mu.unicsv(); + mu.unicsv(msg.stamp.secs+msg.stamp.nsecs*1e-9); } break; diff --git a/src/state.cpp b/src/state.cpp index c44aafe..3f2d3f4 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -803,10 +803,15 @@ State::velocity_covariance ( const Matrix<double,3,3> &p ) } /* ----- end of method State::velocity_covariance ----- */ void -State::unicsv ( ) +State::unicsv ( const double time ) { - body->unicsv(); + body->unicsv(time); printf(",%d\n", features.size()); +#ifdef FEATUREMAP + for (auto feature:features ) { + feature->unicsv(time,body->ned(),body->qhat()); + } +#endif return ; } /* ----- end of method State::unicsv ----- */ diff --git a/src/state.h b/src/state.h index a92f123..14f94bd 100644 --- a/src/state.h +++ b/src/state.h @@ -110,7 +110,7 @@ class State #else Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z, const Quaterniond &q); #endif - void unicsv(); + void unicsv(const double time); protected: /* ==================== METHODS ======================================= */ diff --git a/src/types.h b/src/types.h index 29271d4..1f02bfc 100644 --- a/src/types.h +++ b/src/types.h @@ -9,6 +9,7 @@ #define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ #define INLIER_THRESHOLD 24. #define PATCHSIZE 51 /* must be odd */ +#define FEATUREMAP using Eigen::Matrix; using Eigen::Matrix3d; using Eigen::Quaterniond; |