From 0663fb75c6bc42ccd093bed66820e92541ed419c Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sun, 28 Jan 2018 11:45:53 -0500 Subject: Add define for FEATUREMAP This will print the feature information in the unicsv so that the mapper Makefile can create a feature map. --- src/body.cpp | 9 ++++++--- src/body.h | 2 +- src/feature.cpp | 11 ++++++++++- src/feature.h | 3 ++- src/main.cpp | 13 ++++--------- src/state.cpp | 9 +++++++-- src/state.h | 2 +- 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]); diff --git a/src/body.h b/src/body.h index 6675d5c..5080d91 100644 --- a/src/body.h +++ b/src/body.h @@ -74,7 +74,7 @@ class Body Matrix skewSymmetric(const Vector3d &x); Matrix omega(const Vector3d &x); Matrix 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 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("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 &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 innovation( const vector &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; -- cgit v1.1