fixed for ILLIXR integration

CMakeLists.txt

@@ -12,8 +12,11 @@ endif()
 option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" OFF)
 set(ILLIXR_ROOT "" CACHE PATH "Directory of ILLIXR includes")
 option(ENABLE_TESTS "Enable building tests" OFF)
-
-if (ILLIXR_INTEGRATION AND NOT ILLIXR_ROOT)
+option(USE_ZED "Whether to build for the zed camera" OFF)
+if (USE_ZED)
+    add_compile_definitions(HAVE_ZED)
+endif()
+if (NOT ILLIXR_ROOT)
     message(FATAL_ERROR "ILLIXR_ROOT must be specified if ILLIXR_INTEGRATION is enabled.")
 endif()
 

ov_msckf/src/core/VioManagerOptions.h

@@ -97,6 +97,11 @@ struct VioManagerOptions {
   /// If we should record the timing performance to file
   bool record_timing_information = false;
 
+  /// Amount of time we will initialize over (seconds)
+  double init_window_time = 1.0;
+
+  ///  Variance threshold on our acceleration to be classified as moving
+  double init_imu_thresh = 1.0;
   /// The path to the file we will record the timing information into
   std::string record_timing_filepath = "ov_msckf_timing.txt";
 
@@ -126,6 +131,8 @@ struct VioManagerOptions {
     PRINT_DEBUG("  - zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier)
     PRINT_DEBUG("  - zupt_max_disparity: %.4f\n", zupt_max_disparity)
     PRINT_DEBUG("  - zupt_only_at_beginning?: %d\n", zupt_only_at_beginning)
+    PRINT_DEBUG("\t- init_window_time: %.2f\n", init_window_time)
+    PRINT_DEBUG("\t- init_imu_thresh: %.2f\n", init_imu_thresh)
     PRINT_DEBUG("  - record timing?: %d\n", (int)record_timing_information)
     PRINT_DEBUG("  - record timing filepath: %s\n", record_timing_filepath.c_str())
   }

ov_msckf/src/slam2.cpp

@@ -8,11 +8,13 @@
 
 #include "core/VioManager.h"
 #include "state/State.h"
+#include "utils/quat_ops.h"
 
 #include "illixr/plugin.hpp"
 #include "illixr/switchboard.hpp"
-#include "illixr/data_format.hpp"
-#include "illixr/opencv_data_types.hpp"
+#include "illixr/data_format/opencv_data_types.hpp"
+#include "illixr/data_format/pose.hpp"
+#include "illixr/data_format/imu.hpp"
 #include "illixr/phonebook.hpp"
 #include "illixr/relative_clock.hpp"
 
@@ -21,24 +23,26 @@ using namespace ov_msckf;
 
 // Comment in if using ZED instead of offline_imu_cam
 // TODO: Pull from config YAML file
-// #define ZED
+// #define HAVE_ZED
 
 VioManagerOptions create_params()
 {
 	VioManagerOptions params;
 
 	// Camera #0
 	Eigen::Matrix<double, 8, 1> intrinsics_0;
-#ifdef ZED
+#ifdef HAVE_ZED
   // ZED calibration tool; fx, fy, cx, cy, k1, k2, p1, p2
   // https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
   intrinsics_0 << 349.686, 349.686, 332.778, 192.423, -0.175708, 0.0284421, 0, 0;
 #else
   // EuRoC
 	intrinsics_0 << 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05;
 #endif
+    std::shared_ptr<ov_core::CamRadtan> camera0(new ov_core::CamRadtan(1280, 720));
+    camera0->set_value(intrinsics_0);
 
-#ifdef ZED
+#ifdef HAVE_ZED
   // Camera extrinsics from https://github.com/rpng/open_vins/issues/52#issuecomment-619480497
   std::vector<double> matrix_TCtoI_0 = {-0.01080233, 0.00183858, 0.99993996, 0.01220425,
             -0.99993288, -0.00420947, -0.01079452, 0.0146056,
@@ -59,30 +63,25 @@ VioManagerOptions create_params()
 
 	// Load these into our state
 	Eigen::Matrix<double, 7, 1> extrinsics_0;
-	extrinsics_0.block(0, 0, 4, 1) = rot_2_quat(T_CtoI_0.block(0, 0, 3, 3).transpose());
+	extrinsics_0.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI_0.block(0, 0, 3, 3).transpose());
 	extrinsics_0.block(4, 0, 3, 1) = -T_CtoI_0.block(0, 0, 3, 3).transpose() * T_CtoI_0.block(0, 3, 3, 1);
 
-	params.camera_fisheye.insert({0, false});
-	params.camera_intrinsics.insert({0, intrinsics_0});
+	params.camera_intrinsics.insert({0, camera0});
 	params.camera_extrinsics.insert({0, extrinsics_0});
 
-#ifdef ZED
-  params.camera_wh.insert({0, {672, 376}});
-#else
-	params.camera_wh.insert({0, {752, 480}});
-#endif
-
 	// Camera #1
 	Eigen::Matrix<double, 8, 1> intrinsics_1;
-#ifdef ZED
+#ifdef HAVE_ZED
   // ZED calibration tool; fx, fy, cx, cy, k1, k2, p1, p2
   intrinsics_1 << 350.01, 350.01, 343.729, 185.405, -0.174559, 0.0277521, 0, 0;
 #else
   // EuRoC
 	intrinsics_1 << 457.587, 456.134, 379.999, 255.238, -0.28368365, 0.07451284, -0.00010473, -3.55590700e-05;
 #endif
+    std::shared_ptr<ov_core::CamRadtan> camera1(new ov_core::CamRadtan(1280, 720));
+    camera1->set_value(intrinsics_1);
 
-#ifdef ZED
+#ifdef HAVE_ZED
   // Camera extrinsics from https://github.com/rpng/open_vins/issues/52#issuecomment-619480497
   std::vector<double> matrix_TCtoI_1 = {-0.01043535, -0.00191061, 0.99994372, 0.01190459,
             -0.99993668, -0.00419281, -0.01044329, -0.04732387,
@@ -103,23 +102,16 @@ VioManagerOptions create_params()
 
 	// Load these into our state
 	Eigen::Matrix<double, 7, 1> extrinsics_1;
-	extrinsics_1.block(0, 0, 4, 1) = rot_2_quat(T_CtoI_1.block(0, 0, 3, 3).transpose());
+	extrinsics_1.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI_1.block(0, 0, 3, 3).transpose());
 	extrinsics_1.block(4, 0, 3, 1) = -T_CtoI_1.block(0, 0, 3, 3).transpose() * T_CtoI_1.block(0, 3, 3, 1);
 
-	params.camera_fisheye.insert({1, false});
-	params.camera_intrinsics.insert({1, intrinsics_1});
+	params.camera_intrinsics.insert({1, camera1});
 	params.camera_extrinsics.insert({1, extrinsics_1});
 
-#ifdef ZED
-  params.camera_wh.insert({1, {672, 376}});
-#else
-	params.camera_wh.insert({1, {752, 480}});
-#endif
-
 	// params.state_options.max_slam_features = 0;
 	params.state_options.num_cameras = 2;
 	params.init_window_time = 0.75;
-#ifdef ZED
+#ifdef HAVE_ZED
   // Hand tuned
   params.init_imu_thresh = 0.5;
 #else
@@ -129,7 +121,7 @@ VioManagerOptions create_params()
 	params.fast_threshold = 15;
 	params.grid_x = 5;
 	params.grid_y = 3;
-#ifdef ZED
+#ifdef HAVE_ZED
   // Hand tuned
   params.num_pts = 200;
 #else
@@ -138,9 +130,8 @@ VioManagerOptions create_params()
 	params.msckf_options.chi2_multipler = 1;
 	params.knn_ratio = .7;
 
-	params.state_options.imu_avg = true;
 	params.state_options.do_fej = true;
-	params.state_options.use_rk4_integration = true;
+	params.state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod::RK4;
 	params.use_stereo = true;
 	params.state_options.do_calib_camera_pose = true;
 	params.state_options.do_calib_camera_intrinsics = true;
@@ -151,7 +142,7 @@ VioManagerOptions create_params()
 	params.state_options.max_slam_in_update = 25;
 	params.state_options.max_msckf_in_update = 999;
 
-#ifdef ZED
+#ifdef HAVE_ZED
   // Pixel noise; ZED works with defaults values but these may better account for rolling shutter
   params.msckf_options.chi2_multipler = 2;
   params.msckf_options.sigma_pix = 5;
@@ -175,8 +166,8 @@ VioManagerOptions create_params()
 
 	params.use_aruco = false;
 
-	params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH");
-  params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH");
+	params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH");
+    params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH");
 
 	return params;
 }
@@ -192,9 +183,9 @@ class slam2 : public plugin {
 		: plugin{std::move(name_), pb_}
 		, sb{pb->lookup_impl<switchboard>()}
 		, _m_rtc{pb->lookup_impl<RelativeClock>()}
-		, _m_pose{sb->get_writer<pose_type>("slow_pose")}
-		, _m_imu_integrator_input{sb->get_writer<imu_integrator_input>("imu_integrator_input")}
-		, _m_cam{sb->get_buffered_reader<binocular_cam_type>("cam")}
+		, _m_pose{sb->get_writer<ILLIXR::data_format::pose_type>("slow_pose")}
+		, _m_imu_integrator_input{sb->get_writer<ILLIXR::data_format::imu_integrator_input>("imu_integrator_input")}
+		, _m_cam{sb->get_buffered_reader<ILLIXR::data_format::binocular_cam_type>("cam")}
 		, open_vins_estimator{manager_params}
 	{
 
@@ -208,28 +199,27 @@ class slam2 : public plugin {
 
 	}
 
-
 	void start() override {
 		plugin::start();
-		sb->schedule<imu_type>(id, "imu", [&](const switchboard::ptr<const imu_type>& datum, std::size_t iteration_no) {
+		sb->schedule<ILLIXR::data_format::imu_type>(id, "imu", [&](const switchboard::ptr<const ILLIXR::data_format::imu_type>& datum, std::size_t iteration_no) {
 			this->feed_imu_cam(datum, iteration_no);
 		});
 	}
 
 
-	void feed_imu_cam(const switchboard::ptr<const imu_type>& datum, [[maybe_unused]]std::size_t iteration_no) {
+	void feed_imu_cam(const switchboard::ptr<const ILLIXR::data_format::imu_type>& datum, [[maybe_unused]]std::size_t iteration_no) {
 		// Ensures that slam doesnt start before valid IMU readings come in
 		if (datum == nullptr) {
 			return;
 		}
 
 		// Feed the IMU measurement. There should always be IMU data in each call to feed_imu_cam
-		open_vins_estimator.feed_measurement_imu(duration2double(datum->time.time_since_epoch()), datum->angular_v, datum->linear_a);
+		open_vins_estimator.feed_measurement_imu({duration2double(datum->time.time_since_epoch()), datum->angular_v, datum->linear_a});
 
-		switchboard::ptr<const binocular_cam_type> cam;
+		switchboard::ptr<const ILLIXR::data_format::binocular_cam_type> cam;
 		// Camera data can only go downstream when there's at least one IMU sample whose timestamp is larger than the camera data's.
 		cam = (cam_buffer == nullptr && _m_cam.size() > 0) ? _m_cam.dequeue() : nullptr;
-		// If there is not cam data this func call, break early
+		// If there is no cam data this func call, break early
 		if (!cam_buffer && !cam) {
 			return;
 		} else if (!cam_buffer && cam) {
@@ -239,19 +229,9 @@ class slam2 : public plugin {
 			return;
 		}
 
-
-#ifdef CV_HAS_METRICS
-		cv::metrics::setAccount(new std::string{std::to_string(iteration_no)});
-		if (iteration_no % 20 == 0) {
-			cv::metrics::dump();
-		}
-#else
-#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)"
-#endif
-
-		cv::Mat img0{cam_buffer->at(image::LEFT)};
-		cv::Mat img1{cam_buffer->at(image::RIGHT)};
-		open_vins_estimator.feed_measurement_stereo(duration2double(cam_buffer->time.time_since_epoch()), img0, img1, 0, 1);
+		cv::Mat img0{cam_buffer->at(ILLIXR::data_format::image::LEFT_EYE)};
+		cv::Mat img1{cam_buffer->at(ILLIXR::data_format::image::RIGHT_EYE)};
+		open_vins_estimator.feed_measurement_camera({duration2double(cam_buffer->time.time_since_epoch()), {0, 1}, img0, img1});
 
 		// Get the pose returned from SLAM
 		state = open_vins_estimator.get_state();
@@ -281,7 +261,7 @@ class slam2 : public plugin {
 			_m_imu_integrator_input.put(_m_imu_integrator_input.allocate(
 				cam_buffer->time,
 				from_seconds(state->_calib_dt_CAMtoIMU->value()(0)),
-				imu_params{
+                ILLIXR::data_format::imu_params{
 					.gyro_noise = manager_params.imu_noises.sigma_w,
 					.acc_noise = manager_params.imu_noises.sigma_a,
 					.gyro_walk = manager_params.imu_noises.sigma_wb,
@@ -305,13 +285,13 @@ class slam2 : public plugin {
 
 private:
 	const std::shared_ptr<switchboard> sb;
-	std::shared_ptr<RelativeClock> _m_rtc; 
-	switchboard::writer<pose_type> _m_pose;
-	switchboard::writer<imu_integrator_input> _m_imu_integrator_input;
-	State *state{};
+	std::shared_ptr<RelativeClock> _m_rtc;
+	switchboard::writer<ILLIXR::data_format::pose_type> _m_pose;
+	switchboard::writer<ILLIXR::data_format::imu_integrator_input> _m_imu_integrator_input;
+	std::shared_ptr<State> state;
 
-	switchboard::ptr<const binocular_cam_type> cam_buffer;
-	switchboard::buffered_reader<binocular_cam_type> _m_cam;
+	switchboard::ptr<const ILLIXR::data_format::binocular_cam_type> cam_buffer;
+	switchboard::buffered_reader<ILLIXR::data_format::binocular_cam_type> _m_cam;
 
 	VioManagerOptions manager_params = create_params();
 	VioManager open_vins_estimator;

ov_msckf/src/state/Propagator.h

@@ -27,11 +27,6 @@
 #include <mutex>
 
 #include "utils/sensor_data.h"
-#ifdef ILLIXR_INTEGRATION
-#include "illixr/data_format.hpp"
-#endif /// ILLIXR_INTEGRATION
-
-
 #include "utils/NoiseManager.h"
 
 namespace ov_msckf {