small changes and doc updates

ReadMe.md

@@ -91,14 +91,11 @@ details on what the system supports.
 
 ## Codebase Extensions
 
-* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop
- closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the
- code originally developed by the HKUST aerial robotics group and can be found in
- their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a
- loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry.
- This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to
- camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop
- closure detection to improve frequency.
+* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
+ a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
+ Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
+ trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture
+ information and estimates all unknown spacial-temporal calibrations between the two sensors.
 
 * **[ov_maplab](https://github.com/rpng/ov_maplab)** - This codebase contains the interface wrapper for exporting
  visual-inertial runs from [OpenVINS](https://github.com/rpng/open_vins) into the ViMap structure taken
@@ -108,11 +105,15 @@ details on what the system supports.
  running the data through OpenVINS. Some example have been provided along with a helper script to export trajectories
  into the standard groundtruth format.
 
-* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
- a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
- Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
- trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture
- information and estimates all unknown spacial-temporal calibrations between the two sensors.
+* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop
+ closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the
+ code originally developed by the HKUST aerial robotics group and can be found in
+ their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a
+ loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry.
+ This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to
+ camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop
+ closure detection to improve frequency.
+
 
 ## Demo Videos
 

ov_core/package.xml

@@ -3,7 +3,7 @@
 
  <!-- Package Information -->
  <name>ov_core</name>
- <version>2.5.0</version>
+ <version>2.6.0</version>
  <description>
  Core algorithms for visual-inertial navigation algorithms.
  </description>

ov_data/package.xml

@@ -3,7 +3,7 @@
 
  <!-- Package Information -->
  <name>ov_data</name>
- <version>2.5.0</version>
+ <version>2.6.0</version>
  <description>
  Data for the OpenVINS project, mostly just groundtruth files...
  </description>

ov_eval/package.xml

@@ -3,7 +3,7 @@
 
  <!-- Package Information -->
  <name>ov_eval</name>
- <version>2.5.0</version>
+ <version>2.6.0</version>
  <description>
  Evaluation methods and scripts for visual-inertial odometry systems.
  </description>

ov_init/launch/dynamic.launch

@@ -17,6 +17,9 @@
  <!-- if we should perturb the initial state values (i.e. calibration) -->
  <arg name="sim_do_perturbation" default="true" />
 
+ <!-- if we should optimize the calibration -->
+ <arg name="mle_opt_calib" default="false" />
+
 
  <!-- ================================================================ -->
  <!-- ================================================================ -->
@@ -40,6 +43,7 @@
 
  <!-- world/filter parameters -->
  <param name="max_cameras" type="int" value="1" />
+ <param name="init_dyn_mle_opt_calib" type="bool" value="$(arg mle_opt_calib)" />
 
  </node>
 

ov_init/launch/mle.launch

@@ -17,6 +17,9 @@
  <!-- if we should perturb the initial state values (i.e. calibration) -->
  <arg name="sim_do_perturbation" default="false" />
 
+ <!-- if we should optimize the calibration -->
+ <arg name="mle_opt_calib" default="false" />
+
 
  <!-- ================================================================ -->
  <!-- ================================================================ -->
@@ -40,6 +43,8 @@
 
  <!-- world/filter parameters -->
  <param name="max_cameras" type="int" value="1" />
+ <param name="init_dyn_mle_opt_calib" type="bool" value="$(arg mle_opt_calib)" />
+ <param name="init_max_features" type="int" value="30" />
 
  </node>
 

ov_init/package.xml

@@ -3,9 +3,9 @@
 
  <!-- Package Information -->
  <name>ov_init</name>
- <version>2.5.0</version>
+ <version>2.6.0</version>
  <description>
- Initialization package which handles dynamic and static initialization.
+ Initialization package which handles static and dynamic initialization.
  </description>
  <url type="website">https://docs.openvins.com/</url>
  <url type="bugtracker">https://github.com/rpng/open_vins/issues</url>

ov_init/src/ceres/Factor_GenericPrior.h

@@ -37,25 +37,38 @@ namespace ov_init {
  * https://journals.sagepub.com/doi/full/10.1177/0278364919835021
  *
  * We have the following minimization problem:
- * argmin ||A * (x - x_lin) + b||^2
+ * @f[
+ * \textrm{argmin} ||A * (x - x_{lin}) + b||^2
+ * @f]
+ *
  *
  * In general we have the following after marginalization:
- * - (A^T*A) = Inf_prior (the prior information)
- * - A^T*b = grad_prior (the prior gradient)
+ * - @f$(A^T*A) = Inf_{prior} @f$ (the prior information)
+ * - @f$A^T*b = grad_{prior} @f$ (the prior gradient)
  *
  * For example, consider we have the following system were we wish to remove the xm states.
  * This is the problem of state marginalization.
+ * @f[
  * [ Arr Arm ] [ xr ] = [ - gr ]
+ * @f]
+ * @f[
  * [ Amr Amm ] [ xm ] = [ - gm ]
+ * @f]
  *
- * We wish to marginalize the xm states which are correlated with the other states xr.
- * The Jacobian (and thus information matrix A) is computed at the current best guess x_lin.
- * We can define the following optimal subcost form which only involves the xr states as:
- * cost^2 = (xr - xr_lin)^T*(A^T*A)*(xr - xr_lin) + b^T*A*(xr - xr_lin) + b^b
+ * We wish to marginalize the xm states which are correlated with the other states @f$ xr @f$.
+ * The Jacobian (and thus information matrix A) is computed at the current best guess @f$ x_{lin} @f$.
+ * We can define the following optimal subcost form which only involves the @f$ xr @f$ states as:
+ * @f[
+ * cost^2 = (xr - xr_{lin})^T*(A^T*A)*(xr - xr_{lin}) + b^T*A*(xr - xr_{lin}) + b^b
+ * @f]
  *
  * where we have:
- * A = sqrt(Arr - Arm*Amm^-1*Amr)
- * b = A^-1 * (gr - Arm*Amm^-1*gm)
+ * @f[
+ * A = sqrt(Arr - Arm*Amm^{-1}*Amr)
+ * @f]
+ * @f[
+ * b = A^-1 * (gr - Arm*Amm^{-1}*gm)
+ * @f]
  *
  */
 class Factor_GenericPrior : public ceres::CostFunction {
@@ -138,6 +151,9 @@ class Factor_GenericPrior : public ceres::CostFunction {
 
  virtual ~Factor_GenericPrior() {}
 
+ /**
+ * @brief Error residual and Jacobian calculation
+ */
  bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
 
  // Location in our state and output residual

ov_init/src/ceres/Factor_ImageReprojCalib.h

@@ -55,6 +55,9 @@ class Factor_ImageReprojCalib : public ceres::CostFunction {
 
  /**
  * @brief Default constructor
+ * @param uv_meas_ Raw pixel uv measurement of a environmental feature
+ * @param pix_sigma_ Raw pixel measurement uncertainty (typically 1)
+ * @param is_fisheye_ If this raw pixel camera uses fisheye distortion
  */
  Factor_ImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_)
  : uv_meas(uv_meas_), pix_sigma(pix_sigma_), is_fisheye(is_fisheye_) {
@@ -76,6 +79,14 @@ class Factor_ImageReprojCalib : public ceres::CostFunction {
 
  virtual ~Factor_ImageReprojCalib() {}
 
+ /**
+ * @brief Error residual and Jacobian calculation
+ *
+ * This computes the Jacobians and residual of the feature projection model.
+ * This is a function of the observing pose, feature in global, and calibration parameters.
+ * The normalized pixel coordinates are found and then distorted using the camera distortion model.
+ * See the @ref update-feat page for more details.
+ */
  bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
 
  // Recover the current state from our parameters

ov_init/src/ceres/Factor_ImuCPIv1.h

@@ -109,6 +109,13 @@ class Factor_ImuCPIv1 : public ceres::CostFunction {
 
  virtual ~Factor_ImuCPIv1() {}
 
+ /**
+ * @brief Error residual and Jacobian calculation
+ *
+ * This computes the error between the integrated preintegrated measurement
+ * and the current state estimate. This also takes into account the
+ * bias linearization point changes.
+ */
  bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
 
  // Get the local variables (these would be different if we relinearized)

ov_init/src/ceres/State_JPLQuatLocal.h

@@ -33,6 +33,16 @@ namespace ov_init {
  */
 class State_JPLQuatLocal : public ceres::LocalParameterization {
 public:
+ /**
+ * @brief State update function for a JPL quaternion representation.
+ *
+ * Implements update operation by left-multiplying the current
+ * quaternion with a quaternion built from a small axis-angle perturbation.
+ *
+ * @f[
+ * \bar{q}=norm\Big(\begin{bmatrix} 0.5*\mathbf{\theta_{dx}} \\ 1 \end{bmatrix}\Big) \hat{\bar{q}}
+ * @f]
+ */
  bool Plus(const double *x, const double *delta, double *x_plus_delta) const override {
 
  // Apply the standard JPL update: q <-- [d_th/2; 1] (x) q
@@ -56,10 +66,18 @@ class State_JPLQuatLocal : public ceres::LocalParameterization {
  return true;
  }
 
+ /**
+ * @brief Computes the jacobian in respect to the local parameterization
+ *
+ * This essentially "tricks" ceres.
+ * Instead of doing what ceres wants:
+ * dr/dlocal= dr/dglobal * dglobal/dlocal
+ *
+ * We instead directly do:
+ * dr/dlocal= [ dr/dlocal, 0] * [I; 0]= dr/dlocal.
+ * Therefore we here define dglobal/dlocal= [I; 0]
+ */
  bool ComputeJacobian(const double *x, double *jacobian) const override {
- // This essentially "tricks" ceres.
- // Instead of doing what ceres wants, dr/dlocal= dr/dglobal * dglobal/dlocal, we instead directly do:
- // dr/dlocal= [ dr/dlocal, 0] * [I; 0]= dr/dlocal. Therefore we here define dglobal/dlocal= [I; 0]
  Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
  j.topRows<3>().setIdentity();
  j.bottomRows<1>().setZero();

ov_init/src/dummy.cpp

@@ -23,8 +23,14 @@
  * @namespace ov_init
  * @brief State initialization code
  *
- * Right now this contains static initialization code for a visual-inertial system.
+ * Right now this contains static and dynamic initialization code for a visual-inertial system.
  * It will wait for the platform to stationary, and then initialize its orientation in the gravity frame.
  *
+ * If the platform is not stationary then we leverage dynamic initialization to try to recover the initial state.
+ * This is an implementation of the work [Estimator initialization in vision-aided inertial navigation with unknown camera-IMU
+ * calibration](https://ieeexplore.ieee.org/document/6386235) @cite Dong2012IROS which solves the initialization problem by first creating a
+ * linear system for recovering tthe velocity, gravity, and feature positions.
+ * After the initial recovery, a full optimization is performed to allow for covariance recovery.
+ *
  */
 namespace ov_init {}

ov_init/src/dynamic/DynamicInitializer.h

@@ -45,12 +45,10 @@ namespace ov_init {
  *
  * This implementation that will try to recover the initial conditions of the system.
  * Additionally, we will try to recover the covariance of the system.
- *
  * To initialize with arbitrary motion:
  * 1. Preintegrate our system to get the relative rotation change (biases assumed known)
- * 2. Recover the IMU gyroscope bias, time offset, and refine R_CtoI with a MLE (optional)
- * 3. Construct linear system with features to recover velocity (solve with |g| constraint)
- * 4. Perform a large MLE with all calibration and recover the covariance.
+ * 2. Construct linear system with features to recover velocity (solve with |g| constraint)
+ * 3. Perform a large MLE with all calibration and recover the covariance.
  *
  * Method is based on this work:
  * > Dong-Si, Tue-Cuong, and Anastasios I. Mourikis.

ov_init/src/init/InertialInitializer.h

@@ -40,7 +40,6 @@ namespace ov_init {
  * This will try to do both dynamic and state initialization of the state.
  * The user can request to wait for a jump in our IMU readings (i.e. device is picked up) or to initialize as soon as possible.
  * For state initialization, the user needs to specify the calibration beforehand, otherwise dynamic is always used.
- *
  * The logic is as follows:
  * 1. Try to perform dynamic initialization of state elements.
  * 2. If this fails and we have calibration then we can try to do static initialization
@@ -50,7 +49,6 @@ namespace ov_init {
  * with unknown camera-IMU calibration](https://ieeexplore.ieee.org/document/6386235) @cite Dong2012IROS which solves the initialization
  * problem by first creating a linear system for recovering the camera to IMU rotation, then for velocity, gravity, and feature positions,
  * and finally a full optimization to allow for covariance recovery.
- *
  * Another paper which might be of interest to the reader is [An Analytical Solution to the IMU Initialization
  * Problem for Visual-Inertial Systems](https://ieeexplore.ieee.org/abstract/document/9462400) which has some detailed
  * experiments on scale recovery and the accelerometer bias.
@@ -68,8 +66,30 @@ class InertialInitializer {
  /**
  * @brief Feed function for inertial data
  * @param message Contains our timestamp and inertial information
+ * @param oldest_time Time that we can discard measurements before
  */
- void feed_imu(const ov_core::ImuData &message) { imu_data->push_back(message); }
+ void feed_imu(const ov_core::ImuData &message, double oldest_time = -1) {
+
+ // Append it to our vector
+ imu_data->emplace_back(message);
+
+ // Sort our imu data (handles any out of order measurements)
+ // std::sort(imu_data->begin(), imu_data->end(), [](const IMUDATA i, const IMUDATA j) {
+ // return i.timestamp < j.timestamp;
+ //});
+
+ // Loop through and delete imu messages that are older than our requested time
+ if (oldest_time != -1) {
+ auto it0 = imu_data->begin();
+ while (it0 != imu_data->end()) {
+ if (message.timestamp < oldest_time) {
+ it0 = imu_data->erase(it0);
+ } else {
+ it0++;
+ }
+ }
+ }
+ }
 
  /**
  * @brief Try to get the initialized system

ov_init/src/static/StaticInitializer.h

@@ -38,7 +38,6 @@ namespace ov_init {
  * @brief Initializer for a static visual-inertial system.
  *
  * This implementation that assumes that the imu starts from standing still.
- *
  * To initialize from standstill:
  * 1. Collect all inertial measurements
  * 2. See if within the last window there was a jump in acceleration