Add file I/O for VioDataset

wave_utils/include/wave/utils/data.hpp

@@ -9,6 +9,7 @@
 
 #include <iostream>
 #include <fstream>
+#include <sstream>
 #include <vector>
 
 #include "wave/utils/math.hpp"
@@ -57,17 +58,27 @@ int mat2csv(std::string file_path, MatX data);
  * The entries must be separated by whitespace, and be in row-major order.
  */
 template <int Rows, int Cols>
-Eigen::Matrix<double, Rows, Cols> matrixFromStream(std::istream &in_stream) {
+inline Eigen::Matrix<double, Rows, Cols> matrixFromStream(std::istream &in) {
     Eigen::Matrix<double, Rows, Cols> mat;
     for (auto i = 0; i < Rows; ++i) {
         for (auto j = 0; j < Cols; ++j) {
-            in_stream >> mat(i, j);
+            in >> mat(i, j);
         }
     }
 
     return mat;
 }
 
+/** Reads a matrix from a string
+ *
+ * The entries must be separated by whitespace, and be in row-major order.
+ */
+template <int Rows, int Cols>
+inline Eigen::Matrix<double, Rows, Cols> matrixFromString(std::string &in) {
+    auto iss = std::istringstream{in};
+    return matrixFromStream<Rows, Cols>(iss);
+};
+
 /** @} group utils */
 }  // namespace wave
 

wave_vision/include/wave/vision/dataset/VioDataset.hpp

@@ -71,15 +71,78 @@ struct VioDataset {
 
     // Calibration
 
-    /** Camera intrinsic matrix */
-    Mat3 camera_K;
+    /** Camera object containing intrinsic matrix */
+    VoTestCamera camera;
     /** Camera transformation from IMU frame (expressed in IMU frame) */
     Vec3 I_p_IC;
     Rotation R_IC;
 
+    /** Geographic datum (arbitrary for now, just used for output) */
+    Vec3 lla_datum{43.472981, -80.540092, 0};
+
+    /* An arbitrary start time, used for output timestamps.txt
+     * @todo maybe we will care about absolute time in future
+     */
+    std::chrono::system_clock::time_point start_time =
+      std::chrono::system_clock::now();
+
     /** Ground truth 3D landmark positions in the world frame. */
     LandmarkMap landmarks;
 
+    // I/O methods
+
+    /**
+     * Writes dataset to the given directory. A format similar to kitti is used.
+     */
+    void outputToDirectory(const std::string &output_dir) const;
+
+    /**
+     * Write calibration files to the given directory.
+     *
+     * To be compatible with kitti this produces 3 files,
+     * - calib_cam_to_cam.txt
+     * - calib_imu_to_velo.txt
+     * - calib_velo_to_cam.txt
+     *
+     * though the transformation imu_to_velo is identity for now.
+     */
+    void outputCalibration(const std::string &output_dir) const;
+
+    /** Writes pose and inertial measurements to the given directory
+     *
+     * The output is in kitti format. It includes:
+     * - A file `timestamps.txt` with one row for each time step
+     * - A directory `data` containing a text file for each time step. The files
+     * are numbered with the format 0000000000.txt.
+     *
+     * Each data file has one row of information matching kitti's
+     * `dataformat.txt` (not included here). When writing, we fill only the
+     * fields `lat` `lon` `alt` `roll` `pitch` `yaw` (with ground truth), and
+     * `vf` `vl` `vu` `wf` `wl` `wu` (with simulated imu measurements).
+     */
+    void outputPoses(const std::string &output_dir) const;
+
+    /** Write feature observations to the given directory.
+     *
+     * The output is placed in a directory called `image_00` and includes
+     *
+     * - A file `timestamps.txt` with one row for each time step - A directory
+     * `features` containing a text file for each time step. The files are
+     * numbered with the format 0000000000.txt.
+     *
+     * Each observations file has one row for each feature measurement. Each row
+     * has three space-separated fields: landmark id, pixels x, and pixels y.
+     */
+    void outputObserved(const std::string &output_dir) const;
+
+    /** Reads a dataset from files in the given directory.
+     *
+     * The format must be the same as that created by outputToDirectory(), or
+     * a kitti dataset.
+     */
+    static VioDataset loadFromDirectory(const std::string &input_dir);
+
+
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 

wave_vision/include/wave/vision/dataset/VoDataset.hpp

@@ -108,7 +108,7 @@ struct VoDataset {
      */
     void outputObserved(const std::string &output_dir);
 
-    /** Writes robot state group truth to the given file.
+    /** Writes robot state ground truth to the given file.
      *
      * The output is space-separated. Each row contains 8 values: the time, the
      * 3d position (x, y, z), and the quaternion orientation (x, y, z, w).

wave_vision/include/wave/vision/dataset/VoTestCamera.hpp

@@ -46,6 +46,20 @@ class VoTestCamera {
     VoTestCamera(int image_width, int image_height, Mat3 K, double hz)
         : image_width{image_width}, image_height{image_height}, K{K}, hz{hz} {}
 
+    // Helper getters
+    double fx() const {
+        return K(0, 0);
+    }
+    double fy() const {
+        return K(1, 1);
+    }
+    double cx() const {
+        return K(0, 2);
+    }
+    double cy() const {
+        return K(1, 2);
+    }
+
     /** Check whether camera is triggered at this time-step
      * @returns boolean to denote true or false
      */