From 2d64153ff2be16c0b08dc585edf82d4066bdf430 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Sun, 6 Aug 2017 22:08:27 -0400 Subject: [PATCH 1/8] Add VioDataset and VioDatasetGenerator classes --- CMakeLists.txt | 2 +- wave_utils/include/wave/utils/math.hpp | 6 + wave_vision/CMakeLists.txt | 3 + .../wave/vision/dataset/VioDataset.hpp | 108 ++++++++++++++++++ wave_vision/src/dataset/VioDataset.cpp | 11 ++ .../tests/dataset_tests/vio_dataset_tests.cpp | 15 +++ 6 files changed, 144 insertions(+), 1 deletion(-) create mode 100644 wave_vision/include/wave/vision/dataset/VioDataset.hpp create mode 100644 wave_vision/src/dataset/VioDataset.cpp create mode 100644 wave_vision/tests/dataset_tests/vio_dataset_tests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6cddcab7..10277bce 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/wave_utils/include/wave/utils/math.hpp b/wave_utils/include/wave/utils/math.hpp index 466926d2..7e413062 100644 --- a/wave_utils/include/wave/utils/math.hpp +++ b/wave_utils/include/wave/utils/math.hpp @@ -43,6 +43,12 @@ typedef Eigen::Affine3d Affine3; typedef Eigen::Quaterniond Quaternion; #endif +/** A vector for fixed-size Eigen types, giving the required alignment. + * See https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html + */ +template +using EigenVector = std::vector>; + /** * Eigen vector comparator */ diff --git a/wave_vision/CMakeLists.txt b/wave_vision/CMakeLists.txt index 218bd4a7..abd045c2 100644 --- a/wave_vision/CMakeLists.txt +++ b/wave_vision/CMakeLists.txt @@ -21,6 +21,7 @@ SET( ${OpenCV_LIBS} ${Boost_LIBRARIES} wave_kinematics + wave_geometry wave_utils ) ADD_LIBRARY( @@ -28,6 +29,7 @@ ADD_LIBRARY( STATIC src/utils.cpp src/dataset/VoDataset.cpp + src/dataset/VioDataset.cpp src/dataset/VoTestCamera.cpp src/detector/fast_detector.cpp src/detector/orb_detector.cpp @@ -46,6 +48,7 @@ WAVE_ADD_TEST(${PROJECT_NAME}_tests tests/matcher_tests/brute_force_tests.cpp tests/tracker_tests/tracker_tests.cpp tests/dataset_tests/vo_dataset_tests.cpp + tests/dataset_tests/vio_dataset_tests.cpp tests/utils_tests.cpp) TARGET_LINK_LIBRARIES(${PROJECT_NAME}_tests ${PROJECT_NAME}) diff --git a/wave_vision/include/wave/vision/dataset/VioDataset.hpp b/wave_vision/include/wave/vision/dataset/VioDataset.hpp new file mode 100644 index 00000000..5b372695 --- /dev/null +++ b/wave_vision/include/wave/vision/dataset/VioDataset.hpp @@ -0,0 +1,108 @@ +/** + * @file + * @ingroup vision + */ +#ifndef WAVE_VISION_VIODATASET_HPP +#define WAVE_VISION_VIODATASET_HPP + +#include "wave/utils/utils.hpp" +#include "wave/vision/dataset/VoDataset.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/kinematics/two_wheel.hpp" +#include "wave/geometry/rotation.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 world frame */ + struct ImuValue { + Vec3 G_vel_GI; + Vec3 G_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 { Gps }; + enum class Camera { Left }; + using PoseMeasurement = Measurement; + using ImuMeasurement = Measurement; + + // Measurements + MeasurementContainer poses; + MeasurementContainer imu_measurements; + LandmarkMeasurementContainer> + feature_measurements; + + // Calibration + + /** Camera intrinsic matrix */ + Mat3 calib_K; + /** Camera transformation from IMU frame (expressed in IMU frame) */ + Vec3 I_p_IC; + Rotation R_IC; + + /** Ground truth 3D world landmarks, where each column represents a feature + * and each row represents the feature position in x, y, z (NWU) */ + EigenVector landmarks; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * 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; + + /** 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() const; +}; + + +/** @} end of group */ +} // namespace wave +#endif // WAVE_VISION_VIODATASET_HPP diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp new file mode 100644 index 00000000..2457bd52 --- /dev/null +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -0,0 +1,11 @@ +#include "wave/vision/dataset/VioDataset.hpp" + +namespace wave { + +VioDataset VioDatasetGenerator::generate() const { + VioDataset dataset; + + return dataset; +} + +} // namespace wave diff --git a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp new file mode 100644 index 00000000..c787c43f --- /dev/null +++ b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp @@ -0,0 +1,15 @@ +#include + +#include "wave/wave_test.hpp" +#include "wave/vision/dataset/VioDataset.hpp" + +namespace wave { + +const auto test_config_file = "tests/data/vo_test.yaml"; +const auto test_output_dir = "/tmp/dataset_test"; + +TEST(VioDataset, constructor) { + VioDataset dataset{}; +} + +} // wave namespace From fde396b0f0593728277b13afd5039177fd9e7818 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Sun, 6 Aug 2017 22:19:14 -0400 Subject: [PATCH 2/8] Separate wave_vision_dataset_tests --- wave_vision/CMakeLists.txt | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/wave_vision/CMakeLists.txt b/wave_vision/CMakeLists.txt index abd045c2..7fabd057 100644 --- a/wave_vision/CMakeLists.txt +++ b/wave_vision/CMakeLists.txt @@ -47,11 +47,13 @@ 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/dataset_tests/vio_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) + WAVE_ADD_TEST( ${PROJECT_NAME}_viz_tests tests/viz_tests/detector_viz_tests.cpp @@ -60,6 +62,14 @@ WAVE_ADD_TEST( tests/viz_tests/tracker_viz_tests.cpp DISABLED # Requires display to run ) +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME}_dataset_tests + ${PROJECT_NAME} + wave_geometry + wave_utils + ${Boost_LIBRARIES} +) + TARGET_LINK_LIBRARIES( ${PROJECT_NAME}_viz_tests ${PROJECT_NAME} From 6298cf45a4dd67a667140fa119517084e7a1793f Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Sun, 6 Aug 2017 22:23:58 -0400 Subject: [PATCH 3/8] Simplify library dependencies --- wave_vision/CMakeLists.txt | 34 ++++++++++------------------------ 1 file changed, 10 insertions(+), 24 deletions(-) diff --git a/wave_vision/CMakeLists.txt b/wave_vision/CMakeLists.txt index 7fabd057..319d558e 100644 --- a/wave_vision/CMakeLists.txt +++ b/wave_vision/CMakeLists.txt @@ -15,15 +15,6 @@ INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) # LIBRARY -SET( - WAVE_VISION_DEPS - yaml-cpp - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - wave_kinematics - wave_geometry - wave_utils -) ADD_LIBRARY( wave_vision STATIC @@ -37,7 +28,14 @@ ADD_LIBRARY( 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 @@ -53,6 +51,7 @@ 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 @@ -62,20 +61,7 @@ WAVE_ADD_TEST( tests/viz_tests/tracker_viz_tests.cpp DISABLED # Requires display to run ) -TARGET_LINK_LIBRARIES( - ${PROJECT_NAME}_dataset_tests - ${PROJECT_NAME} - wave_geometry - wave_utils - ${Boost_LIBRARIES} -) - -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) From 63176deb6731006cf5aa3564aec6e5dbfe60734e Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Sun, 6 Aug 2017 23:49:14 -0400 Subject: [PATCH 4/8] Implement generate() --- wave_utils/include/wave/utils/math.hpp | 6 -- .../wave/vision/dataset/VioDataset.hpp | 29 +++++---- .../include/wave/vision/dataset/VoDataset.hpp | 3 +- wave_vision/src/dataset/VioDataset.cpp | 64 ++++++++++++++++++- .../tests/dataset_tests/vio_dataset_tests.cpp | 10 +++ 5 files changed, 91 insertions(+), 21 deletions(-) diff --git a/wave_utils/include/wave/utils/math.hpp b/wave_utils/include/wave/utils/math.hpp index 7e413062..466926d2 100644 --- a/wave_utils/include/wave/utils/math.hpp +++ b/wave_utils/include/wave/utils/math.hpp @@ -43,12 +43,6 @@ typedef Eigen::Affine3d Affine3; typedef Eigen::Quaterniond Quaternion; #endif -/** A vector for fixed-size Eigen types, giving the required alignment. - * See https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html - */ -template -using EigenVector = std::vector>; - /** * Eigen vector comparator */ diff --git a/wave_vision/include/wave/vision/dataset/VioDataset.hpp b/wave_vision/include/wave/vision/dataset/VioDataset.hpp index 5b372695..9a18e342 100644 --- a/wave_vision/include/wave/vision/dataset/VioDataset.hpp +++ b/wave_vision/include/wave/vision/dataset/VioDataset.hpp @@ -56,28 +56,30 @@ struct VioDataset { }; enum class ImuSensor { Imu }; - enum class PoseSensor { Gps }; + enum class PoseSensor { Pose }; enum class Camera { Left }; - using PoseMeasurement = Measurement; - using ImuMeasurement = Measurement; + + using LandmarkMeasurementContainer = + LandmarkMeasurementContainer>; + using PoseContainer = + MeasurementContainer>; + using ImuContainer = MeasurementContainer>; // Measurements - MeasurementContainer poses; - MeasurementContainer imu_measurements; - LandmarkMeasurementContainer> - feature_measurements; + PoseContainer poses; + ImuContainer imu_measurements; + LandmarkMeasurementContainer feature_measurements; // Calibration /** Camera intrinsic matrix */ - Mat3 calib_K; + Mat3 camera_K; /** Camera transformation from IMU frame (expressed in IMU frame) */ Vec3 I_p_IC; Rotation R_IC; - /** Ground truth 3D world landmarks, where each column represents a feature - * and each row represents the feature position in x, y, z (NWU) */ - EigenVector landmarks; + /** Ground truth 3D landmark positions in the world frame. */ + LandmarkMap landmarks; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -93,13 +95,16 @@ class VioDatasetGenerator : private VoDatasetGenerator { // 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() const; + VioDataset generate(); }; diff --git a/wave_vision/include/wave/vision/dataset/VoDataset.hpp b/wave_vision/include/wave/vision/dataset/VoDataset.hpp index 8087d484..bde2ac23 100644 --- a/wave_vision/include/wave/vision/dataset/VoDataset.hpp +++ b/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 */ diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp index 2457bd52..3a85b6b3 100644 --- a/wave_vision/src/dataset/VioDataset.cpp +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -2,9 +2,71 @@ namespace wave { -VioDataset VioDatasetGenerator::generate() const { +// Helper functions for 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 observations, + VioDataset::LandmarkMeasurementContainer &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); + } +} + +// Handle one VoDataset state, adding measurements to containers +void addVoStateToDataset(ImageNum i, + TimePoint time_start, + const VoInstant &state, + VioDataset &dataset) { + auto d = std::chrono::duration(state.time); + + TimePoint time_point = + time_start + std::chrono::duration_cast(d); + + auto pose = VioDataset::PoseValue{}; + pose.G_p_GI = state.robot_G_p_GB; + pose.R_GI = Rotation{}.setFromMatrix(state.robot_q_GB.matrix()); + + 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); +} + +} // 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); + } + + // 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; } diff --git a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp index c787c43f..259c52dd 100644 --- a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp +++ b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp @@ -12,4 +12,14 @@ 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 From bfa71d9e02611325ca223f82680bb10e79093208 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Mon, 7 Aug 2017 13:33:36 -0400 Subject: [PATCH 5/8] Add inertial measurements --- .../wave/vision/dataset/VioDataset.hpp | 6 +- wave_vision/src/dataset/VioDataset.cpp | 72 ++++++++++++++++--- 2 files changed, 67 insertions(+), 11 deletions(-) diff --git a/wave_vision/include/wave/vision/dataset/VioDataset.hpp b/wave_vision/include/wave/vision/dataset/VioDataset.hpp index 9a18e342..779f1249 100644 --- a/wave_vision/include/wave/vision/dataset/VioDataset.hpp +++ b/wave_vision/include/wave/vision/dataset/VioDataset.hpp @@ -39,10 +39,10 @@ struct VioDataset { /** 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 world frame */ + * the world frame, expressed in the IMU frame */ struct ImuValue { - Vec3 G_vel_GI; - Vec3 G_ang_vel_GI; + Vec3 I_vel_GI; + Vec3 I_ang_vel_GI; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp index 3a85b6b3..377bf39d 100644 --- a/wave_vision/src/dataset/VioDataset.cpp +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -2,7 +2,7 @@ namespace wave { -// Helper functions for this file +// Helper functions used only in this file namespace { // Add measurements from VoDataset's landmark measurement map to a container void addVoMeasurementsToContainer( @@ -18,19 +18,28 @@ void addVoMeasurementsToContainer( } } +// 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(dt_seconds); + return time_start + std::chrono::duration_cast(d); +} + // Handle one VoDataset state, adding measurements to containers void addVoStateToDataset(ImageNum i, TimePoint time_start, const VoInstant &state, VioDataset &dataset) { - auto d = std::chrono::duration(state.time); - - TimePoint time_point = - time_start + std::chrono::duration_cast(d); + const auto time_point = timePointAfterStartTime(time_start, state.time); - auto pose = VioDataset::PoseValue{}; - pose.G_p_GI = state.robot_G_p_GB; - pose.R_GI = Rotation{}.setFromMatrix(state.robot_q_GB.matrix()); + auto pose = poseFromVoState(state); dataset.poses.emplace(time_point, VioDataset::PoseSensor::Pose, pose); @@ -42,6 +51,45 @@ void addVoStateToDataset(ImageNum i, 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() { @@ -58,10 +106,18 @@ VioDataset VioDatasetGenerator::generate() { // 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(); From 7556ea6eecf5dae405925309bdb4e31c62557818 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Mon, 7 Aug 2017 14:11:32 -0400 Subject: [PATCH 6/8] Add TimePoint definition It's not in master yet --- wave_vision/src/dataset/VioDataset.cpp | 3 +++ wave_vision/tests/dataset_tests/vio_dataset_tests.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp index 377bf39d..bd1cda03 100644 --- a/wave_vision/src/dataset/VioDataset.cpp +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -2,6 +2,9 @@ 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 diff --git a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp index 259c52dd..db3b2a86 100644 --- a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp +++ b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp @@ -6,7 +6,6 @@ namespace wave { const auto test_config_file = "tests/data/vo_test.yaml"; -const auto test_output_dir = "/tmp/dataset_test"; TEST(VioDataset, constructor) { VioDataset dataset{}; From 4d5d5b7ea83fa7a6a0b9a617eb5b2716decae91a Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Tue, 8 Aug 2017 12:23:43 -0400 Subject: [PATCH 7/8] Fix name collision --- wave_vision/include/wave/vision/dataset/VioDataset.hpp | 4 ++-- wave_vision/src/dataset/VioDataset.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wave_vision/include/wave/vision/dataset/VioDataset.hpp b/wave_vision/include/wave/vision/dataset/VioDataset.hpp index 779f1249..1f26939a 100644 --- a/wave_vision/include/wave/vision/dataset/VioDataset.hpp +++ b/wave_vision/include/wave/vision/dataset/VioDataset.hpp @@ -59,7 +59,7 @@ struct VioDataset { enum class PoseSensor { Pose }; enum class Camera { Left }; - using LandmarkMeasurementContainer = + using ObsContainer = LandmarkMeasurementContainer>; using PoseContainer = MeasurementContainer>; @@ -68,7 +68,7 @@ struct VioDataset { // Measurements PoseContainer poses; ImuContainer imu_measurements; - LandmarkMeasurementContainer feature_measurements; + ObsContainer feature_measurements; // Calibration diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp index bd1cda03..88cfefc7 100644 --- a/wave_vision/src/dataset/VioDataset.cpp +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -13,7 +13,7 @@ void addVoMeasurementsToContainer( TimePoint time_point, VioDataset::Camera sensor_id, const std::vector observations, - VioDataset::LandmarkMeasurementContainer &container) { + VioDataset::ObsContainer &container) { for (const auto &obs : observations) { const auto &landmark_id = obs.first; const auto &measurement = obs.second; @@ -29,7 +29,7 @@ VioDataset::PoseValue poseFromVoState(const VoInstant &state) { return pose; } -// Get a TimePoint given a start time and duration in seconds +// 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(dt_seconds); return time_start + std::chrono::duration_cast(d); From 0dd1dba607848b2069e8fa5f957c913adde050e9 Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Tue, 8 Aug 2017 12:31:32 -0400 Subject: [PATCH 8/8] Separate VioDatasetGenerator.hpp --- wave_vision/CMakeLists.txt | 1 + .../wave/vision/dataset/VioDataset.hpp | 27 +--- .../vision/dataset/VioDatasetGenerator.hpp | 40 ++++++ wave_vision/src/dataset/VioDataset.cpp | 131 +---------------- .../src/dataset/VioDatasetGenerator.cpp | 132 ++++++++++++++++++ .../tests/dataset_tests/vio_dataset_tests.cpp | 2 +- 6 files changed, 176 insertions(+), 157 deletions(-) create mode 100644 wave_vision/include/wave/vision/dataset/VioDatasetGenerator.hpp create mode 100644 wave_vision/src/dataset/VioDatasetGenerator.cpp diff --git a/wave_vision/CMakeLists.txt b/wave_vision/CMakeLists.txt index 319d558e..530de258 100644 --- a/wave_vision/CMakeLists.txt +++ b/wave_vision/CMakeLists.txt @@ -21,6 +21,7 @@ ADD_LIBRARY( 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 diff --git a/wave_vision/include/wave/vision/dataset/VioDataset.hpp b/wave_vision/include/wave/vision/dataset/VioDataset.hpp index 1f26939a..dc9349f3 100644 --- a/wave_vision/include/wave/vision/dataset/VioDataset.hpp +++ b/wave_vision/include/wave/vision/dataset/VioDataset.hpp @@ -6,13 +6,12 @@ #define WAVE_VISION_VIODATASET_HPP #include "wave/utils/utils.hpp" -#include "wave/vision/dataset/VoDataset.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/kinematics/two_wheel.hpp" #include "wave/geometry/rotation.hpp" +#include "wave/vision/dataset/VoTestCamera.hpp" namespace wave { /** @addtogroup vision @@ -84,30 +83,6 @@ struct VioDataset { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/** - * 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_VIODATASET_HPP diff --git a/wave_vision/include/wave/vision/dataset/VioDatasetGenerator.hpp b/wave_vision/include/wave/vision/dataset/VioDatasetGenerator.hpp new file mode 100644 index 00000000..a7180a4d --- /dev/null +++ b/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 diff --git a/wave_vision/src/dataset/VioDataset.cpp b/wave_vision/src/dataset/VioDataset.cpp index 88cfefc7..4c0b9f96 100644 --- a/wave_vision/src/dataset/VioDataset.cpp +++ b/wave_vision/src/dataset/VioDataset.cpp @@ -1,132 +1,3 @@ #include "wave/vision/dataset/VioDataset.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 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(dt_seconds); - return time_start + std::chrono::duration_cast(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 +namespace wave {} // namespace wave diff --git a/wave_vision/src/dataset/VioDatasetGenerator.cpp b/wave_vision/src/dataset/VioDatasetGenerator.cpp new file mode 100644 index 00000000..763473f3 --- /dev/null +++ b/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 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(dt_seconds); + return time_start + std::chrono::duration_cast(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 diff --git a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp index db3b2a86..ebfab754 100644 --- a/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp +++ b/wave_vision/tests/dataset_tests/vio_dataset_tests.cpp @@ -1,7 +1,7 @@ #include #include "wave/wave_test.hpp" -#include "wave/vision/dataset/VioDataset.hpp" +#include "wave/vision/dataset/VioDatasetGenerator.hpp" namespace wave {