initial commit

ov_init/src/ceres/Factor_GenericPrior.h

@@ -0,0 +1,236 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2022 Patrick Geneva
+ * Copyright (C) 2018-2022 Guoquan Huang
+ * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef OV_INIT_CERES_GENERICPRIOR_H
+#define OV_INIT_CERES_GENERICPRIOR_H
+
+#include <ceres/ceres.h>
+
+#include "utils/quat_ops.h"
+
+namespace ov_init {
+
+/**
+ * @brief Factor for generic state priors for specific types.
+ *
+ * This is a general factor which handles state priors which have non-zero linear errors.
+ * In general a unitary factor will have zero error when it is created, thus this extra term can be ignored.
+ * But if performing marginalization, this can be non-zero. See the following paper Section 3.2 Eq. 25-35
+ * https://journals.sagepub.com/doi/full/10.1177/0278364919835021
+ *
+ * We have the following minimization problem:
+ * @f[
+ * \textrm{argmin} ||A * (x - x_{lin}) + b||^2
+ * @f]
+ *
+ *
+ * In general we have the following after marginalization:
+ * - @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 @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:
+ * @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 {
+public:
+  /// State estimates at the time of marginalization to linearize the problem
+  Eigen::MatrixXd x_lin;
+
+  /// State type for each variable in x_lin. Can be [quat, quat_yaw, vec3, vec8]
+  std::vector<std::string> x_type;
+
+  /// The square-root of the information s.t. sqrtI^T * sqrtI = marginal information
+  Eigen::MatrixXd sqrtI;
+
+  /// Constant term inside the cost s.t. sqrtI^T * b = marginal gradient (can be zero)
+  Eigen::MatrixXd b;
+
+  /**
+   * @brief Default constructor
+   */
+  Factor_GenericPrior(const Eigen::MatrixXd &x_lin_, const std::vector<std::string> &x_type_, const Eigen::MatrixXd &prior_Info,
+                      const Eigen::MatrixXd &prior_grad)
+      : x_lin(x_lin_), x_type(x_type_) {
+
+    // First assert that our state and variables are of the correct size
+    int state_size = 0;
+    int state_error_size = 0;
+    for (auto const &str : x_type_) {
+      if (str == "quat") {
+        state_size += 4;
+        state_error_size += 3;
+      } else if (str == "quat_yaw") {
+        state_size += 4;
+        state_error_size += 1;
+      } else if (str == "vec3") {
+        state_size += 3;
+        state_error_size += 3;
+      } else if (str == "vec8") {
+        state_size += 8;
+        state_error_size += 8;
+      } else {
+        std::cerr << "type - " << str << " not implemented in prior" << std::endl;
+        std::exit(EXIT_FAILURE);
+      }
+    }
+    assert(x_lin.rows() == state_size);
+    assert(x_lin.cols() == 1);
+    assert(prior_Info.rows() == state_error_size);
+    assert(prior_Info.cols() == state_error_size);
+    assert(prior_grad.rows() == state_error_size);
+    assert(prior_grad.cols() == 1);
+
+    // Now lets base-compute the square-root information and constant term b
+    // Comes from the form: cost = A * (x - x_lin) + b
+    Eigen::LLT<Eigen::MatrixXd> lltOfI(prior_Info);
+    sqrtI = lltOfI.matrixL().transpose();
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(prior_Info.rows(), prior_Info.rows());
+    b = sqrtI.triangularView<Eigen::Upper>().solve(I) * prior_grad;
+
+    // Check that we have a valid matrix that we can get the information of
+    if (std::isnan(prior_Info.norm()) || std::isnan(sqrtI.norm()) || std::isnan(b.norm())) {
+      std::cerr << "prior_Info - " << std::endl << prior_Info << std::endl << std::endl;
+      std::cerr << "prior_Info_inv - " << std::endl << prior_Info.inverse() << std::endl << std::endl;
+      std::cerr << "b - " << std::endl << b << std::endl << std::endl;
+      std::exit(EXIT_FAILURE);
+    }
+
+    // Set the number of measurements, and the block sized
+    set_num_residuals(state_error_size);
+    for (auto const &str : x_type_) {
+      if (str == "quat")
+        mutable_parameter_block_sizes()->push_back(4);
+      if (str == "quat_yaw")
+        mutable_parameter_block_sizes()->push_back(4);
+      if (str == "vec3")
+        mutable_parameter_block_sizes()->push_back(3);
+      if (str == "vec8")
+        mutable_parameter_block_sizes()->push_back(8);
+    }
+  }
+
+  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
+    int local_it = 0;
+    int global_it = 0;
+    Eigen::MatrixXd res = Eigen::MatrixXd::Zero(num_residuals(), 1);
+
+    // Loop through each state and calculate its residual and Jacobian
+    for (size_t i = 0; i < x_type.size(); i++) {
+      if (x_type[i] == "quat") {
+        Eigen::Vector4d q_i = Eigen::Map<const Eigen::Vector4d>(parameters[i]);
+        Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i);
+        Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1));
+        Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin);
+        res.block(local_it, 0, 3, 1) = -theta_err;
+        if (jacobians && jacobians[i]) {
+          Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], num_residuals(), 4);
+          jacobian.setZero();
+          Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse();
+          Eigen::Matrix3d H_theta = -Jr_inv * R_lin.transpose();
+          jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3) * H_theta;
+        }
+        global_it += 4;
+        local_it += 3;
+      } else if (x_type[i] == "quat_yaw") {
+        Eigen::Vector3d ez = Eigen::Vector3d(0.0, 0.0, 1.0);
+        Eigen::Vector4d q_i = Eigen::Map<const Eigen::Vector4d>(parameters[i]);
+        Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i);
+        Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1));
+        Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin);
+        res(local_it, 0) = -(ez.transpose() * theta_err)(0, 0);
+        if (jacobians && jacobians[i]) {
+          Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], num_residuals(), 4);
+          jacobian.setZero();
+          Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse();
+          Eigen::Matrix<double, 1, 3> H_theta = -ez.transpose() * (Jr_inv * R_lin.transpose());
+          jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 1) * H_theta;
+        }
+        global_it += 4;
+        local_it += 1;
+      } else if (x_type[i] == "vec3") {
+        Eigen::Matrix<double, 3, 1> p_i = Eigen::Map<const Eigen::Matrix<double, 3, 1>>(parameters[i]);
+        res.block(local_it, 0, 3, 1) = p_i - x_lin.block(global_it, 0, 3, 1);
+        if (jacobians && jacobians[i]) {
+          Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], num_residuals(), 3);
+          jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3);
+        }
+        global_it += 3;
+        local_it += 3;
+      } else if (x_type[i] == "vec8") {
+        Eigen::Matrix<double, 8, 1> p_i = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[i]);
+        res.block(local_it, 0, 8, 1) = p_i - x_lin.block(global_it, 0, 8, 1);
+        if (jacobians && jacobians[i]) {
+          Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], num_residuals(), 8);
+          jacobian.block(0, 0, num_residuals(), 8) = sqrtI.block(0, local_it, num_residuals(), 8);
+        }
+        global_it += 8;
+        local_it += 8;
+      } else {
+        std::cerr << "type - " << x_type[i] << " not implemented in prior" << std::endl;
+        std::exit(EXIT_FAILURE);
+      }
+    }
+
+    // Now that we have done x - x_lin we need to multiply by sqrtI and add b to get full cost
+    // Jacobians will already have sqrtI applied to them...
+    res = sqrtI * res;
+    res += b;
+
+    // Store the residuals into ceres
+    for (int i = 0; i < res.rows(); i++) {
+      residuals[i] = res(i, 0);
+    }
+    return true;
+  }
+};
+
+} // namespace ov_init
+
+#endif // OV_INIT_CERES_GENERICPRIOR_H

ov_init/src/ceres/Factor_ImageReprojCalib.h

@@ -0,0 +1,201 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2022 Patrick Geneva
+ * Copyright (C) 2018-2022 Guoquan Huang
+ * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef OV_INIT_CERES_IMAGEREPROJCALIB_H
+#define OV_INIT_CERES_IMAGEREPROJCALIB_H
+
+#include <Eigen/Dense>
+#include <ceres/ceres.h>
+#include <ceres/rotation.h>
+#include <deque>
+#include <iostream>
+#include <map>
+
+#include "cam/CamEqui.h"
+#include "cam/CamRadtan.h"
+#include "utils/quat_ops.h"
+
+namespace ov_init {
+
+/**
+ * @brief Factor of feature bearing observation (raw) with calibration
+ */
+class Factor_ImageReprojCalib : public ceres::CostFunction {
+public:
+  // Measurement observation of the feature (raw pixel coordinates)
+  Eigen::Vector2d uv_meas;
+
+  // Measurement noise
+  double pix_sigma = 1.0;
+  Eigen::Matrix<double, 2, 2> sqrtQ;
+
+  // If distortion model is fisheye or radtan
+  bool is_fisheye = false;
+
+  // If value of 1 then this residual adds to the problem, otherwise if zero it is "gated"
+  double gate = 1.0;
+
+  /**
+   * @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_) {
+
+    // Square root information inverse
+    sqrtQ = Eigen::Matrix<double, 2, 2>::Identity();
+    sqrtQ(0, 0) *= 1.0 / pix_sigma;
+    sqrtQ(1, 1) *= 1.0 / pix_sigma;
+
+    // Parameters we are a function of
+    set_num_residuals(2);
+    mutable_parameter_block_sizes()->push_back(4); // q_GtoIi
+    mutable_parameter_block_sizes()->push_back(3); // p_IiinG
+    mutable_parameter_block_sizes()->push_back(3); // p_FinG
+    mutable_parameter_block_sizes()->push_back(4); // q_ItoC
+    mutable_parameter_block_sizes()->push_back(3); // p_IinC
+    mutable_parameter_block_sizes()->push_back(8); // focal, center, distortion
+  }
+
+  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
+    Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
+    Eigen::Matrix3d R_GtoIi = ov_core::quat_2_Rot(q_GtoIi);
+    Eigen::Vector3d p_IiinG = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
+    Eigen::Vector3d p_FinG = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
+    Eigen::Vector4d q_ItoC = Eigen::Map<const Eigen::Vector4d>(parameters[3]);
+    Eigen::Matrix3d R_ItoC = ov_core::quat_2_Rot(q_ItoC);
+    Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);
+
+    // order: f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4
+    Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);
+
+    // Transform the feature into the current camera frame of reference
+    Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
+    Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
+
+    // Normalized projected feature bearing
+    Eigen::Vector2d uv_norm;
+    uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);
+
+    // Square-root information and gate
+    Eigen::Matrix<double, 2, 2> sqrtQ_gate = gate * sqrtQ;
+
+    // Get the distorted raw image coordinate using the camera model
+    // Also if jacobians are requested, then compute derivatives
+    Eigen::Vector2d uv_dist;
+    Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;
+    if (is_fisheye) {
+      ov_core::CamEqui cam(0, 0);
+      cam.set_value(camera_vals);
+      uv_dist = cam.distort_d(uv_norm);
+      if (jacobians) {
+        cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
+        H_dz_dzn = sqrtQ_gate * H_dz_dzn;
+        H_dz_dzeta = sqrtQ_gate * H_dz_dzeta;
+      }
+    } else {
+      ov_core::CamRadtan cam(0, 0);
+      cam.set_value(camera_vals);
+      uv_dist = cam.distort_d(uv_norm);
+      if (jacobians) {
+        cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
+        H_dz_dzn = sqrtQ_gate * H_dz_dzn;
+        H_dz_dzeta = sqrtQ_gate * H_dz_dzeta;
+      }
+    }
+
+    // Compute residual
+    // NOTE: we make this negative ceres cost function is only min||f(x)||^2
+    // NOTE: thus since we have found the derivative of uv_meas = f(x) + n
+    // NOTE: we need to reformulate into a zero error constraint 0 = f(x) + n - uv_meas
+    // NOTE: if it was the other way (0 = uv_meas - f(x) - n) all the Jacobians would need to be flipped
+    Eigen::Vector2d res = uv_dist - uv_meas;
+    res = sqrtQ_gate * res;
+    residuals[0] = res(0);
+    residuals[1] = res(1);
+
+    // Compute jacobians if requested by ceres
+    if (jacobians) {
+
+      // Normalized coordinates in respect to projection function
+      Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
+      H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2);
+      Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc;
+
+      // Jacobian wrt q_GtoIi
+      if (jacobians[0]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[0]);
+        jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * ov_core::skew_x(p_FinIi);
+        jacobian.block(0, 3, 2, 1).setZero();
+      }
+
+      // Jacobian wrt p_IiinG
+      if (jacobians[1]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[1]);
+        jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi;
+      }
+
+      // Jacobian wrt feature p_FinG
+      if (jacobians[2]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[2]);
+        jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi;
+      }
+
+      // Jacbian wrt IMU-camera transform q_ItoC
+      if (jacobians[3]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
+        jacobian.block(0, 0, 2, 3) = H_dz_dpfc * ov_core::skew_x(R_ItoC * p_FinIi);
+        jacobian.block(0, 3, 2, 1).setZero();
+      }
+
+      // Jacbian wrt IMU-camera transform p_IinC
+      if (jacobians[4]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
+        jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
+      }
+
+      // Jacbian wrt camera intrinsic
+      if (jacobians[5]) {
+        Eigen::Map<Eigen::Matrix<double, 2, 8, Eigen::RowMajor>> jacobian(jacobians[5]);
+        jacobian.block(0, 0, 2, 8) = H_dz_dzeta;
+      }
+    }
+    return true;
+  }
+};
+
+} // namespace ov_init
+
+#endif // OV_INIT_CERES_IMAGEREPROJCALIB_H

ov_init/src/ceres/Factor_ImuCPIv1.h

@@ -0,0 +1,311 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2022 Patrick Geneva
+ * Copyright (C) 2018-2022 Guoquan Huang
+ * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef OV_INIT_CERES_IMUCPIV1_H
+#define OV_INIT_CERES_IMUCPIV1_H
+
+#include <ceres/ceres.h>
+
+#include "utils/quat_ops.h"
+
+namespace ov_init {
+
+/**
+ * @brief Factor for IMU continuous preintegration version 1
+ */
+class Factor_ImuCPIv1 : public ceres::CostFunction {
+public:
+  // Preintegrated measurements and time interval
+  Eigen::Vector3d alpha;
+  Eigen::Vector3d beta;
+  Eigen::Vector4d q_breve;
+  double dt;
+
+  // Preintegration linearization points
+  Eigen::Vector3d b_w_lin_save;
+  Eigen::Vector3d b_a_lin_save;
+
+  // Prinetegrated bias jacobians
+  Eigen::Matrix3d J_q; // J_q - orientation wrt bias w
+  Eigen::Matrix3d J_a; // J_a - position wrt bias w
+  Eigen::Matrix3d J_b; // J_b - velocity wrt bias w
+  Eigen::Matrix3d H_a; // H_a - position wrt bias a
+  Eigen::Matrix3d H_b; // H_b - velocity wrt bias a
+
+  // Sqrt of the preintegration information
+  Eigen::Matrix<double, 15, 15> sqrtI_save;
+
+  // Gravity
+  Eigen::Vector3d grav_save;
+
+  /**
+   * @brief Default constructor
+   */
+  Factor_ImuCPIv1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta, Eigen::Vector4d &q_KtoK1,
+                  Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q, Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha,
+                  Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, Eigen::Matrix<double, 15, 15> &covariance) {
+    // Save measurements
+    this->alpha = alpha;
+    this->beta = beta;
+    this->q_breve = q_KtoK1;
+    this->dt = deltatime;
+    this->grav_save = grav;
+
+    // Linearization points
+    this->b_a_lin_save = ba_lin;
+    this->b_w_lin_save = bg_lin;
+
+    // Save bias jacobians
+    this->J_q = J_q;
+    this->J_a = J_alpha;
+    this->J_b = J_beta;
+    this->H_a = H_alpha;
+    this->H_b = H_beta;
+
+    // Check that we have a valid covariance matrix that we can get the information of
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(covariance.rows(), covariance.rows());
+    Eigen::MatrixXd information = covariance.llt().solve(I);
+    if (std::isnan(information.norm())) {
+      std::cerr << "P - " << std::endl << covariance << std::endl << std::endl;
+      std::cerr << "Pinv - " << std::endl << covariance.inverse() << std::endl << std::endl;
+      std::exit(EXIT_FAILURE);
+    }
+
+    // Get square-root of our information matrix
+    Eigen::LLT<Eigen::MatrixXd> lltOfI(information);
+    sqrtI_save = lltOfI.matrixL().transpose();
+
+    // Set the number of measurements, and the block sized
+    set_num_residuals(15);
+    mutable_parameter_block_sizes()->push_back(4); // q_GtoI1
+    mutable_parameter_block_sizes()->push_back(3); // bg_1
+    mutable_parameter_block_sizes()->push_back(3); // v_I1inG
+    mutable_parameter_block_sizes()->push_back(3); // ba_1
+    mutable_parameter_block_sizes()->push_back(3); // p_I1inG
+    mutable_parameter_block_sizes()->push_back(4); // q_GtoI2
+    mutable_parameter_block_sizes()->push_back(3); // bg_2
+    mutable_parameter_block_sizes()->push_back(3); // v_I2inG
+    mutable_parameter_block_sizes()->push_back(3); // ba_2
+    mutable_parameter_block_sizes()->push_back(3); // p_I2inG
+  }
+
+  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)
+    Eigen::Vector3d gravity = grav_save;
+    Eigen::Matrix<double, 15, 15> sqrtI = sqrtI_save;
+    Eigen::Vector3d b_w_lin = b_w_lin_save;
+    Eigen::Vector3d b_a_lin = b_a_lin_save;
+
+    // Get the state estimates from the ceres parameters.
+    // Each clone is stored in the canonical VINS format: q, bw, v, ba, p
+    Eigen::Vector4d q_1 = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
+    Eigen::Matrix3d R_1 = ov_core::quat_2_Rot(q_1);
+    Eigen::Vector4d q_2 = Eigen::Map<const Eigen::Vector4d>(parameters[5]);
+
+    // Get bias_w
+    Eigen::Vector3d b_w1 = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
+    Eigen::Vector3d b_w2 = Eigen::Map<const Eigen::Vector3d>(parameters[6]);
+
+    // Get bias_a
+    Eigen::Vector3d b_a1 = Eigen::Map<const Eigen::Vector3d>(parameters[3]);
+    Eigen::Vector3d b_a2 = Eigen::Map<const Eigen::Vector3d>(parameters[8]);
+
+    // Get velocity
+    Eigen::Vector3d v_1 = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
+    Eigen::Vector3d v_2 = Eigen::Map<const Eigen::Vector3d>(parameters[7]);
+
+    // Get positions
+    Eigen::Vector3d p_1 = Eigen::Map<const Eigen::Vector3d>(parameters[4]);
+    Eigen::Vector3d p_2 = Eigen::Map<const Eigen::Vector3d>(parameters[9]);
+
+    // Get the change in clone 1's biases from the linearization points
+    Eigen::Vector3d dbw = b_w1 - b_w_lin;
+    Eigen::Vector3d dba = b_a1 - b_a_lin;
+
+    // Quaternion associated with the bias w correction
+    // Eigen::Vector4d q_b= rot_2_quat(Exp(-J_q*dbw));
+    Eigen::Vector4d q_b;
+    q_b.block(0, 0, 3, 1) = 0.5 * J_q * dbw;
+    q_b(3, 0) = 1.0;
+    q_b = q_b / q_b.norm();
+
+    // Relative orientation from state estimates
+    Eigen::Vector4d q_1_to_2 = ov_core::quat_multiply(q_2, ov_core::Inv(q_1));
+
+    // Intermediate quaternions for error/jacobian computation
+    Eigen::Vector4d q_res_minus = ov_core::quat_multiply(q_1_to_2, ov_core::Inv(q_breve));
+    Eigen::Vector4d q_res_plus = ov_core::quat_multiply(q_res_minus, q_b);
+
+    //================================================================================
+    //================================================================================
+    //================================================================================
+
+    // Computer residual
+    Eigen::Matrix<double, 15, 1> res;
+    res.block(0, 0, 3, 1) = 2 * q_res_plus.block(0, 0, 3, 1);
+    res.block(3, 0, 3, 1) = b_w2 - b_w1;
+    res.block(6, 0, 3, 1) = R_1 * (v_2 - v_1 + gravity * dt) - J_b * dbw - H_b * dba - beta;
+    res.block(9, 0, 3, 1) = b_a2 - b_a1;
+    res.block(12, 0, 3, 1) = R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)) - J_a * dbw - H_a * dba - alpha;
+    res = sqrtI * res;
+
+    // Store residuals
+    for (int i = 0; i < res.rows(); i++) {
+      residuals[i] = res(i, 0);
+    }
+
+    //================================================================================
+    //================================================================================
+    //================================================================================
+
+    // Compute jacobians if asked
+    if (jacobians) {
+
+      // Stores the total preintegrated jacobian into one spot
+      Eigen::Matrix<double, 15, 30> Jacobian = Eigen::Matrix<double, 15, 30>::Zero();
+
+      // Quick identity
+      Eigen::Matrix<double, 3, 3> eye = Eigen::MatrixXd::Identity(3, 3);
+      Eigen::Matrix<double, 4, 1> q_meas_plus = ov_core::quat_multiply(ov_core::Inv(q_breve), q_b);
+
+      // Dtheta wrt theta 1
+      Jacobian.block(0, 0, 3, 3) = -((q_1_to_2(3, 0) * eye - ov_core::skew_x(q_1_to_2.block(0, 0, 3, 1))) *
+                                         (q_meas_plus(3, 0) * eye + ov_core::skew_x(q_meas_plus.block(0, 0, 3, 1))) -
+                                     q_1_to_2.block(0, 0, 3, 1) * q_meas_plus.block(0, 0, 3, 1).transpose());
+
+      // Dtheta wrt theta 2
+      Jacobian.block(0, 15, 3, 3) = q_res_plus(3, 0) * eye + ov_core::skew_x(q_res_plus.block(0, 0, 3, 1));
+
+      // Dtheta wrt bw 1
+      Jacobian.block(0, 3, 3, 3) = (q_res_minus(3, 0) * eye - ov_core::skew_x(q_res_minus.block(0, 0, 3, 1))) * J_q;
+
+      // Dbw wrt bw1 and bw2
+      Jacobian.block(3, 3, 3, 3) = -eye;
+      Jacobian.block(3, 18, 3, 3) = eye;
+
+      // Dvelocity wrt theta 1
+      Jacobian.block(6, 0, 3, 3) = ov_core::skew_x(R_1 * (v_2 - v_1 + gravity * dt));
+
+      // Dvelocity wrt v 1
+      Jacobian.block(6, 6, 3, 3) = -R_1;
+
+      // Dvelocity wrt v 2
+      Jacobian.block(6, 21, 3, 3) = R_1;
+
+      // Dvelocity wrt bw 1
+      Jacobian.block(6, 3, 3, 3) = -J_b;
+
+      // Dvelocity wrt ba 1
+      Jacobian.block(6, 9, 3, 3) = -H_b;
+
+      // Dbw wrt ba1 and ba2
+      Jacobian.block(9, 9, 3, 3) = -eye;
+      Jacobian.block(9, 24, 3, 3) = eye;
+
+      // Dposition wrt theta 1
+      Jacobian.block(12, 0, 3, 3) = ov_core::skew_x(R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)));
+      // Dposition wrt v 1
+      Jacobian.block(12, 6, 3, 3) = -R_1 * dt;
+      // Dposition wrt p 1
+      Jacobian.block(12, 12, 3, 3) = -R_1;
+
+      // Dposition wrt p 2
+      Jacobian.block(12, 27, 3, 3) = R_1;
+
+      // Dposition wrt bw 1
+      Jacobian.block(12, 3, 3, 3) = -J_a;
+      // Dposition wrt ba 1
+      Jacobian.block(12, 9, 3, 3) = -H_a;
+
+      // Apply sqrt info
+      Jacobian = sqrtI * Jacobian;
+
+      // Now store jacobians
+      // Th1
+      if (jacobians[0]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> J_th1(jacobians[0], 15, 4);
+        J_th1.block(0, 0, 15, 3) = Jacobian.block(0, 0, 15, 3);
+        J_th1.block(0, 3, 15, 1).setZero();
+      }
+      // Th2
+      if (jacobians[5]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> J_th2(jacobians[5], 15, 4);
+        J_th2.block(0, 0, 15, 3) = Jacobian.block(0, 15, 15, 3);
+        J_th2.block(0, 3, 15, 1).setZero();
+      }
+      // bw1
+      if (jacobians[1]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_bw1(jacobians[1], 15, 3);
+        J_bw1.block(0, 0, 15, 3) = Jacobian.block(0, 3, 15, 3);
+      }
+      // bw2
+      if (jacobians[6]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_bw2(jacobians[6], 15, 3);
+        J_bw2.block(0, 0, 15, 3) = Jacobian.block(0, 18, 15, 3);
+      }
+      // v1
+      if (jacobians[2]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_v1(jacobians[2], 15, 3);
+        J_v1.block(0, 0, 15, 3) = Jacobian.block(0, 6, 15, 3);
+      }
+      // v2
+      if (jacobians[7]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_v2(jacobians[7], 15, 3);
+        J_v2.block(0, 0, 15, 3) = Jacobian.block(0, 21, 15, 3);
+      }
+      // ba1
+      if (jacobians[3]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_ba1(jacobians[3], 15, 3);
+        J_ba1.block(0, 0, 15, 3) = Jacobian.block(0, 9, 15, 3);
+      }
+      // ba2
+      if (jacobians[8]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_ba2(jacobians[8], 15, 3);
+        J_ba2.block(0, 0, 15, 3) = Jacobian.block(0, 24, 15, 3);
+      }
+      // p1
+      if (jacobians[4]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_p1(jacobians[4], 15, 3);
+        J_p1.block(0, 0, 15, 3) = Jacobian.block(0, 12, 15, 3);
+      }
+      // p2
+      if (jacobians[9]) {
+        Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_p2(jacobians[9], 15, 3);
+        J_p2.block(0, 0, 15, 3) = Jacobian.block(0, 27, 15, 3);
+      }
+    }
+    return true;
+  }
+};
+
+} // namespace ov_init
+
+#endif // OV_INIT_CERES_IMUCPIV1_H

ov_init/src/ceres/State_JPLQuatLocal.h

@@ -0,0 +1,94 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2022 Patrick Geneva
+ * Copyright (C) 2018-2022 Guoquan Huang
+ * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef OV_INIT_CERES_JPLQUATLOCAL_H
+#define OV_INIT_CERES_JPLQUATLOCAL_H
+
+#include <ceres/ceres.h>
+
+#include "utils/quat_ops.h"
+
+namespace ov_init {
+
+/**
+ * @brief JPL quaternion CERES state parameterization
+ */
+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
+    Eigen::Map<const Eigen::Vector4d> q(x);
+
+    // Get delta into eigen
+    Eigen::Map<const Eigen::Vector3d> d_th(delta);
+    Eigen::Matrix<double, 4, 1> d_q;
+    double theta = d_th.norm();
+    if (theta < 1e-8) {
+      d_q << .5 * d_th, 1.0;
+    } else {
+      d_q.block(0, 0, 3, 1) = (d_th / theta) * std::sin(theta / 2);
+      d_q(3, 0) = std::cos(theta / 2);
+    }
+    d_q = ov_core::quatnorm(d_q);
+
+    // Do the update
+    Eigen::Map<Eigen::Vector4d> q_plus(x_plus_delta);
+    q_plus = ov_core::quat_multiply(d_q, q);
+    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 {
+    Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
+    j.topRows<3>().setIdentity();
+    j.bottomRows<1>().setZero();
+    return true;
+  }
+
+  int GlobalSize() const override { return 4; };
+
+  int LocalSize() const override { return 3; };
+};
+
+} // namespace ov_init
+
+#endif // OV_INIT_CERES_JPLQUATLOCAL_H