Add define for FEATUREMAP

This will print the feature information in the unicsv so that the mapper
Makefile can create a feature map.

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<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;