Add VioDataset class

CMakeLists.txt

@@ -50,7 +50,7 @@ ADD_DEPENDENCIES(wave_matching wave_utils)
 ADD_DEPENDENCIES(wave_optimization wave_utils)
 
 # libwave_vision.a
-ADD_DEPENDENCIES(wave_vision wave_utils)
+ADD_DEPENDENCIES(wave_vision wave_kinematics wave_geometry wave_utils)
 
 # libwave_benchmark.a
 ADD_DEPENDENCIES(wave_benchmark wave_utils)

wave_vision/CMakeLists.txt

@@ -15,27 +15,28 @@ INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIR})
 
 # LIBRARY
-SET(
-    WAVE_VISION_DEPS
-    yaml-cpp
-    ${OpenCV_LIBS}
-    ${Boost_LIBRARIES}
-    wave_kinematics
-    wave_utils
-)
 ADD_LIBRARY(
     wave_vision
     STATIC
     src/utils.cpp
     src/dataset/VoDataset.cpp
+    src/dataset/VioDataset.cpp
+    src/dataset/VioDatasetGenerator.cpp
     src/dataset/VoTestCamera.cpp
     src/detector/fast_detector.cpp
     src/detector/orb_detector.cpp
     src/descriptor/brisk_descriptor.cpp
     src/descriptor/orb_descriptor.cpp
     src/matcher/brute_force_matcher.cpp
 )
-TARGET_LINK_LIBRARIES(wave_vision ${WAVE_VISION_DEPS})
+TARGET_LINK_LIBRARIES(wave_vision
+    wave_kinematics
+    wave_geometry
+    wave_utils
+    yaml-cpp
+    ${OpenCV_LIBS}
+    ${Boost_LIBRARIES}
+    )
 
 # UNIT TESTS
 WAVE_ADD_TEST(${PROJECT_NAME}_tests
@@ -45,10 +46,14 @@ WAVE_ADD_TEST(${PROJECT_NAME}_tests
               tests/descriptor_tests/orb_tests.cpp
               tests/matcher_tests/brute_force_tests.cpp
               tests/tracker_tests/tracker_tests.cpp
-              tests/dataset_tests/vo_dataset_tests.cpp
               tests/utils_tests.cpp)
 TARGET_LINK_LIBRARIES(${PROJECT_NAME}_tests ${PROJECT_NAME})
 
+WAVE_ADD_TEST(${PROJECT_NAME}_dataset_tests
+    tests/dataset_tests/vo_dataset_tests.cpp
+    tests/dataset_tests/vio_dataset_tests.cpp)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_dataset_tests ${PROJECT_NAME})
+
 WAVE_ADD_TEST(
     ${PROJECT_NAME}_viz_tests
     tests/viz_tests/detector_viz_tests.cpp
@@ -57,12 +62,7 @@ WAVE_ADD_TEST(
     tests/viz_tests/tracker_viz_tests.cpp
     DISABLED # Requires display to run
 )
-TARGET_LINK_LIBRARIES(
-    ${PROJECT_NAME}_viz_tests
-    ${PROJECT_NAME}
-    wave_utils
-    ${Boost_LIBRARIES}
-)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_viz_tests ${PROJECT_NAME})
 
 # COPY TEST DATA
 FILE(COPY tests/data tests/config DESTINATION ${PROJECT_BINARY_DIR}/tests)

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

@@ -0,0 +1,88 @@
+/**
+ * @file
+ * @ingroup vision
+ */
+#ifndef WAVE_VISION_VIODATASET_HPP
+#define WAVE_VISION_VIODATASET_HPP
+
+#include "wave/utils/utils.hpp"
+#include "wave/containers/measurement_container.hpp"
+#include "wave/containers/landmark_measurement_container.hpp"
+#include "wave/containers/measurement.hpp"
+#include "wave/containers/landmark_measurement.hpp"
+#include "wave/geometry/rotation.hpp"
+#include "wave/vision/dataset/VoTestCamera.hpp"
+
+namespace wave {
+/** @addtogroup vision
+ *  @{ */
+
+/**
+ * A set of pre-recorded visual and inertial measurements.
+ * The data could be synthetic or real.
+ *
+ * This includes:
+ * - GPS position (can be used as ground truth)
+ * - 3D landmark positions in the world (if available, as ground truth)
+ * - Landmark measurements in the image frame
+ * - Inertial measurements in the IMU frame
+ * - Intrinsic and extrinsic calibration
+ *
+ * @todo In this version, inertial measurements include velocity, not
+ * acceleration. This may change.
+ * @todo Currently only one camera is included.
+ */
+struct VioDataset {
+    // Types
+    // @todo - unify with other definitions
+
+    /** IMU measured value - note this version includes velocity.
+     * The affixes indicate it is the motion of the IMU with respect to the
+     * the world frame, expressed in the IMU frame */
+    struct ImuValue {
+        Vec3 I_vel_GI;
+        Vec3 I_ang_vel_GI;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+
+    /** Pose value - either synthetic ground truth or from GPS and pre-processed
+     *  IMU data. The affixes indicate it is the motion of the IMU with respect
+     *  to the world frame, expressed in the world frame */
+    struct PoseValue {
+        Vec3 G_p_GI;
+        Rotation R_GI;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+
+    enum class ImuSensor { Imu };
+    enum class PoseSensor { Pose };
+    enum class Camera { Left };
+
+    using ObsContainer =
+      LandmarkMeasurementContainer<LandmarkMeasurement<Camera>>;
+    using PoseContainer =
+      MeasurementContainer<Measurement<PoseValue, PoseSensor>>;
+    using ImuContainer = MeasurementContainer<Measurement<ImuValue, ImuSensor>>;
+
+    // Measurements
+    PoseContainer poses;
+    ImuContainer imu_measurements;
+    ObsContainer feature_measurements;
+
+    // Calibration
+
+    /** Camera intrinsic matrix */
+    Mat3 camera_K;
+    /** Camera transformation from IMU frame (expressed in IMU frame) */
+    Vec3 I_p_IC;
+    Rotation R_IC;
+
+    /** Ground truth 3D landmark positions in the world frame. */
+    LandmarkMap landmarks;
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/** @} end of group */
+}  // namespace wave
+#endif  // WAVE_VISION_VIODATASET_HPP

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

@@ -0,0 +1,40 @@
+/**
+ * @file
+ * @ingroup vision
+ */
+#ifndef WAVE_VISION_VIODATASETGENERATOR_HPP
+#define WAVE_VISION_VIODATASETGENERATOR_HPP
+
+#include "wave/vision/dataset/VioDataset.hpp"
+#include "wave/vision/dataset/VoDataset.hpp"
+#include "wave/kinematics/two_wheel.hpp"
+
+namespace wave {
+/** @addtogroup vision
+ *  @{ */
+/**
+ * Synthetic VIO dataset generator.
+ *
+ * Currently uses the interface and parameters of VoDatasetGenerator,
+ * just defining another generate() method.
+ */
+class VioDatasetGenerator : private VoDatasetGenerator {
+ public:
+    // Inherit base class constructors
+    using VoDatasetGenerator::VoDatasetGenerator;
+
+    // Re-use base class `configure`
+    using VoDatasetGenerator::configure;
+
+    /** Simulates a two wheel robot moving in circle in a world of randomly
+     * generated 3D point landmarks, and generates a VioDataset object.
+     *
+     * A measurement including robot pose is stored at every timestep (with an
+     * arbitrary dt), but feature observations are only made at some timesteps.
+     */
+    VioDataset generate();
+};
+
+/** @} end of group */
+}  // namespace wave
+#endif  // WAVE_VISION_VIODATASETGENERATOR_HPP

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

@@ -44,8 +44,7 @@ struct VoInstant {
  * - 3D Features observed in image frame on the two wheel robot
  */
 struct VoDataset {
-    /** Ground truth 3D world features, where each column represents a feature
-     * and each row represents the landmark position in x, y, z (NWU)  */
+    /** Ground truth 3D landmark positions in the world frame. */
     LandmarkMap landmarks;
 
     /** For each time step, a set of measurements */

wave_vision/src/dataset/VioDataset.cpp

@@ -0,0 +1,3 @@
+#include "wave/vision/dataset/VioDataset.hpp"
+
+namespace wave {}  // namespace wave

wave_vision/src/dataset/VioDatasetGenerator.cpp

@@ -0,0 +1,132 @@
+#include "wave/vision/dataset/VioDatasetGenerator.hpp"
+
+namespace wave {
+
+// @todo another commit (not in master yet) makes this definition elsewhere
+using TimePoint = std::chrono::steady_clock::time_point;
+
+// Helper functions used only in this file
+namespace {
+// Add measurements from VoDataset's landmark measurement map to a container
+void addVoMeasurementsToContainer(
+  ImageNum i,
+  TimePoint time_point,
+  VioDataset::Camera sensor_id,
+  const std::vector<LandmarkObservation> observations,
+  VioDataset::ObsContainer &container) {
+    for (const auto &obs : observations) {
+        const auto &landmark_id = obs.first;
+        const auto &measurement = obs.second;
+        container.emplace(time_point, sensor_id, landmark_id, i, measurement);
+    }
+}
+
+// Convert one VoDataset state to a PoseValue
+VioDataset::PoseValue poseFromVoState(const VoInstant &state) {
+    auto pose = VioDataset::PoseValue{};
+    pose.G_p_GI = state.robot_G_p_GB;
+    pose.R_GI = Rotation{}.setFromMatrix(state.robot_q_GB.matrix());
+    return pose;
+}
+
+// Get a  TimePoint given a start time and duration in seconds
+TimePoint timePointAfterStartTime(TimePoint time_start, double dt_seconds) {
+    const auto d = std::chrono::duration<double>(dt_seconds);
+    return time_start + std::chrono::duration_cast<TimePoint::duration>(d);
+}
+
+// Handle one VoDataset state, adding measurements to containers
+void addVoStateToDataset(ImageNum i,
+                         TimePoint time_start,
+                         const VoInstant &state,
+                         VioDataset &dataset) {
+    const auto time_point = timePointAfterStartTime(time_start, state.time);
+
+    auto pose = poseFromVoState(state);
+
+    dataset.poses.emplace(time_point, VioDataset::PoseSensor::Pose, pose);
+
+    // Reorganize camera measurements
+    addVoMeasurementsToContainer(i,
+                                 time_point,
+                                 VioDataset::Camera::Left,
+                                 state.features_observed,
+                                 dataset.feature_measurements);
+}
+
+// Produce imu measurements from two poses
+VioDataset::ImuValue imuFromPoses(const VioDataset::PoseValue &p1,
+                                  const VioDataset::PoseValue &p2,
+                                  double dt_seconds) {
+    VioDataset::ImuValue res;
+
+    // Convert motion expressed in world frame to imu frame
+    // @todo this is what happens when there is no Rotation.inverse() method
+    auto R_I1_G = p1.R_GI;  // Not correct until the next line!
+    R_I1_G.invert();        // OK, now R_IG == R_GI.inverse().
+    auto R_I2_G = p2.R_GI;  // Not correct until the next line!
+    R_I2_G.invert();        // OK, now R_IG == R_GI.inverse().
+
+    const auto I_p_I1_G = R_I1_G.rotate(p1.G_p_GI);
+    const auto I_p_I2_G = R_I2_G.rotate(p2.G_p_GI);
+
+    // Angular velocity
+    res.I_ang_vel_GI = R_I2_G.manifoldMinus(R_I1_G) / dt_seconds;
+
+    // Linear velocity
+    res.I_vel_GI = (I_p_I2_G - I_p_I1_G) / dt_seconds;
+
+    return res;
+}
+
+// Produce imu measurements from two VoDataset states, and add them to container
+void addImuStatesToDataset(TimePoint time_start,
+                           const VoInstant &state1,
+                           const VoInstant &state2,
+                           VioDataset &dataset) {
+    const auto p1 = poseFromVoState(state1);
+    const auto p2 = poseFromVoState(state2);
+    const auto dt_seconds = state1.time - state2.time;
+    const auto imu_value = imuFromPoses(p1, p2, dt_seconds);
+    const auto time_point = timePointAfterStartTime(time_start, state1.time);
+    dataset.imu_measurements.emplace(
+      time_point, VioDataset::ImuSensor::Imu, imu_value);
+}
+
+}  // namespace
+
+VioDataset VioDatasetGenerator::generate() {
+    VioDataset dataset;
+
+    // Use the existing VO generator for now, just change the format
+    VoDataset vo = VoDatasetGenerator::generate();
+
+    dataset.landmarks = std::move(vo.landmarks);
+
+    // Choose arbitrary start time
+    auto time_start = std::chrono::steady_clock::now();
+
+    // Handle measurements
+    for (auto i = 0u; i < vo.states.size(); ++i) {
+        addVoStateToDataset(i, time_start, vo.states[i], dataset);
+
+        // Calculate imu measurements
+        // (the last one is missing)
+        if (i > 0) {
+            addImuStatesToDataset(
+              time_start, vo.states[i - 1], vo.states[i], dataset);
+        }
+    }
+
+    // Handle calibration
+    dataset.camera_K = vo.camera_K;
+
+    // In VoDatasetGenerator, the transformation from body to camera is just a
+    // rotation from NWU to EDN
+    dataset.I_p_IC = Vec3::Zero();
+    dataset.R_IC = Rotation{Vec3{-M_PI_2, 0, -M_PI_2}};
+
+    return dataset;
+}
+
+}  // namespace wave

wave_vision/tests/dataset_tests/vio_dataset_tests.cpp

@@ -0,0 +1,24 @@
+#include <boost/filesystem.hpp>
+
+#include "wave/wave_test.hpp"
+#include "wave/vision/dataset/VioDatasetGenerator.hpp"
+
+namespace wave {
+
+const auto test_config_file = "tests/data/vo_test.yaml";
+
+TEST(VioDataset, constructor) {
+    VioDataset dataset{};
+}
+
+TEST(VioDataset, generate) {
+    VioDatasetGenerator generator;
+    generator.configure(test_config_file);
+    auto dataset = generator.generate();
+
+    // expected value from test_config_file
+    EXPECT_EQ(100u, dataset.landmarks.size());
+    EXPECT_FALSE(dataset.poses.empty());
+}
+
+}  // wave namespace