    bool imu_avg = false;
    double dt = 0;
    Eigen::Matrix<double, 3, 1> alpha_tau = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 3, 1> beta_tau = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 4, 1> q_tau_k;
    Eigen::Matrix<double, 3, 3> R_tau_k = Eigen::Matrix<double, 3, 3>::Identity();
    Eigen::Matrix<double, 3, 3> J_q = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> J_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> J_b = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> H_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> H_b = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 1> b_w_lin;
    Eigen::Matrix<double, 3, 1> b_a_lin;
    Eigen::Matrix<double, 4, 1> q_k_lin;
    Eigen::Matrix<double, 3, 1> grav = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 12, 12> Q_c = Eigen::Matrix<double, 12, 12>::Zero();
    Eigen::Matrix<double, 15, 15> P_meas = Eigen::Matrix<double, 15, 15>::Zero();
    Eigen::Matrix<double, 3, 1> e_1;
    Eigen::Matrix<double, 3, 1> e_2;
    Eigen::Matrix<double, 3, 1> e_3;
    Eigen::Matrix<double, 3, 3> e_1x;
    Eigen::Matrix<double, 3, 3> e_2x;
    Eigen::Matrix<double, 3, 3> e_3x;
  public:
    void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1;
	double f_2;
	double f_3;
	double f_4;

	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);

	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;

	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;

	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;

	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;

	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}

	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;

	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;

	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;

	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();

	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();

	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;

	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();

	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();

	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());

	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);

	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;

	P_meas = P_big.block(0, 0, 15, 15);

	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
  private:
    Eigen::Matrix<double, 21, 21> P_big = Eigen::Matrix<double, 21, 21>::Zero();
    Eigen::Matrix<double, 21, 21> Discrete_J_b = Eigen::Matrix<double, 21, 21>::Identity();
  public:
    bool state_transition_jacobians = true;
    Eigen::Matrix<double, 3, 3> O_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> O_b = Eigen::Matrix<double, 3, 3>::Zero();
    void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1;
	double f_2;
	double f_3;
	double f_4;

	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);

	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;

	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;

	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;

	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;

	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}

	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;

	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;

	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;

	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();

	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();

	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;

	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();

	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();

	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());

	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);

	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;

	P_meas = P_big.block(0, 0, 15, 15);

	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
  public:
    bool Plus(const double *x, const double *delta, double *x_plus_delta) const override {
	Eigen::Map<const Eigen::Vector4d> q(x);

	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 << 0.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 = quatType::quatNorm(d_q);

	Eigen::Map<Eigen::Vector4d> q_plus(x_plus_delta);
	q_plus = quatType::quatMultiply(d_q, q);

	return true;
}
    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;
}
  public:
    Eigen::MatrixXd x_lin;
    std::vector<std::string> x_type;
    Eigen::MatrixXd sqrt_I;
    Eigen::MatrixXd b;
    factorGenericPrior(const Eigen::MatrixXd &x_lin_, const std::vector<std::string> &x_type_, const Eigen::MatrixXd &prior_Info,
		const Eigen::MatrixXd &prior_grad) {
	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 == "vec1")
		{
			state_size += 1;
			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);

	Eigen::LLT<Eigen::MatrixXd> llt_of_I(prior_Info);
	sqrt_I = llt_of_I.matrixL().transpose();
	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(prior_Info.rows(), prior_Info.rows());
	b = sqrt_I.triangularView<Eigen::Upper>().solve(I) * prior_grad;

	if (std::isnan(prior_Info.norm()) || std::isnan(sqrt_I.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_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 == "vec1")
			mutable_parameter_block_sizes()->push_back(1);
		if (str == "vec3")
			mutable_parameter_block_sizes()->push_back(3);
		if (str == "vec8")
			mutable_parameter_block_sizes()->push_back(8);
	}
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(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 = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		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;

		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 * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		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;
		}

		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;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		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;
}
  public:
    Eigen::Vector3d alpha;
    Eigen::Vector3d beta;
    Eigen::Vector4d q_breve;
    double dt;
    Eigen::Vector3d b_w_lin_save;
    Eigen::Vector3d b_a_lin_save;
    Eigen::Matrix3d J_q;
    Eigen::Matrix3d J_a;
    Eigen::Matrix3d J_b;
    Eigen::Matrix3d H_a;
    Eigen::Matrix3d H_b;
    Eigen::Matrix<double, 15, 15> sqrt_I_save;
    Eigen::Vector3d grav_save;
    factorImuPreIntegrationV1(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) {
	this->alpha = alpha;
	this->beta = beta;
	this->q_breve = q_KtoK1;
	this->dt = deltatime;
	this->grav_save = grav;

	this->b_a_lin_save = ba_lin;
	this->b_w_lin_save = bg_lin;

	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;

	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);
	}

	Eigen::LLT<Eigen::MatrixXd> llt_of_I(information);
	sqrt_I_save = llt_of_I.matrixL().transpose();

	set_num_residuals(15);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(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 = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		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;

		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 * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		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;
		}

		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;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		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;
}
  public:
    Eigen::Vector2d uv_meas;
    double pix_sigma = 1.0;
    Eigen::Matrix<double, 2, 2> sqrt_Q;
    bool is_fisheye = false;
    double gate = 1.0;
    factorImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_) {
	sqrt_Q = Eigen::Matrix<double, 2, 2>::Identity();
	sqrt_Q(0, 0) *= 1.0 / pix_sigma;
	sqrt_Q(1, 1) *= 1.0 / pix_sigma;

	set_num_residuals(2);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(8);
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(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 = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		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;

		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 * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		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;
		}

		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;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		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;
}
};

class preIntegrationBase {
  public:
    preIntegrationBase(double sigma_w_, double sigma_wb_, double sigma_a_, double sigma_ab_, bool imu_avg_ = false) {
	Q_c.block(0, 0, 3, 3) = std::pow(sigma_w_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(3, 3, 3, 3) = std::pow(sigma_wb_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(6, 6, 3, 3) = std::pow(sigma_a_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(9, 9, 3, 3) = std::pow(sigma_ab_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	imu_avg = imu_avg_;
	e_1 << 1, 0, 0;
	e_2 << 0, 1, 0;
	e_3 << 0, 0, 1;
	e_1x = quatType::skewSymmetric(e_1);
	e_2x = quatType::skewSymmetric(e_2);
	e_3x = quatType::skewSymmetric(e_3);
}
    void setLinearizationPoints(Eigen::Matrix<double, 3, 1> b_w_lin_, Eigen::Matrix<double, 3, 1> b_a_lin_,
		Eigen::Matrix<double, 4, 1> q_k_lin_ = Eigen::Matrix<double, 4, 1>::Zero(), Eigen::Matrix<double, 3, 1> grav_ = Eigen::Matrix<double, 3, 1>::Zero()) {
	b_w_lin = b_w_lin_;
	b_a_lin = b_a_lin_;
	q_k_lin = q_k_lin_;
	grav = grav_;
}
    virtual void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) = 0 {
	double delta_t = t_1 - t_0;
	dt += delta_t;
	if (delta_t == 0) return;
	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;
	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}
	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;
	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);
	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;
	bool small_w = (mag_w < 0.008726646);
	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);
	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;
	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;
	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();
	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);
	double f_1;
	double f_2;
	double f_3;
	double f_4;
	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}
	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);
	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;
	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;
	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;
	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;
	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;
	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);
	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);
	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;
	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;
	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;
	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;
	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;
		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;
		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;
		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;
		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;
		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;
		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;
	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;
	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;
	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();
	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();
	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;
	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();
	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();
	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());
	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);
	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;
	P_meas = P_big.block(0, 0, 15, 15);
	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}
	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}

class propagator {
  public:
    propagator(noiseManager noises_, double gravity_mag_) {
	noises.sigma_w_2 = std::pow(noises.sigma_w, 2);
	noises.sigma_a_2 = std::pow(noises.sigma_a, 2);
	noises.sigma_wb_2 = std::pow(noises.sigma_wb, 2);
	noises.sigma_ab_2 = std::pow(noises.sigma_ab, 2);

	gravity << 0.0, 0.0, gravity_mag_;

	have_last_prop_time_offset = false;
	last_prop_time_offset = 0.0;
}
    void feedImu(const imuData &imu_data, double oldest_time = -1) {
	v_imu_data.emplace_back(imu_data);

	cleanOldImuMeasurements(oldest_time - 0.10);
}
    void cleanOldImuMeasurements(double oldest_time) {
	if (oldest_time < 0)
		return;
	auto it0 = v_imu_data.begin();
	while (it0 != v_imu_data.end())
	{
		if (it0->timestamp < oldest_time)
		{
			it0 = v_imu_data.erase(it0);
		}
		else
		{
			it0++;
		}
	}
}
    void invalidateCache() {
	cache_imu_valid = false;
}
    void resetTransitionMatrices(std::shared_ptr<state> state_ptr);
    static imuData interpolateData(const imuData &imu_1, const imuData &imu_2, double timestamp) {
	double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);

	imuData data;
	data.timestamp = timestamp;
	data.acc = (1 - lambda) * imu_1.acc + lambda * imu_2.acc;
	data.gyr = (1 - lambda) * imu_1.gyr + lambda * imu_2.gyr;

	return data;
}
    static std::vector<imuData> selectImuReadings(const std::vector<imuData> &v_imu_data_, double time_0, double time_1, bool warn = true) {
	std::vector<imuData> prop_data;

	if (v_imu_data_.empty())
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements. IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	for (size_t i = 0; i < v_imu_data_.size() - 1; i++)
	{
		if (v_imu_data_.at(i + 1).timestamp > time_0 && v_imu_data_.at(i).timestamp < time_0)
		{
			imuData data = propagator::interpolateData(v_imu_data_.at(i), v_imu_data_.at(i + 1), time_0);
			prop_data.push_back(data);

			continue;
		}

		if (v_imu_data_.at(i).timestamp >= time_0 && v_imu_data_.at(i + 1).timestamp <= time_1)
		{
			prop_data.push_back(v_imu_data_.at(i));
			continue;
		}

		if (v_imu_data_.at(i + 1).timestamp > time_1)
		{
			if (v_imu_data_.at(i).timestamp > time_1 && i == 0)
			{
				break;
			}
			else if (v_imu_data_.at(i).timestamp > time_1)
			{
				imuData data = propagator::interpolateData(v_imu_data_.at(i - 1), v_imu_data_.at(i), time_1);
				prop_data.push_back(data);
			}
			else
			{
				prop_data.push_back(v_imu_data_.at(i));
			}

			if (prop_data.at(prop_data.size() - 1).timestamp != time_1)
			{
				imuData data = propagator::interpolateData(v_imu_data_.at(i), v_imu_data_.at(i + 1), time_1);
				prop_data.push_back(data);
			}

			break;
		}
	}

	if (prop_data.empty())
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements to propagate with (" << (int)prop_data.size() << " of 2). IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	if (prop_data.at(prop_data.size() - 1).timestamp != time_1)
	{
		if (warn)
			std::cout << std::fixed << "propagator::selectImuReadings(): Missing inertial measurements to propagate with (" << time_1 - v_imu_data_.at(v_imu_data_.size() - 1).timestamp
				<< " sec missing)" << std::endl;

		imuData data = propagator::interpolateData(v_imu_data_.at(v_imu_data_.size() - 2), v_imu_data_.at(v_imu_data_.size() - 1), time_1);
		prop_data.push_back(data);
	}

	for (size_t i = 0; i < prop_data.size() - 1; i++)
	{
		if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12)
		{
			if (warn)
				std::cout << "propagator::selectImuReadings(): Zero DT between IMU reading " << (int)i << " and " << (int)(i + 1) << ", removing it" << std::endl;

			prop_data.erase(prop_data.begin() + i);
			i--;
		}
	}

	if (prop_data.size() < 2)
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements to propagate with (" << (int)prop_data.size() << " of 2). IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	return prop_data;
}
    void propagateAndClone(std::shared_ptr<state> state_ptr, double timestamp) {
	if (state_ptr->timestamp == timestamp)
	{
		std::cout << "propagator::propagateAndClone(): Propagation called again at same timestep at last update timestep!" << std::endl;
		std::exit(EXIT_FAILURE);
	}

	if (state_ptr->timestamp > timestamp)
	{
		std::cout << "propagator::propagateAndClone(): Propagation called trying to propagate backwards in time!" << std::endl;
		std::cout << std::fixed << "propagator::propagateAndClone(): desired propagation = " << timestamp - state_ptr->timestamp << std::endl;
		std::exit(EXIT_FAILURE);
	}

	if (!have_last_prop_time_offset)
	{
		last_prop_time_offset = state_ptr->calib_dt_imu_cam->value()(0);
		have_last_prop_time_offset = true;
	}

	double t_off_new = state_ptr->calib_dt_imu_cam->value()(0);

	double time_0 = state_ptr->timestamp + last_prop_time_offset;
	double time_1 = timestamp + t_off_new;
	std::vector<imuData> prop_data;
	{
		prop_data = propagator::selectImuReadings(v_imu_data, time_0, time_1);
	}

	Eigen::MatrixXd phi_sum = Eigen::MatrixXd::Identity(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Eigen::MatrixXd Qd_sum = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	double dt_sum = 0;

	if (prop_data.size() > 1)
	{
		for (size_t i = 0; i < prop_data.size() - 1; i++)
		{
			Eigen::MatrixXd F, Qdi;
			predictAndCompute(state_ptr, prop_data.at(i), prop_data.at(i + 1), F, Qdi);

			phi_sum = F * phi_sum;
			Qd_sum = F * Qd_sum * F.transpose() + Qdi;
			Qd_sum = 0.5 * (Qd_sum + Qd_sum.transpose());
			dt_sum += prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp;
		}
	}
	assert(std::abs((time_1 - time_0) - dt_sum) < 1e-4);

	Eigen::Vector3d last_acc = Eigen::Vector3d::Zero();
	Eigen::Vector3d last_gyr = Eigen::Vector3d::Zero();
	if (!prop_data.empty())
	{
		Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
		Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
		Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());
		last_acc = state_ptr->calib_imu_acc->getRot() * Da * (prop_data.at(prop_data.size() - 1).acc - state_ptr->imu_ptr->getBa());
		last_gyr = state_ptr->calib_imu_gyr->getRot() * Dw * (prop_data.at(prop_data.size() - 1).gyr - state_ptr->imu_ptr->getBg() - Tg * last_acc);
	}

	std::vector<std::shared_ptr<baseType>> phi_order;
	phi_order.push_back(state_ptr->imu_ptr);

	if (state_ptr->options.do_calib_imu_intrinsics)
	{
		phi_order.push_back(state_ptr->calib_imu_dw);
		phi_order.push_back(state_ptr->calib_imu_da);

		if (state_ptr->options.do_calib_imu_g_sensitivity)
		{
			phi_order.push_back(state_ptr->calib_imu_tg);
		}

		if (state_ptr->options.imu_model == ImuModel::KALIBR)
		{
			phi_order.push_back(state_ptr->calib_imu_gyr);
		}
		else
		{
			phi_order.push_back(state_ptr->calib_imu_acc);
		}
	}

	stateHelper::ekfPropagation(state_ptr, phi_order, phi_order, phi_sum, Qd_sum);

	state_ptr->timestamp = timestamp;
	last_prop_time_offset = t_off_new;

	stateHelper::augmentClone(state_ptr, last_gyr);
}
    void predictAndCompute(std::shared_ptr<state> state_ptr, const imuData &data_minus, const imuData &data_plus,
    Eigen::MatrixXd &F, Eigen::MatrixXd &Qd) {
	double dt = data_plus.timestamp - data_minus.timestamp;

	Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
	Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
	Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());

	Eigen::Vector3d a_hat1 = data_minus.acc - state_ptr->imu_ptr->getBa();
	Eigen::Vector3d a_hat2 = data_plus.acc - state_ptr->imu_ptr->getBa();
	Eigen::Vector3d a_hat_avg = 0.5 * (a_hat1 + a_hat2);

	Eigen::Vector3d a_uncorrected = a_hat_avg;
	Eigen::Matrix3d R_imu_acc = state_ptr->calib_imu_acc->getRot();
	a_hat1 = R_imu_acc * Da * a_hat1;
	a_hat2 = R_imu_acc * Da * a_hat2;
	a_hat_avg = R_imu_acc * Da * a_hat_avg;

	Eigen::Vector3d w_hat1 = data_minus.gyr - state_ptr->imu_ptr->getBg() - Tg * a_hat1;
	Eigen::Vector3d w_hat2 = data_plus.gyr - state_ptr->imu_ptr->getBg() - Tg * a_hat2;
	Eigen::Vector3d w_hat_avg = 0.5 * (w_hat1 + w_hat2);

	Eigen::Vector3d w_uncorrected = w_hat_avg;
	Eigen::Matrix3d R_imu_gyr = state_ptr->calib_imu_gyr->getRot();
	w_hat1 = R_imu_gyr * Dw * w_hat1;
	w_hat2 = R_imu_gyr * Dw * w_hat2;
	w_hat_avg = R_imu_gyr * Dw * w_hat_avg;

	Eigen::Matrix<double, 3, 18> Xi_sum = Eigen::Matrix<double, 3, 18>::Zero(3, 18);
	computeXiSum(state_ptr, dt, w_hat_avg, a_hat_avg, Xi_sum);

	Eigen::Vector4d new_q;
	Eigen::Vector3d new_v, new_p;
	predictMeanRk4(state_ptr, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p);

	F = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, 12);
	computeCoefficientAnalytic(state_ptr, dt, w_hat_avg, a_hat_avg, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G);

	Eigen::Matrix<double, 12, 12> Qc = Eigen::Matrix<double, 12, 12>::Zero();
	Qc.block(0, 0, 3, 3) = std::pow(noises.sigma_w, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(3, 3, 3, 3) = std::pow(noises.sigma_a, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(6, 6, 3, 3) = std::pow(noises.sigma_wb, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(9, 9, 3, 3) = std::pow(noises.sigma_ab, 2) / dt * Eigen::Matrix3d::Identity();

	Qd = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Qd = G * Qc * G.transpose();
	Qd = 0.5 * (Qd + Qd.transpose());

	Eigen::Matrix<double, 16, 1> imu_x = state_ptr->imu_ptr->value();
	imu_x.block(0, 0, 4, 1) = new_q;
	imu_x.block(4, 0, 3, 1) = new_p;
	imu_x.block(7, 0, 3, 1) = new_v;
	state_ptr->imu_ptr->setValue(imu_x);
	state_ptr->imu_ptr->setFej(imu_x);
}
    static Eigen::MatrixXd computeDwH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &gyr_uncorrected) {
	Eigen::Vector3d e_1 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 0);
	Eigen::Vector3d e_2 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 1);
	Eigen::Vector3d e_3 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 2);

	double w_1 = gyr_uncorrected(0);
	double w_2 = gyr_uncorrected(1);
	double w_3 = gyr_uncorrected(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);

	Eigen::MatrixXd H_Dw = Eigen::MatrixXd::Zero(3, 6);
	if (state_ptr->options.imu_model == ImuModel::KALIBR)
		H_Dw << w_1 * Eigen::MatrixXd::Identity(3, 3), w_2 * e_2, w_2 * e_3, w_3 * e_3;
	else
		H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Dw;
}
    static Eigen::MatrixXd computeDaH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &acc_uncorrected) {
	Eigen::Vector3d e_1 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 0);
	Eigen::Vector3d e_2 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 1);
	Eigen::Vector3d e_3 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 2);

	double a_1 = acc_uncorrected(0);
	double a_2 = acc_uncorrected(1);
	double a_3 = acc_uncorrected(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);

	Eigen::MatrixXd H_Da = Eigen::MatrixXd::Zero(3, 6);
	if (state_ptr->options.imu_model == ImuModel::KALIBR)
		H_Da << a_1 * Eigen::MatrixXd::Identity(3, 3), a_2 * e_2, a_2 * e_3, a_3 * e_3;
	else
		H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Da;
}
    static Eigen::MatrixXd computeTgH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &acc) {
	double a_1 = acc(0);
	double a_2 = acc(1);
	double a_3 = acc(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);
	assert(state_ptr->options.do_calib_imu_g_sensitivity);

	Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3, 9);
	H_Tg << a_1 * Eigen::MatrixXd::Identity(3, 3), a_2 * Eigen::MatrixXd::Identity(3, 3), a_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Tg;
}
    bool have_last_prop_time_offset;
    double last_prop_time_offset;
  protected:
    void predictMeanRk4(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr_0, const Eigen::Vector3d &un_acc_0,
    const Eigen::Vector3d &un_gyr_1, const Eigen::Vector3d &un_acc_1, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) {
	Eigen::Vector3d un_gyr = un_gyr_0;
	Eigen::Vector3d un_acc = un_acc_0;
	Eigen::Vector3d w_alpha = (un_gyr_1 - un_gyr_0) / dt;
	Eigen::Vector3d a_jerk = (un_acc_1 - un_acc_0) / dt;

	Eigen::Vector4d q_0 = state_ptr->imu_ptr->getQuat();
	Eigen::Vector3d p_0 = state_ptr->imu_ptr->getPos();
	Eigen::Vector3d v_0 = state_ptr->imu_ptr->getVel();

	Eigen::Vector4d dq_0 = {0, 0, 0, 1};
	Eigen::Vector4d q_0_dot = 0.5 * quatType::omega(un_gyr) * dq_0;
	Eigen::Vector3d p_0_dot = v_0;
	Eigen::Matrix3d R_0_world = quatType::quatToRot(quatType::quatMultiply(dq_0, q_0));
	Eigen::Vector3d v_0_dot = R_0_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k1_q = q_0_dot * dt;
	Eigen::Vector3d k1_p = p_0_dot * dt;
	Eigen::Vector3d k1_v = v_0_dot * dt;

	un_gyr += 0.5 * w_alpha * dt;
	un_acc += 0.5 * a_jerk * dt;

	Eigen::Vector4d dq_1 = quatType::quatNorm(dq_0 + 0.5 * k1_q);
	Eigen::Vector3d v_1 = v_0 + 0.5 * k1_v;
	Eigen::Vector4d q_1_dot = 0.5 * quatType::omega(un_gyr) * dq_1;
	Eigen::Vector3d p_1_dot = v_1;
	Eigen::Matrix3d R_1_world = quatType::quatToRot(quatType::quatMultiply(dq_1, q_0));
	Eigen::Vector3d v_1_dot = R_1_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k2_q = q_1_dot * dt;
	Eigen::Vector3d k2_p = p_1_dot * dt;
	Eigen::Vector3d k2_v = v_1_dot * dt;

	Eigen::Vector4d dq_2 = quatType::quatNorm(dq_0 + 0.5 * k2_q);
	Eigen::Vector3d v_2 = v_0 + 0.5 * k2_v;

	Eigen::Vector4d q_2_dot = 0.5 * quatType::omega(un_gyr) * dq_2;
	Eigen::Vector3d p_2_dot = v_2;
	Eigen::Matrix3d R_2_world = quatType::quatToRot(quatType::quatMultiply(dq_2, q_0));
	Eigen::Vector3d v_2_dot = R_2_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k3_q = q_2_dot * dt;
	Eigen::Vector3d k3_p = p_2_dot * dt;
	Eigen::Vector3d k3_v = v_2_dot * dt;

	un_gyr += 0.5 * w_alpha * dt;
	un_acc += 0.5 * a_jerk * dt;

	Eigen::Vector4d dq_3 = quatType::quatNorm(dq_0 + k3_q);
	Eigen::Vector3d v_3 = v_0 + k3_v;

	Eigen::Vector4d q_3_dot = 0.5 * quatType::omega(un_gyr) * dq_3;
	Eigen::Vector3d p_3_dot = v_3;
	Eigen::Matrix3d R_3_world = quatType::quatToRot(quatType::quatMultiply(dq_3, q_0));
	Eigen::Vector3d v_3_dot = R_3_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k4_q = q_3_dot * dt;
	Eigen::Vector3d k4_p = p_3_dot * dt;
	Eigen::Vector3d k4_v = v_3_dot * dt;

	Eigen::Vector4d dq = quatType::quatNorm(dq_0 + (1.0 / 6.0) * k1_q + (1.0 / 3.0) * k2_q + (1.0 / 3.0) * k3_q + (1.0 / 6.0) * k4_q);
	new_q = quatType::quatMultiply(dq, q_0);
	new_p = p_0 + (1.0 / 6.0) * k1_p + (1.0 / 3.0) * k2_p + (1.0 / 3.0) * k3_p + (1.0 / 6.0) * k4_p;
	new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v;
}
    void computeXiSum(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr,
    const Eigen::Vector3d &un_acc, Eigen::Matrix<double, 3, 18> &Xi_sum) {
	double un_gyr_norm = un_gyr.norm();
	double d_th = un_gyr_norm * dt;
	Eigen::Vector3d un_gyr_normalized = Eigen::Vector3d::Zero();

	if (un_gyr_norm > 1e-12)
		un_gyr_normalized = un_gyr / un_gyr_norm;

	double dt_2 = std::pow(dt, 2);
	double dt_3 = std::pow(dt, 3);
	double norm_2 = std::pow(un_gyr_norm, 2);
	double norm_3 = std::pow(un_gyr_norm, 3);
	double cos_dth = std::cos(d_th);
	double sin_dth = std::sin(d_th);
	double d_th_2 = std::pow(d_th, 2);
	double d_th_3 = std::pow(d_th, 3);

	Eigen::Matrix3d sK = quatType::skewSymmetric(un_gyr_normalized);
	Eigen::Matrix3d sK2 = sK * sK;
	Eigen::Matrix3d sA = quatType::skewSymmetric(un_acc);

	Eigen::Matrix3d R_temp = quatType::expSo3(-un_gyr * dt);
	Eigen::Matrix3d Jr_temp = quatType::JrighySo3(-un_gyr * dt);

	Eigen::Matrix3d Xi_1, Xi_2, Xi_3, Xi_4;

  	if (!(un_gyr_norm < 1.0 / 180 * M_PI / 2))
  	{
		Xi_1 = Eigen::Matrix3d::Identity() * dt + (1.0 - cos_dth) / un_gyr_norm * sK + (dt - sin_dth / un_gyr_norm) * sK2;

		Xi_2 = 1.0 / 2 * dt_2 * Eigen::Matrix3d::Identity() + (d_th - sin_dth) / norm_2 * sK + (1.0 / 2 * dt_2 - (1.0 - cos_dth) / norm_2) * sK2;

		Xi_3 = 1.0 / 2 * dt_2 * sA + (sin_dth - d_th) / norm_2 * sA * sK + (sin_dth - d_th * cos_dth) / norm_2 * sK * sA +
			(1.0 / 2 * dt_2 - (1.0 - cos_dth) / norm_2) * sA * sK2 +
			(1.0 / 2 * dt_2 + (1.0 - cos_dth - d_th * sin_dth) / norm_2) * (sK2 * sA + un_gyr_normalized.dot(un_acc) * sK) -
			(3 * sin_dth - 2 * d_th - d_th * cos_dth) / norm_2 * un_gyr_normalized.dot(un_acc) * sK2;

		Xi_4 = 1.0 / 6 * dt_3 * sA + (2 * (1.0 - cos_dth) - d_th_2) / (2 * norm_3) * sA * sK +
			((2 * (1.0 - cos_dth) - d_th * sin_dth) / norm_3) * sK * sA + ((sin_dth - d_th) / norm_3 + dt_3 / 6) * sA * sK2 +
			((d_th - 2 * sin_dth + 1.0 / 6 * d_th_3 + d_th * cos_dth) / norm_3) * (sK2 * sA + un_gyr_normalized.dot(un_acc) * sK) +
			(4 * cos_dth - 4 + d_th_2 + d_th * sin_dth) / norm_3 * un_gyr_normalized.dot(un_acc) * sK2;

	}
	else
	{
		Xi_1 = dt * (Eigen::Matrix3d::Identity() + sin_dth * sK + (1.0 - cos_dth) * sK2);

		Xi_2 = 1.0 / 2 * dt * Xi_1;

		Xi_3 = 1.0 / 2 * dt_2 * (sA + sin_dth * (-sA * sK + sK * sA + un_gyr_normalized.dot(un_acc) * sK2)
			+ (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + un_gyr_normalized.dot(un_acc) * sK));

		Xi_4 = 1.0 / 3 * dt * Xi_3;
	}

	Xi_sum.setZero();
	Xi_sum.block<3, 3>(0, 0) = R_temp;
	Xi_sum.block<3, 3>(0, 3) = Xi_1;
	Xi_sum.block<3, 3>(0, 6) = Xi_2;
	Xi_sum.block<3, 3>(0, 9) = Jr_temp;
	Xi_sum.block<3, 3>(0, 12) = Xi_3;
	Xi_sum.block<3, 3>(0, 15) = Xi_4;
}
    void computeCoefficientAnalytic(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr, const Eigen::Vector3d &un_acc,
    const Eigen::Vector3d &gyr_uncorrected, const Eigen::Vector3d &acc_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v,
    const Eigen::Vector3d &new_p, const Eigen::Matrix<double, 3, 18> &Xi_sum, Eigen::MatrixXd &F, Eigen::MatrixXd &G) {
	int local_size = 0;
	int th_id = local_size;
	local_size += state_ptr->imu_ptr->q()->getSize();
	int p_id = local_size;
	local_size += state_ptr->imu_ptr->p()->getSize();
	int v_id = local_size;
	local_size += state_ptr->imu_ptr->v()->getSize();
	int bg_id = local_size;
	local_size += state_ptr->imu_ptr->bg()->getSize();
	int ba_id = local_size;
	local_size += state_ptr->imu_ptr->ba()->getSize();

	int Dw_id = -1;
	int Da_id = -1;
	int Tg_id = -1;
	int th_imu_acc_id = -1;
	int th_imu_gyr_id = -1;

	if (state_ptr->options.do_calib_imu_intrinsics)
	{
		Dw_id = local_size;
		local_size += state_ptr->calib_imu_dw->getSize();
		Da_id = local_size;
		local_size += state_ptr->calib_imu_da->getSize();

		if (state_ptr->options.do_calib_imu_g_sensitivity)
		{
			Tg_id = local_size;
			local_size += state_ptr->calib_imu_tg->getSize();
		}

		if (state_ptr->options.imu_model == ImuModel::KALIBR)
		{
			th_imu_gyr_id = local_size;
			local_size += state_ptr->calib_imu_gyr->getSize();
		}
		else
		{
			th_imu_acc_id = local_size;
			local_size += state_ptr->calib_imu_acc->getSize();
		}
	}

	Eigen::Matrix3d R_k = state_ptr->imu_ptr->getRot();
	Eigen::Vector3d v_k = state_ptr->imu_ptr->getVel();
	Eigen::Vector3d p_k = state_ptr->imu_ptr->getPos();

	if (state_ptr->options.do_fej)
	{
		R_k = state_ptr->imu_ptr->getRotFej();
		v_k = state_ptr->imu_ptr->getVelFej();
		p_k = state_ptr->imu_ptr->getPosFej();
	}

	Eigen::Matrix3d dR_temp = quatType::quatToRot(new_q) * R_k.transpose();

	Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
	Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
	Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());
	Eigen::Matrix3d R_imu_acc = state_ptr->calib_imu_acc->getRot();
	Eigen::Matrix3d R_imu_gyr = state_ptr->calib_imu_gyr->getRot();
	Eigen::Vector3d acc_k = R_imu_acc * Da * acc_uncorrected;
	Eigen::Vector3d gyr_k = R_imu_gyr * Dw * gyr_uncorrected;

	Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3);
	Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6);
	Eigen::Matrix3d Jr_temp = Xi_sum.block<3, 3>(0, 9);
	Eigen::Matrix3d Xi_3 = Xi_sum.block<3, 3>(0, 12);
	Eigen::Matrix3d Xi_4 = Xi_sum.block<3, 3>(0, 15);

	F.block<3, 3>(th_id, th_id) = dR_temp;
	F.block<3, 3>(p_id, th_id) = - quatType::skewSymmetric(new_p - p_k - v_k * dt + 0.5 * gravity * dt * dt) * R_k.transpose();
	F.block<3, 3>(v_id, th_id) = - quatType::skewSymmetric(new_v - v_k + gravity * dt) * R_k.transpose();

	F.block<3, 3>(p_id, p_id).setIdentity();

	F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt;
	F.block<3, 3>(v_id, v_id).setIdentity();

	F.block<3, 3>(th_id, bg_id) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw;
	F.block<3, 3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw;
	F.block<3, 3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw;
	F.block<3, 3>(bg_id, bg_id).setIdentity();

	F.block<3, 3>(th_id, ba_id) = dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * Da;
	F.block<3, 3>(p_id, ba_id) = - R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	F.block<3, 3>(v_id, ba_id) = - R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	F.block<3, 3>(ba_id, ba_id).setIdentity();

	if (Dw_id != -1)
	{
		Eigen::MatrixXd H_Dw = computeDwH(state_ptr, gyr_uncorrected);
		F.block(th_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = dR_temp * Jr_temp * dt * R_imu_gyr * H_Dw;
		F.block(p_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = - R_k.transpose() * Xi_4 * R_imu_gyr * H_Dw;
		F.block(v_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = - R_k.transpose() * Xi_3 * R_imu_gyr * H_Dw;
		F.block(Dw_id, Dw_id, state_ptr->calib_imu_dw->getSize(), state_ptr->calib_imu_dw->getSize()).setIdentity();
	}

	if (Da_id != -1)
	{
		Eigen::MatrixXd H_Da = computeDaH(state_ptr, acc_uncorrected);
		F.block(th_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * H_Da;
		F.block(p_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * H_Da;
		F.block(v_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * H_Da;
		F.block(Da_id, Da_id, state_ptr->calib_imu_da->getSize(), state_ptr->calib_imu_da->getSize()).setIdentity();
	}

	if (Tg_id != -1)
	{
		Eigen::MatrixXd H_Tg = computeTgH(state_ptr, acc_k);
		F.block(th_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * H_Tg;
		F.block(p_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw * H_Tg;
		F.block(v_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw * H_Tg;
		F.block(Tg_id, Tg_id, state_ptr->calib_imu_tg->getSize(), state_ptr->calib_imu_tg->getSize()).setIdentity();
	}

	if (th_imu_acc_id != -1)
	{
		F.block<3, 3>(th_id, th_imu_acc_id) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(p_id, th_imu_acc_id) = R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(v_id, th_imu_acc_id) = R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(th_imu_acc_id, th_imu_acc_id).setIdentity();
	}

	if (th_imu_gyr_id != -1)
	{
		F.block<3, 3>(th_id, th_imu_gyr_id) = dR_temp * Jr_temp * dt * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(p_id, th_imu_gyr_id) = -R_k.transpose() * Xi_4 * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(v_id, th_imu_gyr_id) = -R_k.transpose() * Xi_3 * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(th_imu_gyr_id, th_imu_gyr_id).setIdentity();
	}

	G.block<3, 3>(th_id, 0) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw;
	G.block<3, 3>(p_id, 0) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw;
	G.block<3, 3>(v_id, 0) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw;
	G.block<3, 3>(th_id, 3) = dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * Da;
	G.block<3, 3>(p_id, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	G.block<3, 3>(v_id, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity();
	G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity();
}
    noiseManager noises;
    std::vector<imuData> v_imu_data;
    Eigen::Vector3d gravity;
    std::atomic<bool> cache_imu_valid;
    double cache_state_time;
    Eigen::MatrixXd cache_state_est;
    Eigen::MatrixXd cache_state_covariance;
    double cache_t_off;
  public:
    updaterMsckf(updaterOptions &options_, featureInitializerOptions &feature_init_options_) {
	options.sigma_pix_sq = std::pow(options.sigma_pix, 2);

	initializer_feature = std::shared_ptr<featureInitializer>(new featureInitializer(feature_init_options_));

	for (int i = 1; i < 500; i++)
	{
		boost::math::chi_squared chi_squared_dist(i);
		chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
	}
}
    void update(std::shared_ptr<state> state_ptr, std::vector<std::shared_ptr<feature>> &feature_vec) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it0)->feature_id);
		int required_meas = 2;

		if (count_meas < 1)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else if (count_meas < required_meas)
		{
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	size_t max_meas_size = 0;
	for (size_t i = 0; i < feature_vec.size(); i++)
	{
		for (const auto &pair : feature_vec.at(i)->timestamps)
		{
			max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
		}
	}

	size_t max_hx_size = state_ptr->maxCovSize();

	Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
	Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
	Eigen::MatrixXd R_big = Eigen::MatrixXd::Identity(max_meas_size, max_meas_size);
	std::unordered_map<std::shared_ptr<baseType>, size_t> Hx_mapping;
	std::vector<std::shared_ptr<baseType>> Hx_order_big;
	size_t count_jacob = 0;
	size_t count_meas = 0;

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		assert(state_ptr->map_points.find((*it2)->feature_id) != state_ptr->map_points.end());
		assert(state_ptr->map_points.at((*it2)->feature_id)->feature_id == (*it2)->feature_id);

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it2)->feature_id);

		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;
		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;

		feat.position_global = map_point->getPointXYZ(false);
		feat.position_global_fej = map_point->getPointXYZ(true);

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		Eigen::MatrixXd H_xf = H_x;

		H_xf.conservativeResize(H_x.rows(), H_x.cols() + H_f.cols());
		H_xf.block(0, H_x.cols(), H_x.rows(), H_f.cols()) = H_f;

		std::vector<std::shared_ptr<baseType>> Hxf_order = Hx_order;
		Hxf_order.push_back(map_point);

		Eigen::MatrixXd P_marg = stateHelper::getMarginalCovariance(state_ptr, Hxf_order);
		Eigen::MatrixXd S = H_xf * P_marg * H_xf.transpose();
		double sigma_pix_sq = options_slam.sigma_pix_sq;
		S.diagonal() += sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
		double chi2 = res.dot(S.llt().solve(res));

		double chi2_check;
		if (res.rows() < 500)
		{
			chi2_check = chi_squared_table[res.rows()];
		}
		else
		{
			boost::math::chi_squared chi_squared_dist(res.rows());
			chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
			std::cout << "chi2_check over the residual limit - " << (int)res.rows() << std::endl;
		}

		double chi2_multipler = options_slam.chi2_multipler;

		if (chi2 > chi2_multipler * chi2_check)
		{
			map_point->update_fail_count++;

			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
			continue;
		}

		size_t count_hx = 0;
		for (const auto &var : Hxf_order)
		{
			if (Hx_mapping.find(var) == Hx_mapping.end())
			{
				Hx_mapping.insert({var, count_jacob});
				Hx_order_big.push_back(var);
				count_jacob += var->getSize();
			}

			Hx_big.block(count_meas, Hx_mapping[var], H_xf.rows(), var->getSize()) = H_xf.block(0, count_hx, H_xf.rows(), var->getSize());
			count_hx += var->getSize();
		}

		R_big.block(count_meas, count_meas, res.rows(), res.rows()) *= sigma_pix_sq;

		res_big.block(count_meas, 0, res.rows(), 1) = res;
		count_meas += res.rows();
		it2++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	for (size_t f = 0; f < feature_vec.size(); f++)
	{
		if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).back() > clone_times.at(clone_times.size() - 1) + 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).back() > clone_times.at(clone_times.size() - 1) + 1e-5))
		{
			if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).front() < clone_times.at(clone_times.size() - 3) - 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).front() < clone_times.at(clone_times.size() - 3) - 1e-5))
				feature_vec[f]->to_delete = true;
		}
		else
		{
			feature_vec[f]->to_delete = true;
		}
	}

	if (count_meas < 1)
	{
		return;
	}
	assert(count_meas <= max_meas_size);
	assert(count_jacob <= max_hx_size);
	res_big.conservativeResize(count_meas, 1);
	Hx_big.conservativeResize(count_meas, count_jacob);
	R_big.conservativeResize(count_meas, count_meas);

	stateHelper::ekfUpdate(state_ptr, Hx_order_big, Hx_big, res_big, R_big);
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	std::cout << std::fixed << "[updaterSlam]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds creating linear system." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds to update ("
		<< (int)feature_vec.size() << " feats of " << (int)Hx_big.rows() << " size)." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	*/
	// Time test
}
  protected:
    updaterOptions options;
    std::shared_ptr<featureInitializer> initializer_feature;
    std::map<int, double> chi_squared_table;
  public:
    updaterSlam(updaterOptions &options_slam_, featureInitializerOptions &feat_init_options_, std::shared_ptr<featureDatabase> database_) {
	options_slam.sigma_pix_sq = std::pow(options_slam.sigma_pix, 2);

	initializer_feature = std::shared_ptr<featureInitializer>(new featureInitializer(feat_init_options_));

	for (int i = 1; i < 500; i++)
	{
		boost::math::chi_squared chi_squared_dist(i);
		chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
	}
}
    void update(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, std::vector<std::shared_ptr<feature>> &feature_vec) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it0)->feature_id);
		int required_meas = 2;

		if (count_meas < 1)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else if (count_meas < required_meas)
		{
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	size_t max_meas_size = 0;
	for (size_t i = 0; i < feature_vec.size(); i++)
	{
		for (const auto &pair : feature_vec.at(i)->timestamps)
		{
			max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
		}
	}

	size_t max_hx_size = state_ptr->maxCovSize();

	Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
	Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
	Eigen::MatrixXd R_big = Eigen::MatrixXd::Identity(max_meas_size, max_meas_size);
	std::unordered_map<std::shared_ptr<baseType>, size_t> Hx_mapping;
	std::vector<std::shared_ptr<baseType>> Hx_order_big;
	size_t count_jacob = 0;
	size_t count_meas = 0;

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		assert(state_ptr->map_points.find((*it2)->feature_id) != state_ptr->map_points.end());
		assert(state_ptr->map_points.at((*it2)->feature_id)->feature_id == (*it2)->feature_id);

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it2)->feature_id);

		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;
		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;

		feat.position_global = map_point->getPointXYZ(false);
		feat.position_global_fej = map_point->getPointXYZ(true);

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		Eigen::MatrixXd H_xf = H_x;

		H_xf.conservativeResize(H_x.rows(), H_x.cols() + H_f.cols());
		H_xf.block(0, H_x.cols(), H_x.rows(), H_f.cols()) = H_f;

		std::vector<std::shared_ptr<baseType>> Hxf_order = Hx_order;
		Hxf_order.push_back(map_point);

		Eigen::MatrixXd P_marg = stateHelper::getMarginalCovariance(state_ptr, Hxf_order);
		Eigen::MatrixXd S = H_xf * P_marg * H_xf.transpose();
		double sigma_pix_sq = options_slam.sigma_pix_sq;
		S.diagonal() += sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
		double chi2 = res.dot(S.llt().solve(res));

		double chi2_check;
		if (res.rows() < 500)
		{
			chi2_check = chi_squared_table[res.rows()];
		}
		else
		{
			boost::math::chi_squared chi_squared_dist(res.rows());
			chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
			std::cout << "chi2_check over the residual limit - " << (int)res.rows() << std::endl;
		}

		double chi2_multipler = options_slam.chi2_multipler;

		if (chi2 > chi2_multipler * chi2_check)
		{
			map_point->update_fail_count++;

			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
			continue;
		}

		size_t count_hx = 0;
		for (const auto &var : Hxf_order)
		{
			if (Hx_mapping.find(var) == Hx_mapping.end())
			{
				Hx_mapping.insert({var, count_jacob});
				Hx_order_big.push_back(var);
				count_jacob += var->getSize();
			}

			Hx_big.block(count_meas, Hx_mapping[var], H_xf.rows(), var->getSize()) = H_xf.block(0, count_hx, H_xf.rows(), var->getSize());
			count_hx += var->getSize();
		}

		R_big.block(count_meas, count_meas, res.rows(), res.rows()) *= sigma_pix_sq;

		res_big.block(count_meas, 0, res.rows(), 1) = res;
		count_meas += res.rows();
		it2++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	for (size_t f = 0; f < feature_vec.size(); f++)
	{
		if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).back() > clone_times.at(clone_times.size() - 1) + 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).back() > clone_times.at(clone_times.size() - 1) + 1e-5))
		{
			if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).front() < clone_times.at(clone_times.size() - 3) - 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).front() < clone_times.at(clone_times.size() - 3) - 1e-5))
				feature_vec[f]->to_delete = true;
		}
		else
		{
			feature_vec[f]->to_delete = true;
		}
	}

	if (count_meas < 1)
	{
		return;
	}
	assert(count_meas <= max_meas_size);
	assert(count_jacob <= max_hx_size);
	res_big.conservativeResize(count_meas, 1);
	Hx_big.conservativeResize(count_meas, count_jacob);
	R_big.conservativeResize(count_meas, count_meas);

	stateHelper::ekfUpdate(state_ptr, Hx_order_big, Hx_big, res_big, R_big);
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	std::cout << std::fixed << "[updaterSlam]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds creating linear system." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds to update ("
		<< (int)feature_vec.size() << " feats of " << (int)Hx_big.rows() << " size)." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	*/
	// Time test
}
    void delayedInit(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, std::vector<std::shared_ptr<feature>> &feature_vec, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		if (count_meas < 2)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	std::unordered_map<size_t, std::unordered_map<double, featureInitializer::clonePose>> clones_cam;
	for (const auto &clone_calib : state_ptr->calib_cam_imu)
	{
		std::unordered_map<double, featureInitializer::clonePose> clones_cami;
		for (const auto &clone_imu : state_ptr->clones_imu)
		{
			Eigen::Matrix<double, 3, 3> R_GtoCi = clone_calib.second->getRot() * clone_imu.second->getRot();
			Eigen::Matrix<double, 3, 1> p_CioinG = clone_imu.second->getPos() - R_GtoCi.transpose() * clone_calib.second->getPos();

			clones_cami.insert({clone_imu.first, featureInitializer::clonePose(R_GtoCi, p_CioinG)});
		}

		clones_cam.insert({clone_calib.first, clones_cami});
	}

	auto it1 = feature_vec.begin();
	while (it1 != feature_vec.end())
	{
		bool success_tri = true;
		success_tri = initializer_feature->singleTriangulation(*it1, clones_cam);

		bool success_refine = true;
		if (initializer_feature->config().refine_features)
		{
			success_refine = initializer_feature->singleGaussnewton(*it1, clones_cam);
		}

		if (!success_tri || !success_refine)
		{
			(*it1)->to_delete = true;
			it1 = feature_vec.erase(it1);
			continue;
		}
		it1++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;

		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;
		assert(feat.color[0].size() == feat.uvs[0].size());
		assert(feat.color[1].size() == feat.uvs[1].size());
		assert(feat.color[(*it2)->anchor_cam_id].back() >= 0);

		feat.anchor_cam_id = (*it2)->anchor_cam_id;
		feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;

		feat.position_global = (*it2)->position_global;
		feat.position_global_fej = (*it2)->position_global;

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		auto map_point_ptr = std::make_shared<mapPoint>();
		map_point_ptr->feature_id = feat.feature_id;
		map_point_ptr->unique_camera_id = (*it2)->anchor_cam_id;

		map_point_ptr->color = feat.color[feat.anchor_cam_id].back();
		map_point_ptr->host_frame = feat.frames[(*it2)->anchor_cam_id].back();
		map_point_ptr->anchor_cam_id = feat.anchor_cam_id;
		map_point_ptr->anchor_clone_timestamp = feat.anchor_clone_timestamp;

		map_point_ptr->setPointXYZ(feat.position_global, false);
		map_point_ptr->setPointXYZ(feat.position_global_fej, true);

		double sigma_pix_sq = options_slam.sigma_pix_sq;
		Eigen::MatrixXd R = sigma_pix_sq * Eigen::MatrixXd::Identity(res.rows(), res.rows());

		double chi2_multipler = options_slam.chi2_multipler;

		if (stateHelper::initialize(state_ptr, map_point_ptr, Hx_order, H_x, H_f, R, res, chi2_multipler))
		{
			bool add = mapManagement::addPointToVoxel(voxel_map, map_point_ptr, state_ptr->options.voxel_size, state_ptr->options.max_num_points_in_voxel,
				state_ptr->options.min_distance_points, voxels_history);

			if (add)
			{
				state_ptr->map_points.insert({(*it2)->feature_id, map_point_ptr});
				(*it2)->to_delete = true;
				it2++;
			}
			else
			{
				(*it2)->to_delete = true;
				it2 = feature_vec.erase(it2);
			}
		}
		else
		{
			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
		}
	}
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	if (!feature_vec.empty())
	{
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds to triangulate." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds initialize (" << (int)feature_vec.size() << " features)." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	}
	*/
	// Time test
}
  protected:
    updaterOptions options_slam;
    std::shared_ptr<featureInitializer> initializer_feature;
    std::shared_ptr<featureDatabase> database;
    std::map<int, double> chi_squared_table;
};

class staticInitializer {
  public:
    inertialInitializer(inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_) {
	imu_data = std::make_shared<std::vector<imuData>>();

	static_initializer = std::make_shared<staticInitializer>(initializer_options, db, imu_data);
	dynamic_initializer = std::make_shared<dynamicInitializer>(initializer_options, db, imu_data);
}
    void feedImu(const imuData &imu_data_, double oldest_time = -1) {
	imu_data->emplace_back(imu_data_);

	if (oldest_time != -1)
	{
		auto it0 = imu_data->begin();

		while (it0 != imu_data->end())
		{
			if (it0->timestamp < oldest_time)
				it0 = imu_data->erase(it0);
			else
				it0++;
		}
	}
}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> imu_state_, bool wait_for_jerk = true) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  protected:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
    std::shared_ptr<staticInitializer> static_initializer;
    std::shared_ptr<dynamicInitializer> dynamic_initializer;
  public:
    explicit staticInitializer(inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_,
    std::shared_ptr<std::vector<imuData>> imu_data_) {

}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> imu_state_, bool wait_for_jerk = true) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  private:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
  public:
    explicit dynamicInitializer(const inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_,
    std::shared_ptr<std::vector<imuData>> imu_data_) {

}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> &imu_state_, std::map<double, std::shared_ptr<poseJpl>> &clones_imu_,
    std::unordered_map<size_t, std::shared_ptr<mapPoint>> &map_points_) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  private:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
};

class baseType {
  public:
    struct updaterHelperFeature
	{
		size_t feature_id;

		std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs;

		std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs_norm;

		std::unordered_map<size_t, std::vector<double>> timestamps;

		std::unordered_map<size_t, std::vector<std::shared_ptr<frame>>> frames;

		std::unordered_map<size_t, std::vector<float>> color;

		int anchor_cam_id = -1;

		double anchor_clone_timestamp = -1;

		Eigen::Vector3d position_anchor;

		Eigen::Vector3d position_anchor_fej;

		Eigen::Vector3d position_global;

		Eigen::Vector3d position_global_fej;
	};
    static void getFeatureJacobianRepresentation(std::shared_ptr<state> state_ptr, updaterHelperFeature &feature, Eigen::MatrixXd &H_f,
		std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<baseType>> &x_order) {
	H_f.resize(3, 3);
	H_f.setIdentity();
	return;
}
    static void getFeatureJacobianFull(std::shared_ptr<state> state_ptr, updaterHelperFeature &feature, Eigen::MatrixXd &H_f,
		Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector<std::shared_ptr<baseType>> &x_order) {
	int total_meas = 0;
	for (auto const &pair : feature.timestamps)
	{
		total_meas += (int)pair.second.size();
	}

	int total_hx = 0;
	std::unordered_map<std::shared_ptr<baseType>, size_t> map_hx;
	for (auto const &pair : feature.timestamps)
	{
		std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(pair.first);
		std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(pair.first);

		if (state_ptr->options.do_calib_camera_pose)
		{
			map_hx.insert({calibration, total_hx});
			x_order.push_back(calibration);
			total_hx += calibration->getSize();
		}

		if (state_ptr->options.do_calib_camera_intrinsics)
		{
			map_hx.insert({distortion, total_hx});
			x_order.push_back(distortion);
			total_hx += distortion->getSize();
		}

		for (size_t m = 0; m < feature.timestamps[pair.first].size(); m++)
		{
			std::shared_ptr<poseJpl> clone_Ci = state_ptr->clones_imu.at(feature.timestamps[pair.first].at(m));

			if (map_hx.find(clone_Ci) == map_hx.end())
			{
				map_hx.insert({clone_Ci, total_hx});
				x_order.push_back(clone_Ci);
				total_hx += clone_Ci->getSize();
			}
		}
	}

	Eigen::Vector3d p_FinG = feature.position_global;
	Eigen::Vector3d p_FinG_fej = feature.position_global_fej;

	int c = 0;
	int jacobsize = 3;

	res = Eigen::VectorXd::Zero(2 * total_meas);
	H_f = Eigen::MatrixXd::Zero(2 * total_meas, jacobsize);
	H_x = Eigen::MatrixXd::Zero(2 * total_meas, total_hx);

	Eigen::MatrixXd dpfg_dlambda;
	std::vector<Eigen::MatrixXd> dpfg_dx;
	std::vector<std::shared_ptr<baseType>> dpfg_dx_order;
	updaterHelper::getFeatureJacobianRepresentation(state_ptr, feature, dpfg_dlambda, dpfg_dx, dpfg_dx_order);

	for (auto const &pair : feature.timestamps)
	{
		std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(pair.first);
		std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(pair.first);
		Eigen::Matrix3d R_ItoC = calibration->getRot();
		Eigen::Vector3d p_IinC = calibration->getPos();

		for (size_t m = 0; m < feature.timestamps[pair.first].size(); m++)
		{
			std::shared_ptr<poseJpl> clone_Ii = state_ptr->clones_imu.at(feature.timestamps[pair.first].at(m));
			Eigen::Matrix3d R_GtoIi = clone_Ii->getRot();
			Eigen::Vector3d p_IiinG = clone_Ii->getPos();

			Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);

			Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
			Eigen::Vector2d uv_norm;
			uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

			Eigen::Vector2d uv_dist;
			uv_dist = state_ptr->cam_intrinsics_cameras.at(pair.first)->distortD(uv_norm);

			Eigen::Vector2d uv_m;
			uv_m << (double)feature.uvs[pair.first].at(m)(0), (double)feature.uvs[pair.first].at(m)(1);
			res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;

			if (state_ptr->options.do_fej)
			{
				R_GtoIi = clone_Ii->getRotFej();
				p_IiinG = clone_Ii->getPosFej();

				p_FinIi = R_GtoIi * (p_FinG_fej - p_IiinG);
				p_FinCi = R_ItoC * p_FinIi + p_IinC;
			}

			Eigen::MatrixXd dz_dzn, dz_dzeta;
			state_ptr->cam_intrinsics_cameras.at(pair.first)->computeDistortJacobian(uv_norm, dz_dzn, dz_dzeta);

			Eigen::MatrixXd dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
			dzn_dpfc << 1 / p_FinCi(2), 0, -p_FinCi(0) / (p_FinCi(2) * p_FinCi(2)), 0, 1 / p_FinCi(2), -p_FinCi(1) / (p_FinCi(2) * p_FinCi(2));

			Eigen::MatrixXd dpfc_dpfg = R_ItoC * R_GtoIi;

			Eigen::MatrixXd dpfc_dclone = Eigen::MatrixXd::Zero(3, 6);
			dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * quatType::skewSymmetric(p_FinIi);
			dpfc_dclone.block(0, 3, 3, 3) = - dpfc_dpfg;

			Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc;
			Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg;

			H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;
			H_x.block(2 * c, map_hx[clone_Ii], 2, clone_Ii->getSize()).noalias() = dz_dpfc * dpfc_dclone;

			for (size_t i = 0; i < dpfg_dx_order.size(); i++)
			{
				H_x.block(2 * c, map_hx[dpfg_dx_order.at(i)], 2, dpfg_dx_order.at(i)->getSize()).noalias() += dz_dpfg * dpfg_dx.at(i);
			}

			if (state_ptr->options.do_calib_camera_pose)
			{
				Eigen::MatrixXd dpfc_dcalib = Eigen::MatrixXd::Zero(3, 6);
				dpfc_dcalib.block(0, 0, 3, 3) = quatType::skewSymmetric(p_FinCi - p_IinC);
				dpfc_dcalib.block(0, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

				H_x.block(2 * c, map_hx[calibration], 2, calibration->getSize()).noalias() += dz_dpfc * dpfc_dcalib;
			}

			if (state_ptr->options.do_calib_camera_intrinsics)
			{
				H_x.block(2 * c, map_hx[distortion], 2, distortion->getSize()) = dz_dzeta;
			}

			if (state_ptr->options.use_huber)
			{
				double loss = res.block(2 * c, 0, 2, 1).norm();
				double hw = loss < setting_huber_th ? 1 : setting_huber_th / loss;

				if (hw < 1) hw = sqrt(hw);

				res.block(2 * c, 0, 2, 1) *= hw;
				H_x.block(2 * c, 0, 2, H_x.cols()) *= hw;
				H_f.block(2 * c, 0, 2, H_f.cols()) *= hw;
			}

			c++;
		}
	}
}
    static void nullspaceProjectInplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
	Eigen::JacobiRotation<double> Ho_GR_temp;

	for (int n = 0; n < H_f.cols(); ++n)
	{
		for (int m = (int)H_f.rows() - 1; m > n; m--)
		{
			Ho_GR_temp.makeGivens(H_f(m - 1, n), H_f(m, n));
			(H_f.block(m - 1, n, 2, H_f.cols() - n)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(H_x.block(m - 1, 0, 2, H_x.cols())).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
		}
	}

	H_x = H_x.block(H_f.cols(), 0, H_x.rows() - H_f.cols(), H_x.cols()).eval();
	res = res.block(H_f.cols(), 0, res.rows() - H_f.cols(), res.cols()).eval();

	assert(H_x.rows() == res.rows());
}
    static void measurementCompressInplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
	if (H_x.rows() <= H_x.cols())
		return;

	Eigen::JacobiRotation<double> Ho_GR_temp;
	for (int n = 0; n < H_x.cols(); n++)
	{
		for (int m = (int)H_x.rows() - 1; m > n; m--)
		{
			Ho_GR_temp.makeGivens(H_x(m - 1, n), H_x(m, n));

			(H_x.block(m - 1, n, 2, H_x.cols() - n)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
		}
	}

	int r = std::min(H_x.rows(), H_x.cols());

	assert(r <= H_x.rows());
	H_x.conservativeResize(r, H_x.cols());
	res.conservativeResize(r, res.cols());
}
};

class state {
  public:
    size_t feature_id;
    bool to_delete;
    std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs;
    std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs_norm;
    std::unordered_map<size_t, std::vector<double>> timestamps;
    std::unordered_map<size_t, std::vector<std::shared_ptr<frame>>> frames;
    std::unordered_map<size_t, std::vector<float>> color;
    int start_frame_id;
    int start_cam_id;
    int num_tracking;
    int anchor_cam_id = -1;
    double anchor_clone_timestamp;
    Eigen::Vector3d position_anchor;
    Eigen::Vector3d position_global;
    void cleanOldMeasurements(const std::vector<double> &valid_times) {
	for (auto const &pair : timestamps)
	{
		assert(timestamps[pair.first].size() == uvs[pair.first].size());
		assert(timestamps[pair.first].size() == uvs_norm[pair.first].size());

		auto it1 = timestamps[pair.first].begin();
		auto it2 = uvs[pair.first].begin();
		auto it3 = uvs_norm[pair.first].begin();
		auto it4 = frames[pair.first].begin();
		auto it5 = color[pair.first].begin();

		while (it1 != timestamps[pair.first].end())
		{
			if (std::find(valid_times.begin(), valid_times.end(), *it1) == valid_times.end())
			{
				it1 = timestamps[pair.first].erase(it1);
				it2 = uvs[pair.first].erase(it2);
				it3 = uvs_norm[pair.first].erase(it3);

				assert((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end());

				if ((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end())
				{
					(*it4)->v_feat_ptr.at(pair.first).erase(feature_id);
				}

				it4 = frames[pair.first].erase(it4);
				it5 = color[pair.first].erase(it5);
			}
			else
			{
				it1++;
				it2++;
				it3++;
				it4++;
				it5++;
			}
		}
	}
}
    void cleanInvalidMeasurements(const std::vector<double> &invalid_times) {
	for (auto const &pair : timestamps)
	{
		assert(timestamps[pair.first].size() == uvs[pair.first].size());
		assert(timestamps[pair.first].size() == uvs_norm[pair.first].size());

		auto it1 = timestamps[pair.first].begin();
		auto it2 = uvs[pair.first].begin();
		auto it3 = uvs_norm[pair.first].begin();
		auto it4 = frames[pair.first].begin();
		auto it5 = color[pair.first].begin();

		while (it1 != timestamps[pair.first].end())
		{
			if (std::find(invalid_times.begin(), invalid_times.end(), *it1) != invalid_times.end())
			{
				it1 = timestamps[pair.first].erase(it1);
				it2 = uvs[pair.first].erase(it2);
				it3 = uvs_norm[pair.first].erase(it3);

				assert((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end());

				if ((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end())
				{
					(*it4)->v_feat_ptr.at(pair.first).erase(feature_id);
				}

				it4 = frames[pair.first].erase(it4);
				it5 = color[pair.first].erase(it5);
				// 2024.10.31 yzk
			}
			else
			{
				it1++;
				it2++;
				it3++;
				it4++;
				it5++;
			}
		}
	}
}
    void cleanOlderMeasurements(double timestamp) {
	for (auto const &pair : timestamps)
	{
		assert(timestamps[pair.first].size() == uvs[pair.first].size());
		assert(timestamps[pair.first].size() == uvs_norm[pair.first].size());

		auto it1 = timestamps[pair.first].begin();
		auto it2 = uvs[pair.first].begin();
		auto it3 = uvs_norm[pair.first].begin();
		auto it4 = frames[pair.first].begin();
		auto it5 = color[pair.first].begin();

		while (it1 != timestamps[pair.first].end())
		{
			if (*it1 <= timestamp)
			{
				it1 = timestamps[pair.first].erase(it1);
				it2 = uvs[pair.first].erase(it2);
				it3 = uvs_norm[pair.first].erase(it3);

				assert((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end());

				if ((*it4)->v_feat_ptr.at(pair.first).find(feature_id) != (*it4)->v_feat_ptr.at(pair.first).end())
				{
					(*it4)->v_feat_ptr.at(pair.first).erase(feature_id);
				}

				it4 = frames[pair.first].erase(it4);
				it5 = color[pair.first].erase(it5);
			}
			else
			{
				it1++;
				it2++;
				it3++;
				it4++;
				it5++;
			}
		}
	}
}
  public:
    featureDatabase() {
	last_track_num = 0;
}
    std::shared_ptr<feature> getFeature(size_t id, bool remove = false) {
	if (features_id_lookup.find(id) != features_id_lookup.end())
	{
		std::shared_ptr<feature> temp = features_id_lookup.at(id);
		if (remove)
		{
			for (int i = 0; i < 2; i++)
			{
				for (auto it_fh = temp->frames[i].begin(); it_fh != temp->frames[i].end(); it_fh++)
				{
					if ((*it_fh) == nullptr || (*it_fh)->is_invalid) continue;

					if ((*it_fh)->v_feat_ptr.at(i).find(temp->feature_id) != (*it_fh)->v_feat_ptr.at(i).end())
						(*it_fh)->v_feat_ptr.at(i).erase(temp->feature_id);
				}
			}

			features_id_lookup.erase(id);
		}
		return temp;
	}
	else
	{
		return nullptr;
	}
}
    void updateFeature(std::shared_ptr<frame> fh, size_t id, double timestamp, size_t cam_id, float u, float v, float u_n, float v_n) {
	if (features_id_lookup.find(id) != features_id_lookup.end())
	{
		std::shared_ptr<feature> feat = features_id_lookup.at(id);

		if (u > 1.1 && u < wG[0] - 3 && v > 1.1 && v < hG[0] - 3)
		{
			feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v));
			feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n));
			feat->timestamps[cam_id].push_back(timestamp);
			feat->frames[cam_id].push_back(fh);

			float color_temp = cam_id == 0 ? feat->frames[cam_id].back()->dI_left[(int)u + (int)v * wG[0]][0]
				: feat->frames[cam_id].back()->dI_right[(int)u + (int)v * wG[0]][0];

			assert(color_temp >= 0);

			feat->color[cam_id].push_back(color_temp);
			feat->num_tracking = fh->frame_id - feat->start_frame_id;
			fh->v_feat_ptr[cam_id][feat->feature_id] = feat;

			last_track_num++;
		}

		return;
	}

	if (u > 1.1 && u < wG[0] - 3 && v > 1.1 && v < hG[0] - 3)
	{
		std::shared_ptr<feature> feat = std::make_shared<feature>();
		feat->feature_id = id;
		feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v));
		feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n));
		feat->timestamps[cam_id].push_back(timestamp);
		feat->frames[cam_id].push_back(fh);

		float color_temp = cam_id == 0 ? feat->frames[cam_id].back()->dI_left[(int)u + (int)v * wG[0]][0]
			: feat->frames[cam_id].back()->dI_right[(int)u + (int)v * wG[0]][0];

		assert(color_temp >= 0);

		feat->color[cam_id].push_back(color_temp);
		feat->start_frame_id = fh->frame_id;
		feat->start_cam_id = cam_id;
		feat->num_tracking = 0;

		features_id_lookup[id] = feat;

		fh->v_feat_ptr[cam_id][feat->feature_id] = feat;
	}
}
    std::vector<std::shared_ptr<feature>> getOldFeatures(double timestamp, bool remove = false, bool skip_deleted = false) {
	std::vector<std::shared_ptr<feature>> feats_old;

	for (auto it = features_id_lookup.begin(); it != features_id_lookup.end();)
	{
		// Skip if already deleted
		if (skip_deleted && (*it).second->to_delete)
		{
			it++;
			continue;
		}

		bool has_newer_measurement = false;
		for (auto const &pair : (*it).second->timestamps)
		{
			has_newer_measurement = (!pair.second.empty() && pair.second.at(pair.second.size() - 1) >= timestamp);
			if (has_newer_measurement)
			{
				break;
			}
		}

		if (!has_newer_measurement)
		{
			feats_old.push_back((*it).second);

			if (remove)
			{
				for (int i = 0; i < 2; i++)
				{
					for (auto it_fh = (*it).second->frames[i].begin(); it_fh != (*it).second->frames[i].end(); it_fh++)
					{
						if ((*it_fh) == nullptr || (*it_fh)->is_invalid) continue;

						if ((*it_fh)->v_feat_ptr.at(i).find((*it).second->feature_id) != (*it_fh)->v_feat_ptr.at(i).end())
							(*it_fh)->v_feat_ptr.at(i).erase((*it).second->feature_id);
					}
				}

				features_id_lookup.erase(it++);
			}
			else
				it++;
		}
		else
		{
			it++;
		}
	}

	return feats_old;
}
    std::vector<std::shared_ptr<feature>> getFeatures(double timestamp, bool remove = false, bool skip_deleted = false) {
	std::vector<std::shared_ptr<feature>> feats_has_timestamp;

	for (auto it = features_id_lookup.begin(); it != features_id_lookup.end();)
	{
		if (skip_deleted && (*it).second->to_delete)
		{
			it++;
			continue;
		}

		bool has_timestamp = false;
		for (auto const &pair : (*it).second->timestamps)
		{
			has_timestamp = (std::find(pair.second.begin(), pair.second.end(), timestamp) != pair.second.end());
			if (has_timestamp)
			{
				break;
			}
		}

		if (has_timestamp)
		{
			feats_has_timestamp.push_back((*it).second);

			if (remove)
			{
				for (int i = 0; i < 2; i++)
				{
					for (auto it_fh = (*it).second->frames[i].begin(); it_fh != (*it).second->frames[i].end(); it_fh++)
					{
						if ((*it_fh) == nullptr || (*it_fh)->is_invalid) continue;

						if ((*it_fh)->v_feat_ptr.at(i).find((*it).second->feature_id) != (*it_fh)->v_feat_ptr.at(i).end())
							(*it_fh)->v_feat_ptr.at(i).erase((*it).second->feature_id);
					}
				}

				features_id_lookup.erase(it++);
			}
			else
				it++;
		}
		else
		{
			it++;
		}
	}

	return feats_has_timestamp;
}
    void cleanUp() {
	for (auto it = features_id_lookup.begin(); it != features_id_lookup.end();)
	{
		if ((*it).second->to_delete)
		{
			for (int i = 0; i < 2; i++)
			{
				for (auto it_fh = (*it).second->frames[i].begin(); it_fh != (*it).second->frames[i].end(); it_fh++)
				{
					if ((*it_fh) == nullptr || (*it_fh)->is_invalid) continue;

					if ((*it_fh)->v_feat_ptr.at(i).find((*it).second->feature_id) != (*it_fh)->v_feat_ptr.at(i).end())
						(*it_fh)->v_feat_ptr.at(i).erase((*it).second->feature_id);
				}
			}

			features_id_lookup.erase(it++);
		}
		else
		{
			it++;
		}
	}
}
    void cleanUpOldmeasurements(double timestamp) {
	for (auto it = features_id_lookup.begin(); it != features_id_lookup.end();)
	{
		(*it).second->cleanOlderMeasurements(timestamp);

		int count_meas = 0;

		for (const auto &pair : (*it).second->timestamps)
		{
			count_meas += (int)(pair.second.size());
		}

		if (count_meas < 1)
		{
			for (int i = 0; i < 2; i++)
			{
				for (auto it_fh = (*it).second->frames[i].begin(); it_fh != (*it).second->frames[i].end(); it_fh++)
				{
					if ((*it_fh) == nullptr || (*it_fh)->is_invalid) continue;

					if ((*it_fh)->v_feat_ptr.at(i).find((*it).second->feature_id) != (*it_fh)->v_feat_ptr.at(i).end())
						(*it_fh)->v_feat_ptr.at(i).erase((*it).second->feature_id);
				}
			}

			features_id_lookup.erase(it++);
		}
		else
		{
			it++;
		}
	}
}
    size_t getSize() {
	return features_id_lookup.size();
}
    std::unordered_map<size_t, std::shared_ptr<feature>> getInternalData() {
	return features_id_lookup;
}
    bool featureCheckParallax(int frame_count) {
    double parallax_sum = 0;
    int parallax_num = 0;

    if (frame_count < 2 || last_track_num < 20)
    {
    	last_track_num = 0;
        return true;
    }

    last_track_num = 0;

    for (auto it = features_id_lookup.begin(); it != features_id_lookup.end(); it++)
	{
		if ((*it).second->start_frame_id <= frame_count - 2)
		{
			if ((*it).second->start_cam_id == 0 && (*it).second->start_frame_id + int((*it).second->timestamps.at(0).size()) - 1 >= frame_count)
			{
				parallax_sum += compensatedParallax2((*it).first, 0, frame_count);
            	parallax_num++;
			}
			else if ((*it).second->start_cam_id == 1 && (*it).second->start_frame_id + int((*it).second->timestamps.at(1).size()) - 1 >= frame_count)
			{
				parallax_sum += compensatedParallax2((*it).first, 1, frame_count);
            	parallax_num++;
			}
		}
	}

    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        return parallax_sum / parallax_num >= setting_min_parallax;
    }
}
    double compensatedParallax2(int feat_id, int cam_id, int frame_count) {
	std::shared_ptr<feature> feat_ptr = features_id_lookup.at(feat_id);

	Eigen::Vector2f uvs_norm_i = feat_ptr->uvs_norm.at(cam_id).at(frame_count - 2 - feat_ptr->start_frame_id);
	Eigen::Vector2f uvs_norm_j = feat_ptr->uvs_norm.at(cam_id).at(frame_count - 1 - feat_ptr->start_frame_id);

    float ans = 0;
    float du = uvs_norm_i(0) - uvs_norm_j(0);
    float dv = uvs_norm_i(1) - uvs_norm_j(1);

    ans = std::max(ans, sqrt(du * du + dv * dv));

    return ans;
}
  protected:
    std::unordered_map<size_t, std::shared_ptr<feature>> features_id_lookup;
    int last_track_num;
  public:
    struct clonePose
  {
    Eigen::Matrix<double, 3, 3> rotation;

    Eigen::Matrix<double, 3, 1> position;

    clonePose(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &p)
    {
      rotation = R;
      position = p;
    }

    clonePose(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 3, 1> &p)
    {
      rotation = quatType::quatToRot(q);
      position = p;
    }

    clonePose()
    {
      rotation = Eigen::Matrix<double, 3, 3>::Identity();
      position = Eigen::Matrix<double, 3, 1>::Zero();
    }

    const Eigen::Matrix<double, 3, 3> &Rot() { return rotation; }

    const Eigen::Matrix<double, 3, 1> &pos() { return position; }
  };
    featureInitializer(featureInitializerOptions &options_) {

}
    bool singleTriangulation(std::shared_ptr<feature> feat, std::unordered_map<size_t, std::unordered_map<double, clonePose>> &clones_cam) {
	int total_meas = 0;
	size_t anchor_most_meas = 0;
	size_t most_meas = 0;

	for (auto const &pair : feat->timestamps)
	{
		total_meas += (int)pair.second.size();

		if (pair.second.size() > most_meas)
    	{
			anchor_most_meas = pair.first;
			most_meas = pair.second.size();
		}
	}

	feat->anchor_cam_id = anchor_most_meas;
	feat->anchor_clone_timestamp = feat->timestamps.at(feat->anchor_cam_id).back();

	Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
	Eigen::Vector3d b = Eigen::Vector3d::Zero();

	clonePose anchor_clone = clones_cam.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp);
	const Eigen::Matrix<double, 3, 3> &R_GtoA = anchor_clone.Rot();
	const Eigen::Matrix<double, 3, 1> &p_AinG = anchor_clone.pos();

	for (auto const &pair : feat->timestamps)
	{
		for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++)
		{
			const Eigen::Matrix<double, 3, 3> &R_GtoCi = clones_cam.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
			const Eigen::Matrix<double, 3, 1> &p_CiinG = clones_cam.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();

			Eigen::Matrix<double, 3, 3> R_AtoCi;
			R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
			Eigen::Matrix<double, 3, 1> p_CiinA;
			p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);

			Eigen::Matrix<double, 3, 1> b_i;
			b_i << feat->uvs_norm.at(pair.first).at(m)(0), feat->uvs_norm.at(pair.first).at(m)(1), 1;
			b_i = R_AtoCi.transpose() * b_i;
			b_i = b_i / b_i.norm();
			Eigen::Matrix3d B_perp = quatType::skewSymmetric(b_i);

			Eigen::Matrix3d Ai = B_perp.transpose() * B_perp;
			A += Ai;
			b += Ai * p_CiinA;
		}
	}

	Eigen::MatrixXd p_f = A.colPivHouseholderQr().solve(b);

	Eigen::JacobiSVD<Eigen::Matrix3d> svd(A);
	Eigen::MatrixXd singular_values;
	singular_values.resize(svd.singularValues().rows(), 1);
	singular_values = svd.singularValues();
	double cond_A = singular_values(0, 0) / singular_values(singular_values.rows() - 1, 0);

	if (std::abs(cond_A) > options.max_cond_number || p_f(2, 0) < options.min_dist || p_f(2, 0) > options.max_dist || std::isnan(p_f.norm()))
	{
		return false;
	}

	feat->position_anchor = p_f;
	feat->position_global = R_GtoA.transpose() * feat->position_anchor + p_AinG;

	return true;
}
    bool singleGaussnewton(std::shared_ptr<feature> feat, std::unordered_map<size_t, std::unordered_map<double, clonePose>> &clones_cam) {
	double rho = 1 / feat->position_anchor(2);
	double alpha = feat->position_anchor(0) / feat->position_anchor(2);
	double beta = feat->position_anchor(1) / feat->position_anchor(2);

	double lam = options.init_lamda;
	double eps = 10000;
	int runs = 0;

	bool recompute = true;
	Eigen::Matrix<double, 3, 3> Hess = Eigen::Matrix<double, 3, 3>::Zero();
	Eigen::Matrix<double, 3, 1> grad = Eigen::Matrix<double, 3, 1>::Zero();

	double cost_old = computeError(clones_cam, feat, alpha, beta, rho);

	const Eigen::Matrix<double, 3, 3> &R_GtoA = clones_cam.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).Rot();
	const Eigen::Matrix<double, 3, 1> &p_AinG = clones_cam.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).pos();

	while (runs < options.max_runs && lam < options.max_lamda && eps > options.min_dx)
	{
		if (recompute)
		{
			Hess.setZero();
			grad.setZero();

			double err = 0;

			for (auto const &pair : feat->timestamps)
			{
				for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++)
				{
					const Eigen::Matrix<double, 3, 3> &R_GtoCi = clones_cam.at(pair.first).at(feat->timestamps[pair.first].at(m)).Rot();
					const Eigen::Matrix<double, 3, 1> &p_CiinG = clones_cam.at(pair.first).at(feat->timestamps[pair.first].at(m)).pos();

					Eigen::Matrix<double, 3, 3> R_AtoCi;
					R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
					Eigen::Matrix<double, 3, 1> p_CiinA;
					p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
					Eigen::Matrix<double, 3, 1> p_AinCi;
					p_AinCi.noalias() = -R_AtoCi * p_CiinA;

					double hi1 = R_AtoCi(0, 0) * alpha + R_AtoCi(0, 1) * beta + R_AtoCi(0, 2) + rho * p_AinCi(0, 0);
					double hi2 = R_AtoCi(1, 0) * alpha + R_AtoCi(1, 1) * beta + R_AtoCi(1, 2) + rho * p_AinCi(1, 0);
					double hi3 = R_AtoCi(2, 0) * alpha + R_AtoCi(2, 1) * beta + R_AtoCi(2, 2) + rho * p_AinCi(2, 0);

					double d_z1_d_alpha = (R_AtoCi(0, 0) * hi3 - hi1 * R_AtoCi(2, 0)) / (pow(hi3, 2));
					double d_z1_d_beta = (R_AtoCi(0, 1) * hi3 - hi1 * R_AtoCi(2, 1)) / (pow(hi3, 2));
					double d_z1_d_rho = (p_AinCi(0, 0) * hi3 - hi1 * p_AinCi(2, 0)) / (pow(hi3, 2));
					double d_z2_d_alpha = (R_AtoCi(1, 0) * hi3 - hi2 * R_AtoCi(2, 0)) / (pow(hi3, 2));
					double d_z2_d_beta = (R_AtoCi(1, 1) * hi3 - hi2 * R_AtoCi(2, 1)) / (pow(hi3, 2));
					double d_z2_d_rho = (p_AinCi(1, 0) * hi3 - hi2 * p_AinCi(2, 0)) / (pow(hi3, 2));
					Eigen::Matrix<double, 2, 3> H;
					H << d_z1_d_alpha, d_z1_d_beta, d_z1_d_rho, d_z2_d_alpha, d_z2_d_beta, d_z2_d_rho;

					Eigen::Matrix<float, 2, 1> z;
					z << hi1 / hi3, hi2 / hi3;
					Eigen::Matrix<float, 2, 1> res = feat->uvs_norm.at(pair.first).at(m) - z;

					err += std::pow(res.norm(), 2);
					grad.noalias() += H.transpose() * res.cast<double>();
					Hess.noalias() += H.transpose() * H;
				}
			}
		}

		Eigen::Matrix<double, 3, 3> Hess_l = Hess;

		for (size_t r = 0; r < (size_t)Hess.rows(); r++)
		{
			Hess_l(r, r) *= (1.0 + lam);
		}

		Eigen::Matrix<double, 3, 1> dx = Hess_l.colPivHouseholderQr().solve(grad);

		double cost = computeError(clones_cam, feat, alpha + dx(0, 0), beta + dx(1, 0), rho + dx(2, 0));

		if (cost <= cost_old && (cost_old - cost) / cost_old < options.min_dcost)
		{
			alpha += dx(0, 0);
			beta += dx(1, 0);
			rho += dx(2, 0);
			eps = 0;
			break;
		}

		if (cost <= cost_old)
		{
			recompute = true;
			cost_old = cost;
			alpha += dx(0, 0);
			beta += dx(1, 0);
			rho += dx(2, 0);
			runs++;
			lam = lam / options.lam_mult;
			eps = dx.norm();
		}
		else
		{
			recompute = false;
			lam = lam * options.lam_mult;
			continue;
		}
	}

	feat->position_anchor(0) = alpha / rho;
	feat->position_anchor(1) = beta / rho;
	feat->position_anchor(2) = 1 / rho;

	Eigen::HouseholderQR<Eigen::MatrixXd> qr(feat->position_anchor);
	Eigen::MatrixXd Q = qr.householderQ();

	double base_line_max = 0.0;

	for (auto const &pair : feat->timestamps)
	{
		for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++)
		{
			const Eigen::Matrix<double, 3, 1> &p_CiinG = clones_cam.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();

			Eigen::Matrix<double, 3, 1> p_CiinA = R_GtoA * (p_CiinG - p_AinG);

			double base_line = ((Q.block(0, 1, 3, 2)).transpose() * p_CiinA).norm();

			if (base_line > base_line_max)
				base_line_max = base_line;
		}
	}

	if (feat->position_anchor(2) < options.min_dist || feat->position_anchor(2) > options.max_dist ||
		(feat->position_anchor.norm() / base_line_max) > options.max_baseline || std::isnan(feat->position_anchor.norm()))
	{
		return false;
	}

	feat->position_global = R_GtoA.transpose() * feat->position_anchor + p_AinG;

	return true;
}
  protected:
    featureInitializerOptions options;
    double computeError(std::unordered_map<size_t, std::unordered_map<double, clonePose>> &clones_cam, std::shared_ptr<feature> feat,
    double alpha, double beta, double rho) {
	double err = 0;

	const Eigen::Matrix<double, 3, 3> &R_GtoA = clones_cam.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).Rot();
	const Eigen::Matrix<double, 3, 1> &p_AinG = clones_cam.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).pos();

	for (auto const &pair : feat->timestamps)
	{
		for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++)
		{
			const Eigen::Matrix<double, 3, 3> &R_GtoCi = clones_cam.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
			const Eigen::Matrix<double, 3, 1> &p_CiinG = clones_cam.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();

			Eigen::Matrix<double, 3, 3> R_AtoCi;
			R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
			Eigen::Matrix<double, 3, 1> p_CiinA;
			p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
			Eigen::Matrix<double, 3, 1> p_AinCi;
			p_AinCi.noalias() = -R_AtoCi * p_CiinA;

			double hi1 = R_AtoCi(0, 0) * alpha + R_AtoCi(0, 1) * beta + R_AtoCi(0, 2) + rho * p_AinCi(0, 0);
			double hi2 = R_AtoCi(1, 0) * alpha + R_AtoCi(1, 1) * beta + R_AtoCi(1, 2) + rho * p_AinCi(1, 0);
			double hi3 = R_AtoCi(2, 0) * alpha + R_AtoCi(2, 1) * beta + R_AtoCi(2, 2) + rho * p_AinCi(2, 0);

			Eigen::Matrix<float, 2, 1> z;
			z << hi1 / hi3, hi2 / hi3;
			Eigen::Matrix<float, 2, 1> res = feat->uvs_norm.at(pair.first).at(m) - z;

			err += pow(res.norm(), 2);
		}
	}

	return err;
}
};

class baseType {
  private:
    stateHelper() {

}
  public:
    static void ekfPropagation(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &order_new,
		const std::vector<std::shared_ptr<baseType>> &order_old, const Eigen::MatrixXd &phi, const Eigen::MatrixXd &Q) {
	if (order_new.empty() || order_old.empty())
	{
		std::cout << "[stateHelper::ekfPropagation]: Called with empty variable arrays!" << std::endl;
		std::exit(EXIT_FAILURE);
	}

	int size_order_new = order_new.at(0)->getSize();
	for (size_t i = 0; i < order_new.size() - 1; i++)
	{
		if (order_new.at(i)->getId() + order_new.at(i)->getSize() != order_new.at(i + 1)->getId())
		{
			std::cout << "[stateHelper::ekfPropagation]: Called with non-contiguous state elements!" << std::endl;
			std::cout << "[stateHelper::ekfPropagation]: This code only support a state transition which is in the same order as the state." << std::endl;
			std::exit(EXIT_FAILURE);
		}
		size_order_new += order_new.at(i + 1)->getSize();
	}

	int size_order_old = order_old.at(0)->getSize();
	for (size_t i = 0; i < order_old.size() - 1; i++)
	{
		size_order_old += order_old.at(i + 1)->getSize();
	}

	assert(size_order_new == phi.rows());
	assert(size_order_old == phi.cols());
	assert(size_order_new == Q.cols());
	assert(size_order_new == Q.rows());

	int current_it = 0;
	std::vector<int> phi_id;
	for (const auto &var : order_old)
	{
		phi_id.push_back(current_it);
		current_it += var->getSize();
	}

	Eigen::MatrixXd cov_phiT = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), phi.rows());
	for (size_t i = 0; i < order_old.size(); i++)
	{
		std::shared_ptr<baseType> var = order_old.at(i);
		cov_phiT.noalias() += state_ptr->cov.block(0, var->getId(), state_ptr->cov.rows(), var->getSize()) * phi.block(0, phi_id[i], phi.rows(), var->getSize()).transpose();
	}

	Eigen::MatrixXd phi_cov_phiT = Q.selfadjointView<Eigen::Upper>();
	for (size_t i = 0; i < order_old.size(); i++)
	{
		std::shared_ptr<baseType> var = order_old.at(i);
		phi_cov_phiT.noalias() += phi.block(0, phi_id[i], phi.rows(), var->getSize()) * cov_phiT.block(var->getId(), 0, var->getSize(), phi.rows());
	}

	int start_id = order_new.at(0)->getId();
	int phi_size = phi.rows();
	int total_size = state_ptr->cov.rows();
	state_ptr->cov.block(start_id, 0, phi_size, total_size) = cov_phiT.transpose();
	state_ptr->cov.block(0, start_id, total_size, phi_size) = cov_phiT;
	state_ptr->cov.block(start_id, start_id, phi_size, phi_size) = phi_cov_phiT;

	Eigen::VectorXd diags = state_ptr->cov.diagonal();
	bool found_neg = false;
	for (int i = 0; i < diags.rows(); i++)
	{
		if (diags(i) < 0.0)
		{
			std::cout << "[stateHelper::ekfPropagation]: Diagonal at " << i << " is " << diags(i) << std::endl;
			found_neg = true;
		}
	}

	if (found_neg)
	{
		std::exit(EXIT_FAILURE);
	}
}
    static void ekfUpdate(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &H_order,
		const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R) {
	assert(res.rows() == R.rows());
	assert(H.rows() == res.rows());
	Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), res.rows());

	int current_it = 0;
	std::vector<int> H_id;
	for (const auto &meas_var : H_order)
	{
		H_id.push_back(current_it);
		current_it += meas_var->getSize();
	}

	for (const auto &var : state_ptr->variables)
	{
		// Sum up effect of each subjacobian = K_i= \sum_m (P_im Hm^T)
		Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->getSize(), res.rows());
		for (size_t i = 0; i < H_order.size(); i++)
		{
			std::shared_ptr<baseType> meas_var = H_order[i];
			M_i.noalias() += state_ptr->cov.block(var->getId(), meas_var->getId(), var->getSize(), meas_var->getSize()) *
				H.block(0, H_id[i], H.rows(), meas_var->getSize()).transpose();
		}
		M_a.block(var->getId(), 0, var->getSize(), res.rows()) = M_i;
	}

	Eigen::MatrixXd P_small = stateHelper::getMarginalCovariance(state_ptr, H_order);
	Eigen::MatrixXd S = H * P_small * H.transpose() + R;
	Eigen::MatrixXd S_inv = Eigen::MatrixXd::Identity(R.rows(), R.rows());
	S.selfadjointView<Eigen::Upper>().llt().solveInPlace(S_inv);
	Eigen::MatrixXd K = M_a * S_inv.selfadjointView<Eigen::Upper>();

	state_ptr->cov.triangularView<Eigen::Upper>() -= K * M_a.transpose();
	state_ptr->cov = state_ptr->cov.selfadjointView<Eigen::Upper>();

	Eigen::VectorXd diags = state_ptr->cov.diagonal();
	bool found_neg = false;

	for (int i = 0; i < diags.rows(); i++)
	{
		if (diags(i) < 0.0)
		{
			std::cout << "[stateHelper::ekfUpdate: Diagonal at " << i << " is " << diags(i) << std::endl;
			found_neg = true;
		}
	}

	if (found_neg)
	{
		std::exit(EXIT_FAILURE);
	}

	Eigen::VectorXd dx = K * res;
	for (size_t i = 0; i < state_ptr->variables.size(); i++)
	{
		state_ptr->variables.at(i)->update(dx.block(state_ptr->variables.at(i)->getId(), 0, state_ptr->variables.at(i)->getSize(), 1));
	}

	if (state_ptr->options.do_calib_camera_intrinsics)
	{
		for (auto const &calib : state_ptr->cam_intrinsics)
		{
			state_ptr->cam_intrinsics_cameras.at(calib.first)->setValue(calib.second->value());
		}
	}
}
    static void setInitialCovariance(std::shared_ptr<state> state_ptr, const Eigen::MatrixXd &covariance,
		const std::vector<std::shared_ptr<baseType>> &order) {
	int i_index = 0;

	for (size_t i = 0; i < order.size(); i++)
	{
		int k_index = 0;
		for (size_t k = 0; k < order.size(); k++)
		{
			state_ptr->cov.block(order[i]->getId(), order[k]->getId(), order[i]->getSize(), order[k]->getSize()) =
				covariance.block(i_index, k_index, order[i]->getSize(), order[k]->getSize());
			k_index += order[k]->getSize();
		}
		i_index += order[i]->getSize();
	}
	state_ptr->cov = state_ptr->cov.selfadjointView<Eigen::Upper>();
}
    static Eigen::MatrixXd getMarginalCovariance(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &small_variables) {
	int cov_size = 0;
	for (size_t i = 0; i < small_variables.size(); i++)
	{
		cov_size += small_variables[i]->getSize();
	}

	Eigen::MatrixXd small_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);
	int i_index = 0;

	for (size_t i = 0; i < small_variables.size(); i++)
	{
		int k_index = 0;

		for (size_t k = 0; k < small_variables.size(); k++)
		{
			small_cov.block(i_index, k_index, small_variables[i]->getSize(), small_variables[k]->getSize()) =
				state_ptr->cov.block(small_variables[i]->getId(), small_variables[k]->getId(), small_variables[i]->getSize(), small_variables[k]->getSize());
			k_index += small_variables[k]->getSize();
		}

		i_index += small_variables[i]->getSize();
	}

	return small_cov;
}
    static Eigen::MatrixXd getFullCovariance(std::shared_ptr<state> state_ptr) {
	int cov_size = (int)state_ptr->cov.rows();

	Eigen::MatrixXd full_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);

	full_cov.block(0, 0, state_ptr->cov.rows(), state_ptr->cov.rows()) = state_ptr->cov;

	return full_cov;
}
    static void marginalize(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> marg) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), marg) == state_ptr->variables.end())
	{
		std::cout << "[stateHelper::marginalize]: Called on variable that is not in the state." << std::endl;
		std::cout << "[stateHelper::marginalize]: Marginalization, does NOT work on sub-variables yet..." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	int marg_size = marg->getSize();
	int marg_id = marg->getId();
	int x2_size = (int)state_ptr->cov.rows() - marg_id - marg_size;

	Eigen::MatrixXd cov_new(state_ptr->cov.rows() - marg_size, state_ptr->cov.rows() - marg_size);

	cov_new.block(0, 0, marg_id, marg_id) = state_ptr->cov.block(0, 0, marg_id, marg_id);
	cov_new.block(0, marg_id, marg_id, x2_size) = state_ptr->cov.block(0, marg_id + marg_size, marg_id, x2_size);
	cov_new.block(marg_id, 0, x2_size, marg_id) = cov_new.block(0, marg_id, marg_id, x2_size).transpose();
	cov_new.block(marg_id, marg_id, x2_size, x2_size) = state_ptr->cov.block(marg_id + marg_size, marg_id + marg_size, x2_size, x2_size);

	state_ptr->cov = cov_new;
	assert(state_ptr->cov.rows() == cov_new.rows());

	std::vector<std::shared_ptr<baseType>> remaining_variables;

	for (size_t i = 0; i < state_ptr->variables.size(); i++)
	{
		if (state_ptr->variables.at(i) != marg)
		{
			if (state_ptr->variables.at(i)->getId() > marg_id)
			{
				state_ptr->variables.at(i)->setLocalId(state_ptr->variables.at(i)->getId() - marg_size);
			}
			remaining_variables.push_back(state_ptr->variables.at(i));
		}
	}

	marg->setLocalId(-1);
	state_ptr->variables = remaining_variables;
}
    static std::shared_ptr<baseType> clone(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> variable_to_clone) {
	int total_size = variable_to_clone->getSize();
	int old_size = (int)state_ptr->cov.rows();
	int new_loc = (int)state_ptr->cov.rows();

	state_ptr->cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_size + total_size, old_size + total_size));

	const std::vector<std::shared_ptr<baseType>> new_variables = state_ptr->variables;
	std::shared_ptr<baseType> new_clone = nullptr;

	for (size_t k = 0; k < state_ptr->variables.size(); k++)
	{
		std::shared_ptr<baseType> type_check = state_ptr->variables.at(k)->checkIfSubvariable(variable_to_clone);
		if (state_ptr->variables.at(k) == variable_to_clone)
		{
			type_check = state_ptr->variables.at(k);
		}
		else if (type_check != variable_to_clone)
		{
			continue;
		}

		int old_loc = type_check->getId();

		state_ptr->cov.block(new_loc, new_loc, total_size, total_size) = state_ptr->cov.block(old_loc, old_loc, total_size, total_size);
		state_ptr->cov.block(0, new_loc, old_size, total_size) = state_ptr->cov.block(0, old_loc, old_size, total_size);
		state_ptr->cov.block(new_loc, 0, total_size, old_size) = state_ptr->cov.block(old_loc, 0, total_size, old_size);

		new_clone = type_check->clone();
		new_clone->setLocalId(new_loc);
		break;
	}

	if (new_clone == nullptr)
	{
		std::cout << "[stateHelper::clone]: Called on variable is not in the state." << std::endl;
		std::cout << "[stateHelper::clone]: Ensure that the variable specified is a variable, or sub-variable." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	state_ptr->variables.push_back(new_clone);
	return new_clone;
}
    static bool initialize(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> new_variable,
		const std::vector<std::shared_ptr<baseType>> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L,
		Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), new_variable) != state_ptr->variables.end())
	{
		std::cout << "[stateHelper::initializeInvertible]: Called on variable that is already in the state." << std::endl;
		std::cout << "[stateHelper::initializeInvertible]: Found this variable at " << new_variable->getId() << " in covariance." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	assert(R.rows() == R.cols());
	assert(R.rows() > 0);
	for (int r = 0; r < R.rows(); r++)
	{
		for (int c = 0; c < R.cols(); c++)
		{
			if (r == c && R(0, 0) != R(r, c))
			{
				std::cout << "[stateHelper::initialize]: Your noise is not isotropic!" << std::endl;
				std::cout << "[stateHelper::initialize]: Found a value of " << R(r, c) << " verses value of " << R(0, 0) << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
			else if (r != c && R(r, c) != 0.0)
			{
				std::cout << "[stateHelper::initialize]: Your noise is not diagonal!" << std::endl;
				std::cout << "[stateHelper::initialize]: Found a value of " << R(r, c) << " at row " << r << " and column " << c <<"." << std::endl;
				std::exit(EXIT_FAILURE);
			}
		}
	}

	size_t new_var_size = new_variable->getSize();
	assert((int)new_var_size == H_L.cols());

	Eigen::JacobiRotation<double> tempHo_GR;
	for (int n = 0; n < H_L.cols(); ++n)
	{
		for (int m = (int)H_L.rows() - 1; m > n; m--)
		{
			tempHo_GR.makeGivens(H_L(m - 1, n), H_L(m, n));
			(H_L.block(m - 1, n, 2, H_L.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
			(H_R.block(m - 1, 0, 2, H_R.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
		}
	}

	Eigen::MatrixXd Hxinit = H_R.block(0, 0, new_var_size, H_R.cols());
	Eigen::MatrixXd H_finit = H_L.block(0, 0, new_var_size, new_var_size);
	Eigen::VectorXd resinit = res.block(0, 0, new_var_size, 1);
	Eigen::MatrixXd Rinit = R.block(0, 0, new_var_size, new_var_size);

	Eigen::MatrixXd Hup = H_R.block(new_var_size, 0, H_R.rows() - new_var_size, H_R.cols());
	Eigen::VectorXd resup = res.block(new_var_size, 0, res.rows() - new_var_size, 1);
	Eigen::MatrixXd Rup = R.block(new_var_size, new_var_size, R.rows() - new_var_size, R.rows() - new_var_size);

	Eigen::MatrixXd P_up = stateHelper::getMarginalCovariance(state_ptr, H_order);
	assert(Rup.rows() == Hup.rows());
	assert(Hup.cols() == P_up.cols());
	Eigen::MatrixXd S = Hup * P_up * Hup.transpose() + Rup;
	double chi2 = resup.dot(S.llt().solve(resup));

	boost::math::chi_squared chi_squared_dist(res.rows());
	double chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
	if (chi2 > chi_2_mult * chi2_check)
	{
		return false;
	}

	stateHelper::initializeInvertible(state_ptr, new_variable, H_order, Hxinit, H_finit, Rinit, resinit);

	if (Hup.rows() > 0)
	{
		stateHelper::ekfUpdate(state_ptr, H_order, Hup, resup, Rup);
	}
	return true;
}
    static void initializeInvertible(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> new_variable,
		const std::vector<std::shared_ptr<baseType>> &H_order, const Eigen::MatrixXd &H_R,
		const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), new_variable) != state_ptr->variables.end())
	{
		std::cout << "[stateHelper::initializeInvertible]: Called on variable that is already in the state." << std::endl;
		std::cout << "[stateHelper::initializeInvertible]: Found this variable at " << new_variable->getId() << " in covariance." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	assert(R.rows() == R.cols());
	assert(R.rows() > 0);
	for (int r = 0; r < R.rows(); r++)
	{
		for (int c = 0; c < R.cols(); c++)
		{
			if (r == c && R(0, 0) != R(r, c))
			{
				std::cout << "[stateHelper::initializeInvertible]: Your noise is not isotropic!" << std::endl;
				std::cout << "[stateHelper::initializeInvertible]: Found a value of " << R(r, c) << " verses value of " << R(0, 0) << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
			else if (r != c && R(r, c) != 0.0)
			{
				std::cout << "[stateHelper::initializeInvertible]: Your noise is not diagonal!" << std::endl;
				std::cout << "[stateHelper::initializeInvertible]: Found a value of " << R(r, c) << " at row " << r << " and column " << c << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
		}
	}

	assert(res.rows() == R.rows());
	assert(H_L.rows() == res.rows());
	assert(H_L.rows() == H_R.rows());
	Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), res.rows());

	int current_it = 0;
	std::vector<int> H_id;
	for (const auto &meas_var : H_order) {
		H_id.push_back(current_it);
		current_it += meas_var->getSize();
	}

	for (const auto &var : state_ptr->variables)
	{
		Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->getSize(), res.rows());
		for (size_t i = 0; i < H_order.size(); i++)
		{
			std::shared_ptr<baseType> meas_var = H_order.at(i);
			M_i += state_ptr->cov.block(var->getId(), meas_var->getId(), var->getSize(), meas_var->getSize()) *
				H_R.block(0, H_id[i], H_R.rows(), meas_var->getSize()).transpose();
		}
		M_a.block(var->getId(), 0, var->getSize(), res.rows()) = M_i;
	}

	Eigen::MatrixXd P_small = stateHelper::getMarginalCovariance(state_ptr, H_order);

	Eigen::MatrixXd M(H_R.rows(), H_R.rows());
	M.triangularView<Eigen::Upper>() = H_R * P_small * H_R.transpose();
	M.triangularView<Eigen::Upper>() += R;

	assert(H_L.rows() == H_L.cols());
	assert(H_L.rows() == new_variable->getSize());
	Eigen::MatrixXd H_Linv = H_L.inverse();
	Eigen::MatrixXd P_LL = H_Linv * M.selfadjointView<Eigen::Upper>() * H_Linv.transpose();

	size_t oldSize = state_ptr->cov.rows();
	state_ptr->cov.conservativeResizeLike(Eigen::MatrixXd::Zero(oldSize + new_variable->getSize(), oldSize + new_variable->getSize()));
	state_ptr->cov.block(0, oldSize, oldSize, new_variable->getSize()).noalias() = -M_a * H_Linv.transpose();
	state_ptr->cov.block(oldSize, 0, new_variable->getSize(), oldSize) = state_ptr->cov.block(0, oldSize, oldSize, new_variable->getSize()).transpose();
	state_ptr->cov.block(oldSize, oldSize, new_variable->getSize(), new_variable->getSize()) = P_LL;

	new_variable->update(H_Linv * res);
	new_variable->setLocalId(oldSize);
	state_ptr->variables.push_back(new_variable);
}
    static void augmentClone(std::shared_ptr<state> state_ptr, Eigen::Matrix<double, 3, 1> last_gyr) {
	if (state_ptr->clones_imu.find(state_ptr->timestamp) != state_ptr->clones_imu.end())
	{
		std::exit(EXIT_FAILURE);
	}

	std::shared_ptr<baseType> pose_temp = stateHelper::clone(state_ptr, state_ptr->imu_ptr->pose());

	std::shared_ptr<poseJpl> pose = std::dynamic_pointer_cast<poseJpl>(pose_temp);
	if (pose == nullptr)
	{
		std::exit(EXIT_FAILURE);
	}

	state_ptr->clones_imu[state_ptr->timestamp] = pose;

	if (state_ptr->options.do_calib_camera_timeoffset)
	{
		Eigen::Matrix<double, 6, 1> dnc_dt = Eigen::MatrixXd::Zero(6, 1);
		dnc_dt.block(0, 0, 3, 1) = last_gyr;
		dnc_dt.block(3, 0, 3, 1) = state_ptr->imu_ptr->getVel();

		state_ptr->cov.block(0, pose->getId(), state_ptr->cov.rows(), 6) +=
			state_ptr->cov.block(0, state_ptr->calib_dt_imu_cam->getId(), state_ptr->cov.rows(), 1) * dnc_dt.transpose();
		state_ptr->cov.block(pose->getId(), 0, 6, state_ptr->cov.rows()) +=
			dnc_dt * state_ptr->cov.block(state_ptr->calib_dt_imu_cam->getId(), 0, 1, state_ptr->cov.rows());
	}
}
    static void marginalizeOldClone(std::shared_ptr<state> state_ptr) {
	if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
	{
		double marginal_time = state_ptr->margtimestep();

		assert(marginal_time != INFINITY);
		stateHelper::marginalize(state_ptr, state_ptr->clones_imu.at(marginal_time));

		state_ptr->clones_imu.erase(marginal_time);

		assert(state_ptr->clones_frame.at(marginal_time)->v_feat_ptr[0].size() == 0);
		assert(state_ptr->clones_frame.at(marginal_time)->v_feat_ptr[1].size() == 0);
		state_ptr->clones_frame.at(marginal_time)->release();
		state_ptr->clones_frame.erase(marginal_time);
	}
}
    static void marginalizeNewClone(std::shared_ptr<state> state_ptr) {
	if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
	{
		std::vector<double> clone_times;
		for (const auto &clone_imu : state_ptr->clones_imu)
		{
			clone_times.emplace_back(clone_imu.first);
		}

		double marginal_time = clone_times.at(clone_times.size() - 2);

		assert(marginal_time != INFINITY);
		stateHelper::marginalize(state_ptr, state_ptr->clones_imu.at(marginal_time));

		std::shared_ptr<frame> fh_temp = state_ptr->clones_frame.at(marginal_time);

		if (fh_temp->v_feat_ptr.find(0) != fh_temp->v_feat_ptr.end())
		{
			auto it0 = fh_temp->v_feat_ptr.at(0).begin();

			while (it0 != fh_temp->v_feat_ptr.at(0).end())
			{
				std::vector<double> marginal_timestamps = {marginal_time};
				(*it0).second->cleanInvalidMeasurements(marginal_timestamps);
				it0 = fh_temp->v_feat_ptr.at(0).begin();
			}
		}

		if (fh_temp->v_feat_ptr.find(1) != fh_temp->v_feat_ptr.end())
		{
			auto it1 = fh_temp->v_feat_ptr.at(1).begin();

			while (it1 != fh_temp->v_feat_ptr.at(1).end())
			{
				std::vector<double> marginal_timestamps = {marginal_time};
				(*it1).second->cleanInvalidMeasurements(marginal_timestamps);
				it1 = fh_temp->v_feat_ptr.at(1).begin();
			}
		}

		state_ptr->clones_imu.erase(marginal_time);
		state_ptr->clones_frame.at(marginal_time)->release();
		state_ptr->clones_frame.erase(marginal_time);
	}
}
    static void marginalizeSlam(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history) {
	// display
	points_history->clear();
	// display

	int ct_marginalized = 0;
	auto it0 = state_ptr->map_points.begin();
	while (it0 != state_ptr->map_points.end())
	{
		if ((*it0).second->should_marg)
		{
			mapManagement::deleteFromVoxel(voxel_map, (*it0).second);
			stateHelper::marginalize(state_ptr, (*it0).second);

			// display
			pcl::PointXYZRGB point_temp;
			point_temp.x = (*it0).second->getPointXYZ(false)[0];
			point_temp.y = (*it0).second->getPointXYZ(false)[1];
			point_temp.z = (*it0).second->getPointXYZ(false)[2];
			point_temp.r = (*it0).second->color;
			point_temp.g = (*it0).second->color;
			point_temp.b = (*it0).second->color;
			points_history->points.push_back(point_temp);
			// display

			it0 = state_ptr->map_points.erase(it0);
			ct_marginalized++;
		}
		else
		{
			it0++;
		}
	}
}
};

class stateOptions {
  public:
    bool do_fej = true;
    bool do_calib_camera_pose = false;
    bool do_calib_camera_intrinsics = false;
    bool do_calib_camera_timeoffset = false;
    bool do_calib_imu_intrinsics = false;
    bool do_calib_imu_g_sensitivity = false;
    ImuModel imu_model = ImuModel::KALIBR;
    int max_clone_size = 11;
    int max_slam_features = 25;
    int max_slam_in_update = 1000;
    int max_msckf_in_update = 1000;
    double voxel_size = 0.5;
    int max_num_points_in_voxel = 10;
    double min_distance_points = 0.15;
    bool use_huber = true;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double init_window_time = 1.0;
    double init_imu_thresh = 1.0;
    double init_max_disparity = 1.0;
    int init_max_features = 50;
    bool init_dyn_use = false;
    bool init_dyn_mle_opt_calib = false;
    int init_dyn_mle_max_iter = 20;
    int init_dyn_mle_max_threads = 20;
    double init_dyn_mle_max_time = 5.0;
    int init_dyn_num_pose = 5;
    double init_dyn_min_deg = 45.0;
    double init_dyn_inflation_orientation = 10.0;
    double init_dyn_inflation_velocity = 10.0;
    double init_dyn_inflation_bias_gyro = 100.0;
    double init_dyn_inflation_bias_accel = 100.0;
    double init_dyn_min_rec_cond = 1e-15;
    Eigen::Vector3d init_dyn_bias_g = Eigen::Vector3d::Zero();
    Eigen::Vector3d init_dyn_bias_a = Eigen::Vector3d::Zero();
    double sigma_w = 1.6968e-04;
    double sigma_wb = 1.9393e-05;
    double sigma_a = 2.0000e-3;
    double sigma_ab = 3.0000e-03;
    double sigma_pix = 1;
    double gravity_mag = 9.81;
    bool downsample_cameras = false;
    double calib_camimu_dt = 0.0;
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_intrinsics;
    std::map<size_t, Eigen::VectorXd> camera_extrinsics;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    bool refine_features = true;
    int max_runs = 5;
    double init_lamda = 1e-3;
    double max_lamda = 1e10;
    double min_dx = 1e-6;
    double min_dcost = 1e-6;
    double lam_mult = 10;
    double min_dist = 0.10;
    double max_dist = 60;
    double max_baseline = 40;
    double max_cond_number = 10000;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double sigma_w = 1.6968e-04;
    double sigma_w_2 = pow(1.6968e-04, 2);
    double sigma_wb = 1.9393e-05;
    double sigma_wb_2 = pow(1.9393e-05, 2);
    double sigma_a = 2.0000e-3;
    double sigma_a_2 = pow(2.0000e-3, 2);
    double sigma_ab = 3.0000e-03;
    double sigma_ab_2 = pow(3.0000e-03, 2);
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double chi2_multipler = 5;
    double sigma_pix = 1;
    double sigma_pix_sq = 1;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    stateOptions state_options;
    inertialInitializerOptions init_options;
    double dt_slam_delay = 2.0;
    noiseManager imu_noises;
    updaterOptions msckf_options;
    updaterOptions slam_options;
    double gravity_mag = 9.81;
    Eigen::Matrix<double, 6, 1> vec_dw;
    Eigen::Matrix<double, 6, 1> vec_da;
    Eigen::Matrix<double, 9, 1> vec_tg;
    Eigen::Matrix<double, 4, 1> q_imu_acc;
    Eigen::Matrix<double, 4, 1> q_imu_gyr;
    double calib_camimu_dt = 0.0;
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_intrinsics;
    std::map<size_t, Eigen::VectorXd> camera_extrinsics;
    bool use_mask = false;
    std::map<size_t, cv::Mat> masks;
    bool downsample_cameras = false;
    int num_pts = 150;
    int fast_threshold = 20;
    int patch_size_x = 5;
    int patch_size_y = 5;
    int min_px_dist = 10;
    HistogramMethod histogram_method = HistogramMethod::HISTOGRAM;
    double track_frequency = 20.0;
    featureInitializerOptions featinit_options;
    double voxel_size = 0.5;
    int max_num_points_in_voxel = 10;
    double min_distance_points = 0.15;
    int nb_voxels_visited = 1;
    bool use_all_points = false;
    bool use_keyframe = true;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
};

class cameraBase {
  public:
    cameraBase(int width, int height) {

}
    virtual void setValue(const Eigen::MatrixXd &calib) {
	assert(calib.rows() == 8);
	camera_values = calib;

	cv::Matx33d tempK;
	tempK(0, 0) = calib(0);
	tempK(0, 1) = 0;
	tempK(0, 2) = calib(2);
	tempK(1, 0) = 0;
	tempK(1, 1) = calib(1);
	tempK(1, 2) = calib(3);
	tempK(2, 0) = 0;
	tempK(2, 1) = 0;
	tempK(2, 2) = 1;
	camera_k_OPENCV = tempK;

	cv::Vec4d tempD;
	tempD(0) = calib(4);
	tempD(1) = calib(5);
	tempD(2) = calib(6);
	tempD(3) = calib(7);
	camera_d_OPENCV = tempD;
}
    virtual Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) = 0 {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2d undistortD(const Eigen::Vector2d &uv_dist) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 = uv_dist.cast<float>();
	ept_2 = undistortF(ept_1);

	return ept_2.cast<double>();
}
    cv::Point2f undistortCV(const cv::Point2f &uv_dist) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 << uv_dist.x, uv_dist.y;
	ept_2 = undistortF(ept_1);
	cv::Point2f pt_out;
	pt_out.x = ept_2(0);
	pt_out.y = ept_2(1);

	return pt_out;
}
    virtual Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) = 0 {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    Eigen::Vector2d distortD(const Eigen::Vector2d &uv_norm) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 = uv_norm.cast<float>();
	ept_2 = distortF(ept_1);

	return ept_2.cast<double>();
}
    cv::Point2f distortCV(const cv::Point2f &uv_norm) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 << uv_norm.x, uv_norm.y;
	ept_2 = distortF(ept_1);
	cv::Point2f pt_out;
	pt_out.x = ept_2(0);
	pt_out.y = ept_2(1);

	return pt_out;
}
    virtual void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) = 0 {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
    Eigen::MatrixXd getValue() {
	return camera_values;
}
    cv::Matx33d getK() {
	return camera_k_OPENCV;
}
    cv::Vec4d getD() {
	return camera_d_OPENCV;
}
    int w() {
	return width;
}
    int h() {
	return height;
}
  protected:
    Eigen::MatrixXd camera_values;
    cv::Matx33d camera_k_OPENCV;
    cv::Vec4d camera_d_OPENCV;
    int width;
    int height;
  public:
    Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) override {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
  public:
    Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) override {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
};

class pixelSelector {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    pixelSelector(int w, int h) {
	random_pattern = new unsigned char[w * h];

	std::srand(3141592);

	for (int i = 0; i < w * h; i++)
		random_pattern[i] = rand() & 0xFF;

	current_potential = 3;

	grad_hist = new int[100 * (1 + w / 32) * (1 + h / 32)];
	ths = new float[(w / 32) * (h / 32) + 100];
	ths_smoothed = new float[(w / 32) * (h / 32) + 100];

	allow_fast = false;
	grad_hist_frame = nullptr;
}
    ~pixelSelector() {
	delete[] random_pattern;
	delete[] grad_hist;
	delete[] ths;
	delete[] ths_smoothed;
}
    int pixelSelectionLeft(std::shared_ptr<frame> fh, float density, int recursions_left = 1, float th_factor = 1) {
	float num_have = 0;
	float num_want = density;
	float quotia;
	int ideal_potential = current_potential;

	// if(fh != grad_hist_frame) makeHistsLeft(fh);
	makeHistsLeft(fh);

	Eigen::Vector3i n = this->selectLeft(fh, current_potential, th_factor);

	num_have = n[0] + n[1] + n[2];
	quotia = num_want / num_have;

	float K = num_have * (current_potential + 1) * (current_potential + 1);
	ideal_potential = sqrtf(K / num_want) - 1;

	if (ideal_potential < 1) ideal_potential = 1;

	if (recursions_left > 0 && quotia > 1.25 && current_potential > 1)
	{
		if(ideal_potential >= current_potential)
			ideal_potential = current_potential - 1;

		current_potential = ideal_potential;
		return pixelSelectionLeft(fh, density, recursions_left - 1, th_factor);
	}
	else if(recursions_left > 0 && quotia < 0.25)
	{
		if(ideal_potential <= current_potential)
			ideal_potential = current_potential + 1;

		current_potential = ideal_potential;
		return pixelSelectionLeft(fh, density, recursions_left - 1, th_factor);
	}

	int num_have_sub = num_have;

	if(quotia < 0.95)
	{
		int wh = wG[0] * hG[0];
		int rn = 0;
		unsigned char char_th = 255*quotia;
		for(int i = 0; i < wh; i++)
		{
			if(fh->selected_pixels_left[i] != 0)
			{
				if(random_pattern[rn] > char_th )
				{
					fh->selected_pixels_left[i] = 0;
					num_have_sub--;
				}
				rn++;
			}
		}
	}

	current_potential = ideal_potential;

	return num_have_sub;
}
    int pixelSelectionRight(std::shared_ptr<frame> fh, float density, int recursions_left = 1, float th_factor = 1) {
	float num_have = 0;
	float num_want = density;
	float quotia;
	int ideal_potential = current_potential;

	// if(fh != grad_hist_frame) makeHistsRight(fh);
	makeHistsRight(fh);

	Eigen::Vector3i n = this->selectRight(fh, current_potential, th_factor);

	num_have = n[0] + n[1] + n[2];
	quotia = num_want / num_have;

	float K = num_have * (current_potential + 1) * (current_potential + 1);
	ideal_potential = sqrtf(K / num_want) - 1;

	if (ideal_potential < 1) ideal_potential = 1;

	if (recursions_left > 0 && quotia > 1.25 && current_potential > 1)
	{
		if(ideal_potential >= current_potential)
			ideal_potential = current_potential - 1;

		current_potential = ideal_potential;
		return pixelSelectionRight(fh, density, recursions_left - 1, th_factor);
	}
	else if (recursions_left > 0 && quotia < 0.25)
	{
		if(ideal_potential <= current_potential)
			ideal_potential = current_potential + 1;

		current_potential = ideal_potential;
		return pixelSelectionRight(fh, density, recursions_left - 1, th_factor);
	}

	int num_have_sub = num_have;

	if (quotia < 0.95)
	{
		int wh = wG[0] * hG[0];
		int rn = 0;
		unsigned char char_th = 255*quotia;
		for (int i = 0; i < wh; i++)
		{
			if (fh->selected_pixels_right[i] != 0)
			{
				if (random_pattern[rn] > char_th)
				{
					fh->selected_pixels_right[i] = 0;
					num_have_sub--;
				}
				rn++;
			}
		}
	}

	current_potential = ideal_potential;

	return num_have_sub;
}
  private:
    void makeHistsLeft(std::shared_ptr<frame> fh) {
	grad_hist_frame = fh;
	float* mapmax_0 = fh->abs_squared_grad_left[0];

	int w = wG[0];
	int h = hG[0];

	int w32 = w / 32;
	int h32 = h / 32;
	ths_step = w32;

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float* map_0 = mapmax_0 + 32 * x + 32 * y * w;
			int* hist_0 = grad_hist;

			memset(hist_0, 0, sizeof(int) * 50);

			for(int j = 0; j < 32; j++)
				for(int i = 0; i < 32; i++)
				{
					int it = i + 32 * x;
					int jt = j + 32 * y;

					if(it > w - 2 || jt > h - 2 || it < 1 || jt < 1) continue;

					int g = sqrtf(map_0[i + j * w]);

					if(g > 48) g = 48;

					hist_0[g + 1]++;
					hist_0[0]++;
				}

			ths[x + y * w32] = computeHistQuantil(hist_0, setting_min_grad_hist_cut) + setting_min_grad_hist_add;
		}

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float sum = 0, num = 0;
			if(x > 0)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x - 1 + (y - 1) * w32];
				}

				if(y < h32 - 1)
				{
					num++;
					sum += ths[x - 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x - 1 + (y) * w32];
			}

			if(x < w32 - 1)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x + 1 + (y - 1) * w32];
				}
				if(y < h32 - 1)
				{
					num++;
					sum += ths[x + 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x + 1 + (y) * w32];
			}

			if(y > 0)
			{
				num++;
				sum += ths[x + (y - 1) * w32];
			}

			if(y < h32 - 1)
			{
				num++;
				sum += ths[x + (y + 1) * w32];
			}

			num++;
			sum += ths[x + y * w32];

			ths_smoothed[x + y * w32] = (sum / num) * (sum / num);
		}
}
    void makeHistsRight(std::shared_ptr<frame> fh) {
	grad_hist_frame = fh;
	float* mapmax_0 = fh->abs_squared_grad_right[0];

	int w = wG[0];
	int h = hG[0];

	int w32 = w / 32;
	int h32 = h / 32;
	ths_step = w32;

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float* map_0 = mapmax_0 + 32 * x + 32 * y * w;
			int* hist_0 = grad_hist;

			memset(hist_0, 0, sizeof(int) * 50);

			for(int j = 0; j < 32; j++)
				for(int i = 0; i < 32; i++)
				{
					int it = i + 32 * x;
					int jt = j + 32 * y;

					if(it > w - 2 || jt > h - 2 || it < 1 || jt < 1) continue;

					int g = sqrtf(map_0[i + j * w]);

					if(g > 48) g = 48;

					hist_0[g + 1]++;
					hist_0[0]++;
				}

			ths[x + y * w32] = computeHistQuantil(hist_0, setting_min_grad_hist_cut) + setting_min_grad_hist_add;
		}

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float sum = 0, num = 0;

			if(x > 0)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x - 1 + (y - 1) * w32];
				}

				if(y < h32 - 1)
				{
					num++;
					sum += ths[x - 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x - 1 + (y) * w32];
			}

			if(x < w32 - 1)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x + 1 + (y - 1) * w32];
				}
				if(y < h32 - 1)
				{
					num++;
					sum += ths[x + 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x + 1 + (y) * w32];
			}

			if(y > 0)
			{
				num++;
				sum += ths[x + (y - 1) * w32];
			}

			if(y < h32 - 1)
			{
				num++;
				sum += ths[x + (y + 1) * w32];
			}

			num++;
			sum += ths[x + y * w32];

			ths_smoothed[x + y * w32] = (sum / num) * (sum / num);
		}
}
    Eigen::Vector3i selectLeft(std::shared_ptr<frame> fh, int pot, float th_factor = 1) {
	Eigen::Vector3f const * const map_0 = fh->dI_left;

	float* mapmax_0 = fh->abs_squared_grad_left[0];
	float* mapmax_1 = fh->abs_squared_grad_left[1];
	float* mapmax_2 = fh->abs_squared_grad_left[2];

	int w = wG[0];
	int w1 = wG[1];
	int w2 = wG[2];
	int h = hG[0];

	const Eigen::Matrix<float, 2, 1> directions[16] = {
		Eigen::Matrix<float, 2, 1>(0, 1.0000),
		Eigen::Matrix<float, 2, 1>(0.3827, 0.9239),
		Eigen::Matrix<float, 2, 1>(0.1951, 0.9808),
		Eigen::Matrix<float, 2, 1>(0.9239, 0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, 0.7071),
		Eigen::Matrix<float, 2, 1>(0.3827, -0.9239),
		Eigen::Matrix<float, 2, 1>(0.8315, 0.5556),
		Eigen::Matrix<float, 2, 1>(0.8315, -0.5556),
		Eigen::Matrix<float, 2, 1>(0.5556, -0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, 0.1951),
		Eigen::Matrix<float, 2, 1>(0.9239, -0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, -0.7071),
		Eigen::Matrix<float, 2, 1>(0.5556, 0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, -0.1951),
		Eigen::Matrix<float, 2, 1>(1.0000, 0.0000),
		Eigen::Matrix<float, 2, 1>(0.1951, -0.9808)};

	memset(fh->selected_pixels_left, 0, w * h * sizeof(PixelSelectorStatus));

	float dw1 = setting_grad_down_weight_per_level;
	float dw2 = dw1 * dw1;

	int n3 = 0, n2 = 0, n4 = 0;

	for(int y4 = 0; y4 < h; y4 += (4 * pot))
	{
		for(int x4 = 0; x4 < w; x4 += (4 * pot))
		{
			int my3 = std::min((4 * pot), h - y4);
			int mx3 = std::min((4 * pot), w - x4);
			int best_idx_4 = -1;
			float best_val_4 = 0;

			Eigen::Matrix<float, 2, 1> dir4 = directions[random_pattern[n2] & 0xF];

			for(int y3 = 0; y3 < my3; y3 += (2 * pot))
			{
				for(int x3 = 0; x3 < mx3; x3 += (2 * pot))
				{
					int x34 = x3 + x4;
					int y34 = y3 + y4;
					int my2 = std::min((2 * pot), h - y34);
					int mx2 = std::min((2 * pot), w - x34);

					int best_idx_3 = -1;
					float best_val_3 = 0;

					Eigen::Matrix<float, 2, 1> dir3 = directions[random_pattern[n2] & 0xF];

					for(int y2 = 0; y2 < my2; y2 += pot)
					{
						for(int x2 = 0; x2 < mx2; x2 += pot)
						{
							int x234 = x2 + x34;
							int y234 = y2 + y34;
							int my1 = std::min(pot, h - y234);
							int mx1 = std::min(pot, w - x234);

							int best_idx_2 = -1;
							float best_val_2 = 0;

							Eigen::Matrix<float, 2, 1> dir2 = directions[random_pattern[n2] & 0xF];

							for(int y1 = 0; y1 < my1; y1 += 1)
							{
								for(int x1 = 0; x1 < mx1; x1 += 1)
								{
									assert(x1 + x234 < w);
									assert(y1 + y234 < h);

									int idx = x1 + x234 + w * (y1 + y234);
									int xf = x1 + x234;
									int yf = y1 + y234;

									if(xf < 4 || xf >= w - 5 || yf < 4 || yf > h - 4) continue;

									if (fh->mask_left[idx] == true) continue;

									float pixel_TH_0 = ths_smoothed[(xf >> 5) + (yf >> 5) * ths_step];
									float pixel_TH_1 = pixel_TH_0 * dw1;
									float pixel_TH_2 = pixel_TH_1 * dw2;

									float ag_0 = mapmax_0[idx];

									if(ag_0 > pixel_TH_0 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir2)));

										if(!setting_select_direction_distribution) dir_norm = ag_0;

										if(dir_norm > best_val_2)
										{
											best_val_2 = dir_norm;
											best_idx_2 = idx;
											best_idx_3 = -2;
											best_idx_4 = -2;
										}
									}

									if(best_idx_3 == -2) continue;

									float ag_1 = mapmax_1[(int)(xf * 0.5f + 0.25f) + (int)(yf * 0.5f + 0.25f) * w1];

									if(ag_1 > pixel_TH_1 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir3)));

										if(!setting_select_direction_distribution) dir_norm = ag_1;

										if(dir_norm > best_val_3)
										{
											best_val_3 = dir_norm;
											best_idx_3 = idx;
											best_idx_4 = -2;
										}
									}

									if(best_idx_4 == -2) continue;

									float ag_2 = mapmax_2[(int)(xf * 0.25f + 0.125) + (int)(yf * 0.25f + 0.125) * w2];

									if(ag_2 > pixel_TH_2 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir4)));

										if(!setting_select_direction_distribution) dir_norm = ag_2;

										if(dir_norm > best_val_4)
										{
											best_val_4 = dir_norm;
											best_idx_4 = idx;
										}
									}
								}
							}

							if(best_idx_2 > 0)
							{
								fh->selected_pixels_left[best_idx_2] = 1;
								best_val_3 = 1e10;
								n2++;
							}
						}
					}

					if(best_idx_3 > 0)
					{
						fh->selected_pixels_left[best_idx_3] = 2;
						best_val_4 = 1e10;
						n3++;
					}
				}

				if(best_idx_4 > 0)
				{
					fh->selected_pixels_left[best_idx_4] = 4;
					n4++;
				}
			}
		}
	}

	return Eigen::Vector3i(n2, n3, n4);
}
    Eigen::Vector3i selectRight(std::shared_ptr<frame> fh, int pot, float th_factor = 1) {
	Eigen::Vector3f const * const map_0 = fh->dI_right;

	float* mapmax_0 = fh->abs_squared_grad_right[0];
	float* mapmax_1 = fh->abs_squared_grad_right[1];
	float* mapmax_2 = fh->abs_squared_grad_right[2];

	int w = wG[0];
	int w1 = wG[1];
	int w2 = wG[2];
	int h = hG[0];

	const Eigen::Matrix<float, 2, 1> directions[16] = {
		Eigen::Matrix<float, 2, 1>(0, 1.0000),
		Eigen::Matrix<float, 2, 1>(0.3827, 0.9239),
		Eigen::Matrix<float, 2, 1>(0.1951, 0.9808),
		Eigen::Matrix<float, 2, 1>(0.9239, 0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, 0.7071),
		Eigen::Matrix<float, 2, 1>(0.3827, -0.9239),
		Eigen::Matrix<float, 2, 1>(0.8315, 0.5556),
		Eigen::Matrix<float, 2, 1>(0.8315, -0.5556),
		Eigen::Matrix<float, 2, 1>(0.5556, -0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, 0.1951),
		Eigen::Matrix<float, 2, 1>(0.9239, -0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, -0.7071),
		Eigen::Matrix<float, 2, 1>(0.5556, 0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, -0.1951),
		Eigen::Matrix<float, 2, 1>(1.0000, 0.0000),
		Eigen::Matrix<float, 2, 1>(0.1951, -0.9808)};

	memset(fh->selected_pixels_right, 0, w * h * sizeof(PixelSelectorStatus));

	float dw1 = setting_grad_down_weight_per_level;
	float dw2 = dw1 * dw1;

	int n3 = 0, n2 = 0, n4 = 0;

	for(int y4 = 0; y4 < h; y4 += (4 * pot))
	{
		for(int x4 = 0; x4 < w; x4 += (4 * pot))
		{
			int my3 = std::min((4 * pot), h - y4);
			int mx3 = std::min((4 * pot), w - x4);
			int best_idx_4 = -1;
			float best_val_4 = 0;

			Eigen::Matrix<float, 2, 1> dir4 = directions[random_pattern[n2] & 0xF];

			for(int y3 = 0; y3 < my3; y3 += (2 * pot))
			{
				for(int x3 = 0; x3 < mx3; x3 += (2 * pot))
				{
					int x34 = x3 + x4;
					int y34 = y3 + y4;
					int my2 = std::min((2 * pot), h - y34);
					int mx2 = std::min((2 * pot), w - x34);

					int best_idx_3 = -1;
					float best_val_3 = 0;

					Eigen::Matrix<float, 2, 1> dir3 = directions[random_pattern[n2] & 0xF];

					for(int y2 = 0; y2 < my2; y2 += pot)
					{
						for(int x2 = 0; x2 < mx2; x2 += pot)
						{
							int x234 = x2 + x34;
							int y234 = y2 + y34;
							int my1 = std::min(pot, h - y234);
							int mx1 = std::min(pot, w - x234);

							int best_idx_2 = -1;
							float best_val_2 = 0;

							Eigen::Matrix<float, 2, 1> dir2 = directions[random_pattern[n2] & 0xF];

							for(int y1 = 0; y1 < my1; y1 += 1)
							{
								for(int x1 = 0; x1 < mx1; x1 += 1)
								{
									assert(x1 + x234 < w);
									assert(y1 + y234 < h);

									int idx = x1 + x234 + w * (y1 + y234);
									int xf = x1 + x234;
									int yf = y1 + y234;

									if (xf < 4 || xf >= w - 5 || yf < 4 || yf > h - 4) continue;

									if (fh->mask_right[idx] == true) continue;

									float pixel_TH_0 = ths_smoothed[(xf >> 5) + (yf >> 5) * ths_step];
									float pixel_TH_1 = pixel_TH_0 * dw1;
									float pixel_TH_2 = pixel_TH_1 * dw2;

									float ag_0 = mapmax_0[idx];

									if(ag_0 > pixel_TH_0 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir2)));

										if(!setting_select_direction_distribution) dir_norm = ag_0;

										if(dir_norm > best_val_2)
										{
											best_val_2 = dir_norm;
											best_idx_2 = idx;
											best_idx_3 = -2;
											best_idx_4 = -2;
										}
									}

									if(best_idx_3 == -2) continue;

									float ag_1 = mapmax_1[(int)(xf * 0.5f + 0.25f) + (int)(yf * 0.5f + 0.25f) * w1];

									if(ag_1 > pixel_TH_1 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir3)));

										if(!setting_select_direction_distribution) dir_norm = ag_1;

										if(dir_norm > best_val_3)
										{
											best_val_3 = dir_norm;
											best_idx_3 = idx;
											best_idx_4 = -2;
										}
									}

									if(best_idx_4 == -2) continue;

									float ag_2 = mapmax_2[(int)(xf * 0.25f + 0.125) + (int)(yf * 0.25f + 0.125) * w2];

									if(ag_2 > pixel_TH_2 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir4)));

										if(!setting_select_direction_distribution) dir_norm = ag_2;

										if(dir_norm > best_val_4)
										{
											best_val_4 = dir_norm;
											best_idx_4 = idx;
										}
									}
								}
							}

							if(best_idx_2 > 0)
							{
								fh->selected_pixels_right[best_idx_2] = 1;
								best_val_3 = 1e10;
								n2++;
							}
						}
					}

					if(best_idx_3 > 0)
					{
						fh->selected_pixels_right[best_idx_3] = 2;
						best_val_4 = 1e10;
						n3++;
					}
				}

				if(best_idx_4 > 0)
				{
					fh->selected_pixels_right[best_idx_4] = 4;
					n4++;
				}
			}
		}
	}

	return Eigen::Vector3i(n2, n3, n4);
}
    int current_potential;
    bool allow_fast;
    unsigned char* random_pattern;
    int* grad_hist;
    float* ths;
    float* ths_smoothed;
    int ths_step;
    std::shared_ptr<frame> grad_hist_frame;
};

class initializerHelper {
  public:
    static imuData interpolateData(const imuData &imu_1, const imuData &imu_2, double timestamp) {
	double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);

	imuData data;
	data.timestamp = timestamp;
	data.acc = (1 - lambda) * imu_1.acc + lambda * imu_2.acc;
	data.gyr = (1 - lambda) * imu_1.gyr + lambda * imu_2.gyr;

	return data;
}
    static std::vector<imuData> selectImuReadings(const std::vector<imuData> &imu_data_tmp, double time_0, double time_1) {
	std::vector<imuData> prop_data;

	if (imu_data_tmp.empty()) return prop_data;

	for (size_t i = 0; i < imu_data_tmp.size() - 1; i++)
	{
		if (imu_data_tmp.at(i + 1).timestamp > time_0 && imu_data_tmp.at(i).timestamp < time_0)
		{
			imuData data = interpolateData(imu_data_tmp.at(i), imu_data_tmp.at(i + 1), time_0);
			prop_data.push_back(data);
			continue;
		}

		if (imu_data_tmp.at(i).timestamp >= time_0 && imu_data_tmp.at(i + 1).timestamp <= time_1)
		{
			prop_data.push_back(imu_data_tmp.at(i));
			continue;
		}

		if (imu_data_tmp.at(i + 1).timestamp > time_1)
		{
			if (imu_data_tmp.at(i).timestamp > time_1 && i == 0)
			{
				break;
			}
			else if (imu_data_tmp.at(i).timestamp > time_1)
			{
				imuData data = interpolateData(imu_data_tmp.at(i - 1), imu_data_tmp.at(i), time_1);
				prop_data.push_back(data);
			}
			else
			{
				prop_data.push_back(imu_data_tmp.at(i));
			}

			if (prop_data.at(prop_data.size() - 1).timestamp != time_1)
			{
				imuData data = interpolateData(imu_data_tmp.at(i), imu_data_tmp.at(i + 1), time_1);
				prop_data.push_back(data);
			}

			break;
		}
	}

	if (prop_data.empty()) return prop_data;

	for (size_t i = 0; i < prop_data.size() - 1; i++)
	{
		if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12)
		{
			prop_data.erase(prop_data.begin() + i);
			i--;
		}
	}

	return prop_data;
}
    static void gramSchmidt(const Eigen::Vector3d &gravity_inI, Eigen::Matrix3d &R_GtoI) {
	Eigen::Vector3d z_axis = gravity_inI / gravity_inI.norm();
	Eigen::Vector3d x_axis, y_axis;
	Eigen::Vector3d e_1(1.0, 0.0, 0.0);
	Eigen::Vector3d e_2(0.0, 1.0, 0.0);

	double inner1 = e_1.dot(z_axis) / z_axis.norm();
	double inner2 = e_2.dot(z_axis) / z_axis.norm();

	if (fabs(inner1) < fabs(inner2))
	{
		x_axis = z_axis.cross(e_1);
		x_axis = x_axis / x_axis.norm();
		y_axis = z_axis.cross(x_axis);
		y_axis = y_axis / y_axis.norm();
	}
	else
	{
		x_axis = z_axis.cross(e_2);
		x_axis = x_axis / x_axis.norm();
		y_axis = z_axis.cross(x_axis);
		y_axis = y_axis / y_axis.norm();
	}

	R_GtoI.block(0, 0, 3, 1) = x_axis;
	R_GtoI.block(0, 1, 3, 1) = y_axis;
	R_GtoI.block(0, 2, 3, 1) = z_axis;
}
    static Eigen::Matrix<double, 7, 1> computeDongsiCoeff(Eigen::MatrixXd &D, const Eigen::MatrixXd &d, double gravity_mag) {
	assert(D.rows() == 3);
	assert(D.cols() == 3);
	assert(d.rows() == 3);
	double D1_1 = D(0, 0), D1_2 = D(0, 1), D1_3 = D(0, 2);
	double D2_1 = D(1, 0), D2_2 = D(1, 1), D2_3 = D(1, 2);
	double D3_1 = D(2, 0), D3_2 = D(2, 1), D3_3 = D(2, 2);
	double d1 = d(0, 0), d2 = d(1, 0), d3 = d(2, 0);
	double g = gravity_mag;

	double D1_1_sq = D1_1 * D1_1, D1_2_sq = D1_2 * D1_2, D1_3_sq = D1_3 * D1_3;
	double D2_1_sq = D2_1 * D2_1, D2_2_sq = D2_2 * D2_2, D2_3_sq = D2_3 * D2_3;
	double D3_1_sq = D3_1 * D3_1, D3_2_sq = D3_2 * D3_2, D3_3_sq = D3_3 * D3_3;
	double d1_sq = d1 * d1, d2_sq = d2 * d2, d3_sq = d3 * d3;
	double g_sq = g * g;

	Eigen::Matrix<double, 7, 1> coeff = Eigen::Matrix<double, 7, 1>::Zero();

	coeff(6) = -(-D1_1_sq * D2_2_sq * D3_3_sq * g_sq + D1_1_sq * D2_2_sq * d3_sq + 2 * D1_1_sq * D2_2 * D2_3 * D3_2 * D3_3 * g_sq -
				D1_1_sq * D2_2 * D2_3 * d2 * d3 - D1_1_sq * D2_2 * D3_2 * d2 * d3 - D1_1_sq * D2_3_sq * D3_2_sq * g_sq +
				D1_1_sq * D2_3 * D3_2 * d2_sq + D1_1_sq * D2_3 * D3_2 * d3_sq - D1_1_sq * D2_3 * D3_3 * d2 * d3 -
				D1_1_sq * D3_2 * D3_3 * d2 * d3 + D1_1_sq * D3_3_sq * d2_sq + 2 * D1_1 * D1_2 * D2_1 * D2_2 * D3_3_sq * g_sq -
				2 * D1_1 * D1_2 * D2_1 * D2_2 * d3_sq - 2 * D1_1 * D1_2 * D2_1 * D2_3 * D3_2 * D3_3 * g_sq + D1_1 * D1_2 * D2_1 * D2_3 * d2 * d3 +
				D1_1 * D1_2 * D2_1 * D3_2 * d2 * d3 - 2 * D1_1 * D1_2 * D2_2 * D2_3 * D3_1 * D3_3 * g_sq + D1_1 * D1_2 * D2_2 * D2_3 * d1 * d3 +
				D1_1 * D1_2 * D2_2 * D3_1 * d2 * d3 + 2 * D1_1 * D1_2 * D2_3_sq * D3_1 * D3_2 * g_sq - D1_1 * D1_2 * D2_3 * D3_1 * d2_sq -
				D1_1 * D1_2 * D2_3 * D3_1 * d3_sq - D1_1 * D1_2 * D2_3 * D3_2 * d1 * d2 + D1_1 * D1_2 * D2_3 * D3_3 * d1 * d3 +
				D1_1 * D1_2 * D3_1 * D3_3 * d2 * d3 - D1_1 * D1_2 * D3_3_sq * d1 * d2 - 2 * D1_1 * D1_3 * D2_1 * D2_2 * D3_2 * D3_3 * g_sq +
				D1_1 * D1_3 * D2_1 * D2_2 * d2 * d3 + 2 * D1_1 * D1_3 * D2_1 * D2_3 * D3_2_sq * g_sq - D1_1 * D1_3 * D2_1 * D3_2 * d2_sq -
				D1_1 * D1_3 * D2_1 * D3_2 * d3_sq + D1_1 * D1_3 * D2_1 * D3_3 * d2 * d3 + 2 * D1_1 * D1_3 * D2_2_sq * D3_1 * D3_3 * g_sq -
				D1_1 * D1_3 * D2_2_sq * d1 * d3 - 2 * D1_1 * D1_3 * D2_2 * D2_3 * D3_1 * D3_2 * g_sq + D1_1 * D1_3 * D2_2 * D3_2 * d1 * d2 +
				D1_1 * D1_3 * D2_3 * D3_1 * d2 * d3 - D1_1 * D1_3 * D2_3 * D3_2 * d1 * d3 + D1_1 * D1_3 * D3_1 * D3_2 * d2 * d3 -
				2 * D1_1 * D1_3 * D3_1 * D3_3 * d2_sq + D1_1 * D1_3 * D3_2 * D3_3 * d1 * d2 + D1_1 * D2_1 * D2_2 * D3_2 * d1 * d3 -
				D1_1 * D2_1 * D2_3 * D3_2 * d1 * d2 + D1_1 * D2_1 * D3_2 * D3_3 * d1 * d3 - D1_1 * D2_1 * D3_3_sq * d1 * d2 -
				D1_1 * D2_2_sq * D3_1 * d1 * d3 + D1_1 * D2_2 * D2_3 * D3_1 * d1 * d2 - D1_1 * D2_3 * D3_1 * D3_2 * d1 * d3 +
				D1_1 * D2_3 * D3_1 * D3_3 * d1 * d2 - D1_2_sq * D2_1_sq * D3_3_sq * g_sq + D1_2_sq * D2_1_sq * d3_sq +
				2 * D1_2_sq * D2_1 * D2_3 * D3_1 * D3_3 * g_sq - D1_2_sq * D2_1 * D2_3 * d1 * d3 - D1_2_sq * D2_1 * D3_1 * d2 * d3 -
				D1_2_sq * D2_3_sq * D3_1_sq * g_sq + D1_2_sq * D2_3 * D3_1 * d1 * d2 + 2 * D1_2 * D1_3 * D2_1_sq * D3_2 * D3_3 * g_sq -
				D1_2 * D1_3 * D2_1_sq * d2 * d3 - 2 * D1_2 * D1_3 * D2_1 * D2_2 * D3_1 * D3_3 * g_sq + D1_2 * D1_3 * D2_1 * D2_2 * d1 * d3 -
				2 * D1_2 * D1_3 * D2_1 * D2_3 * D3_1 * D3_2 * g_sq + D1_2 * D1_3 * D2_1 * D3_1 * d2_sq + D1_2 * D1_3 * D2_1 * D3_1 * d3_sq -
				D1_2 * D1_3 * D2_1 * D3_3 * d1 * d3 + 2 * D1_2 * D1_3 * D2_2 * D2_3 * D3_1_sq * g_sq - D1_2 * D1_3 * D2_2 * D3_1 * d1 * d2 -
				D1_2 * D1_3 * D3_1_sq * d2 * d3 + D1_2 * D1_3 * D3_1 * D3_3 * d1 * d2 - D1_2 * D2_1_sq * D3_2 * d1 * d3 +
				D1_2 * D2_1 * D2_2 * D3_1 * d1 * d3 + D1_2 * D2_1 * D2_3 * D3_2 * d1_sq + D1_2 * D2_1 * D2_3 * D3_2 * d3_sq -
				D1_2 * D2_1 * D2_3 * D3_3 * d2 * d3 - D1_2 * D2_1 * D3_1 * D3_3 * d1 * d3 - D1_2 * D2_1 * D3_2 * D3_3 * d2 * d3 +
				D1_2 * D2_1 * D3_3_sq * d1_sq + D1_2 * D2_1 * D3_3_sq * d2_sq - D1_2 * D2_2 * D2_3 * D3_1 * d1_sq -
				D1_2 * D2_2 * D2_3 * D3_1 * d3_sq + D1_2 * D2_2 * D2_3 * D3_3 * d1 * d3 + D1_2 * D2_2 * D3_1 * D3_3 * d2 * d3 -
				D1_2 * D2_2 * D3_3_sq * d1 * d2 + D1_2 * D2_3_sq * D3_1 * d2 * d3 - D1_2 * D2_3_sq * D3_2 * d1 * d3 +
				D1_2 * D2_3 * D3_1_sq * d1 * d3 - D1_2 * D2_3 * D3_1 * D3_3 * d1_sq - D1_2 * D2_3 * D3_1 * D3_3 * d2_sq +
				D1_2 * D2_3 * D3_2 * D3_3 * d1 * d2 - D1_3_sq * D2_1_sq * D3_2_sq * g_sq + 2 * D1_3_sq * D2_1 * D2_2 * D3_1 * D3_2 * g_sq -
				D1_3_sq * D2_1 * D3_1 * d2 * d3 + D1_3_sq * D2_1 * D3_2 * d1 * d3 - D1_3_sq * D2_2_sq * D3_1_sq * g_sq +
				D1_3_sq * D3_1_sq * d2_sq - D1_3_sq * D3_1 * D3_2 * d1 * d2 + D1_3 * D2_1_sq * D3_2 * d1 * d2 -
				D1_3 * D2_1 * D2_2 * D3_1 * d1 * d2 - D1_3 * D2_1 * D2_2 * D3_2 * d1_sq - D1_3 * D2_1 * D2_2 * D3_2 * d3_sq +
				D1_3 * D2_1 * D2_2 * D3_3 * d2 * d3 + D1_3 * D2_1 * D3_1 * D3_3 * d1 * d2 + D1_3 * D2_1 * D3_2_sq * d2 * d3 -
				D1_3 * D2_1 * D3_2 * D3_3 * d1_sq - D1_3 * D2_1 * D3_2 * D3_3 * d2_sq + D1_3 * D2_2_sq * D3_1 * d1_sq +
				D1_3 * D2_2_sq * D3_1 * d3_sq - D1_3 * D2_2_sq * D3_3 * d1 * d3 - D1_3 * D2_2 * D2_3 * D3_1 * d2 * d3 +
				D1_3 * D2_2 * D2_3 * D3_2 * d1 * d3 - D1_3 * D2_2 * D3_1 * D3_2 * d2 * d3 + D1_3 * D2_2 * D3_2 * D3_3 * d1 * d2 -
				D1_3 * D2_3 * D3_1_sq * d1 * d2 + D1_3 * D2_3 * D3_1 * D3_2 * d1_sq + D1_3 * D2_3 * D3_1 * D3_2 * d2_sq -
				D1_3 * D2_3 * D3_2_sq * d1 * d2 + D2_1 * D2_2 * D3_2 * D3_3 * d1 * d3 - D2_1 * D2_2 * D3_3_sq * d1 * d2 -
				D2_1 * D2_3 * D3_2_sq * d1 * d3 + D2_1 * D2_3 * D3_2 * D3_3 * d1 * d2 - D2_2_sq * D3_1 * D3_3 * d1 * d3 +
				D2_2_sq * D3_3_sq * d1_sq + D2_2 * D2_3 * D3_1 * D3_2 * d1 * d3 + D2_2 * D2_3 * D3_1 * D3_3 * d1 * d2 -
				2 * D2_2 * D2_3 * D3_2 * D3_3 * d1_sq - D2_3_sq * D3_1 * D3_2 * d1 * d2 + D2_3_sq * D3_2_sq * d1_sq) /
				g_sq;

	coeff(5) = (-(2 * D1_1_sq * D2_2_sq * D3_3 * g_sq - 2 * D1_1_sq * D2_2 * D2_3 * D3_2 * g_sq + 2 * D1_1_sq * D2_2 * D3_3_sq * g_sq -
				2 * D1_1_sq * D2_2 * d3_sq - 2 * D1_1_sq * D2_3 * D3_2 * D3_3 * g_sq + 2 * D1_1_sq * D2_3 * d2 * d3 +
				2 * D1_1_sq * D3_2 * d2 * d3 - 2 * D1_1_sq * D3_3 * d2_sq - 4 * D1_1 * D1_2 * D2_1 * D2_2 * D3_3 * g_sq +
				2 * D1_1 * D1_2 * D2_1 * D2_3 * D3_2 * g_sq - 2 * D1_1 * D1_2 * D2_1 * D3_3_sq * g_sq + 2 * D1_1 * D1_2 * D2_1 * d3_sq +
				2 * D1_1 * D1_2 * D2_2 * D2_3 * D3_1 * g_sq + 2 * D1_1 * D1_2 * D2_3 * D3_1 * D3_3 * g_sq - 2 * D1_1 * D1_2 * D2_3 * d1 * d3 -
				2 * D1_1 * D1_2 * D3_1 * d2 * d3 + 2 * D1_1 * D1_2 * D3_3 * d1 * d2 + 2 * D1_1 * D1_3 * D2_1 * D2_2 * D3_2 * g_sq +
				2 * D1_1 * D1_3 * D2_1 * D3_2 * D3_3 * g_sq - 2 * D1_1 * D1_3 * D2_1 * d2 * d3 - 2 * D1_1 * D1_3 * D2_2_sq * D3_1 * g_sq -
				4 * D1_1 * D1_3 * D2_2 * D3_1 * D3_3 * g_sq + 2 * D1_1 * D1_3 * D2_2 * d1 * d3 + 2 * D1_1 * D1_3 * D2_3 * D3_1 * D3_2 * g_sq +
				2 * D1_1 * D1_3 * D3_1 * d2_sq - 2 * D1_1 * D1_3 * D3_2 * d1 * d2 - 2 * D1_1 * D2_1 * D3_2 * d1 * d3 +
				2 * D1_1 * D2_1 * D3_3 * d1 * d2 + 2 * D1_1 * D2_2_sq * D3_3_sq * g_sq - 2 * D1_1 * D2_2_sq * d3_sq -
				4 * D1_1 * D2_2 * D2_3 * D3_2 * D3_3 * g_sq + 2 * D1_1 * D2_2 * D2_3 * d2 * d3 + 2 * D1_1 * D2_2 * D3_1 * d1 * d3 +
				2 * D1_1 * D2_2 * D3_2 * d2 * d3 + 2 * D1_1 * D2_3_sq * D3_2_sq * g_sq - 2 * D1_1 * D2_3 * D3_1 * d1 * d2 -
				2 * D1_1 * D2_3 * D3_2 * d2_sq - 2 * D1_1 * D2_3 * D3_2 * d3_sq + 2 * D1_1 * D2_3 * D3_3 * d2 * d3 +
				2 * D1_1 * D3_2 * D3_3 * d2 * d3 - 2 * D1_1 * D3_3_sq * d2_sq + 2 * D1_2_sq * D2_1_sq * D3_3 * g_sq -
				2 * D1_2_sq * D2_1 * D2_3 * D3_1 * g_sq - 2 * D1_2 * D1_3 * D2_1_sq * D3_2 * g_sq + 2 * D1_2 * D1_3 * D2_1 * D2_2 * D3_1 * g_sq +
				2 * D1_2 * D1_3 * D2_1 * D3_1 * D3_3 * g_sq - 2 * D1_2 * D1_3 * D2_3 * D3_1_sq * g_sq - 2 * D1_2 * D2_1 * D2_2 * D3_3_sq * g_sq +
				2 * D1_2 * D2_1 * D2_2 * d3_sq + 2 * D1_2 * D2_1 * D2_3 * D3_2 * D3_3 * g_sq - 2 * D1_2 * D2_1 * D3_3 * d1_sq -
				2 * D1_2 * D2_1 * D3_3 * d2_sq + 2 * D1_2 * D2_2 * D2_3 * D3_1 * D3_3 * g_sq - 2 * D1_2 * D2_2 * D2_3 * d1 * d3 -
				2 * D1_2 * D2_2 * D3_1 * d2 * d3 + 2 * D1_2 * D2_2 * D3_3 * d1 * d2 - 2 * D1_2 * D2_3_sq * D3_1 * D3_2 * g_sq +
				2 * D1_2 * D2_3 * D3_1 * d1_sq + 2 * D1_2 * D2_3 * D3_1 * d2_sq + 2 * D1_2 * D2_3 * D3_1 * d3_sq -
				2 * D1_2 * D2_3 * D3_3 * d1 * d3 - 2 * D1_2 * D3_1 * D3_3 * d2 * d3 + 2 * D1_2 * D3_3_sq * d1 * d2 -
				2 * D1_3_sq * D2_1 * D3_1 * D3_2 * g_sq + 2 * D1_3_sq * D2_2 * D3_1_sq * g_sq + 2 * D1_3 * D2_1 * D2_2 * D3_2 * D3_3 * g_sq -
				2 * D1_3 * D2_1 * D2_2 * d2 * d3 - 2 * D1_3 * D2_1 * D2_3 * D3_2_sq * g_sq + 2 * D1_3 * D2_1 * D3_2 * d1_sq +
				2 * D1_3 * D2_1 * D3_2 * d2_sq + 2 * D1_3 * D2_1 * D3_2 * d3_sq - 2 * D1_3 * D2_1 * D3_3 * d2 * d3 -
				2 * D1_3 * D2_2_sq * D3_1 * D3_3 * g_sq + 2 * D1_3 * D2_2_sq * d1 * d3 + 2 * D1_3 * D2_2 * D2_3 * D3_1 * D3_2 * g_sq -
				2 * D1_3 * D2_2 * D3_1 * d1_sq - 2 * D1_3 * D2_2 * D3_1 * d3_sq - 2 * D1_3 * D2_2 * D3_2 * d1 * d2 +
				2 * D1_3 * D2_2 * D3_3 * d1 * d3 + 2 * D1_3 * D3_1 * D3_3 * d2_sq - 2 * D1_3 * D3_2 * D3_3 * d1 * d2 -
				2 * D2_1 * D2_2 * D3_2 * d1 * d3 + 2 * D2_1 * D2_2 * D3_3 * d1 * d2 - 2 * D2_1 * D3_2 * D3_3 * d1 * d3 +
				2 * D2_1 * D3_3_sq * d1 * d2 + 2 * D2_2_sq * D3_1 * d1 * d3 - 2 * D2_2_sq * D3_3 * d1_sq - 2 * D2_2 * D2_3 * D3_1 * d1 * d2 +
				2 * D2_2 * D2_3 * D3_2 * d1_sq + 2 * D2_2 * D3_1 * D3_3 * d1 * d3 - 2 * D2_2 * D3_3_sq * d1_sq -
				2 * D2_3 * D3_1 * D3_3 * d1 * d2 + 2 * D2_3 * D3_2 * D3_3 * d1_sq) /
				g_sq);

	coeff(4) = ((D1_1_sq * D2_2_sq * g_sq + 4 * D1_1_sq * D2_2 * D3_3 * g_sq - 2 * D1_1_sq * D2_3 * D3_2 * g_sq + D1_1_sq * D3_3_sq * g_sq -
				D1_1_sq * d2_sq - D1_1_sq * d3_sq - 2 * D1_1 * D1_2 * D2_1 * D2_2 * g_sq - 4 * D1_1 * D1_2 * D2_1 * D3_3 * g_sq +
				2 * D1_1 * D1_2 * D2_3 * D3_1 * g_sq + D1_1 * D1_2 * d1 * d2 + 2 * D1_1 * D1_3 * D2_1 * D3_2 * g_sq -
				4 * D1_1 * D1_3 * D2_2 * D3_1 * g_sq - 2 * D1_1 * D1_3 * D3_1 * D3_3 * g_sq + D1_1 * D1_3 * d1 * d3 + D1_1 * D2_1 * d1 * d2 +
				4 * D1_1 * D2_2_sq * D3_3 * g_sq - 4 * D1_1 * D2_2 * D2_3 * D3_2 * g_sq + 4 * D1_1 * D2_2 * D3_3_sq * g_sq -
				4 * D1_1 * D2_2 * d3_sq - 4 * D1_1 * D2_3 * D3_2 * D3_3 * g_sq + 4 * D1_1 * D2_3 * d2 * d3 + D1_1 * D3_1 * d1 * d3 +
				4 * D1_1 * D3_2 * d2 * d3 - 4 * D1_1 * D3_3 * d2_sq + D1_2_sq * D2_1_sq * g_sq + 2 * D1_2 * D1_3 * D2_1 * D3_1 * g_sq -
				4 * D1_2 * D2_1 * D2_2 * D3_3 * g_sq + 2 * D1_2 * D2_1 * D2_3 * D3_2 * g_sq - 2 * D1_2 * D2_1 * D3_3_sq * g_sq -
				D1_2 * D2_1 * d1_sq - D1_2 * D2_1 * d2_sq + 2 * D1_2 * D2_1 * d3_sq + 2 * D1_2 * D2_2 * D2_3 * D3_1 * g_sq +
				D1_2 * D2_2 * d1 * d2 + 2 * D1_2 * D2_3 * D3_1 * D3_3 * g_sq - 3 * D1_2 * D2_3 * d1 * d3 - 3 * D1_2 * D3_1 * d2 * d3 +
				4 * D1_2 * D3_3 * d1 * d2 + D1_3_sq * D3_1_sq * g_sq + 2 * D1_3 * D2_1 * D2_2 * D3_2 * g_sq +
				2 * D1_3 * D2_1 * D3_2 * D3_3 * g_sq - 3 * D1_3 * D2_1 * d2 * d3 - 2 * D1_3 * D2_2_sq * D3_1 * g_sq -
				4 * D1_3 * D2_2 * D3_1 * D3_3 * g_sq + 4 * D1_3 * D2_2 * d1 * d3 + 2 * D1_3 * D2_3 * D3_1 * D3_2 * g_sq - D1_3 * D3_1 * d1_sq +
				2 * D1_3 * D3_1 * d2_sq - D1_3 * D3_1 * d3_sq - 3 * D1_3 * D3_2 * d1 * d2 + D1_3 * D3_3 * d1 * d3 + D2_1 * D2_2 * d1 * d2 -
				3 * D2_1 * D3_2 * d1 * d3 + 4 * D2_1 * D3_3 * d1 * d2 + D2_2_sq * D3_3_sq * g_sq - D2_2_sq * d1_sq - D2_2_sq * d3_sq -
				2 * D2_2 * D2_3 * D3_2 * D3_3 * g_sq + D2_2 * D2_3 * d2 * d3 + 4 * D2_2 * D3_1 * d1 * d3 + D2_2 * D3_2 * d2 * d3 -
				4 * D2_2 * D3_3 * d1_sq + D2_3_sq * D3_2_sq * g_sq - 3 * D2_3 * D3_1 * d1 * d2 + 2 * D2_3 * D3_2 * d1_sq - D2_3 * D3_2 * d2_sq -
				D2_3 * D3_2 * d3_sq + D2_3 * D3_3 * d2 * d3 + D3_1 * D3_3 * d1 * d3 + D3_2 * D3_3 * d2 * d3 - D3_3_sq * d1_sq - D3_3_sq * d2_sq) /
				g_sq);

	coeff(3) = ((2 * D1_1 * d2_sq + 2 * D1_1 * d3_sq + 2 * D2_2 * d1_sq + 2 * D2_2 * d3_sq + 2 * D3_3 * d1_sq + 2 * D3_3 * d2_sq -
				2 * D1_1 * D2_2_sq * g_sq - 2 * D1_1_sq * D2_2 * g_sq - 2 * D1_1 * D3_3_sq * g_sq - 2 * D1_1_sq * D3_3 * g_sq -
				2 * D2_2 * D3_3_sq * g_sq - 2 * D2_2_sq * D3_3 * g_sq - 2 * D1_2 * d1 * d2 - 2 * D1_3 * d1 * d3 - 2 * D2_1 * d1 * d2 -
				2 * D2_3 * d2 * d3 - 2 * D3_1 * d1 * d3 - 2 * D3_2 * d2 * d3 + 2 * D1_1 * D1_2 * D2_1 * g_sq + 2 * D1_1 * D1_3 * D3_1 * g_sq +
				2 * D1_2 * D2_1 * D2_2 * g_sq - 8 * D1_1 * D2_2 * D3_3 * g_sq + 4 * D1_1 * D2_3 * D3_2 * g_sq + 4 * D1_2 * D2_1 * D3_3 * g_sq -
				2 * D1_2 * D2_3 * D3_1 * g_sq - 2 * D1_3 * D2_1 * D3_2 * g_sq + 4 * D1_3 * D2_2 * D3_1 * g_sq + 2 * D1_3 * D3_1 * D3_3 * g_sq +
				2 * D2_2 * D2_3 * D3_2 * g_sq + 2 * D2_3 * D3_2 * D3_3 * g_sq) /
				g_sq);

	coeff(2) = (-(d1_sq + d2_sq + d3_sq - D1_1_sq * g_sq - D2_2_sq * g_sq - D3_3_sq * g_sq - 4 * D1_1 * D2_2 * g_sq + 2 * D1_2 * D2_1 * g_sq -
				4 * D1_1 * D3_3 * g_sq + 2 * D1_3 * D3_1 * g_sq - 4 * D2_2 * D3_3 * g_sq + 2 * D2_3 * D3_2 * g_sq) /
				g_sq);

	coeff(1) = (-(2 * D1_1 * g_sq + 2 * D2_2 * g_sq + 2 * D3_3 * g_sq) / g_sq);

	coeff(0) = 1;

	return coeff;
}
};

class mapManagement {
  public:
    static bool addPointToVoxel(voxelHashMap &voxel_map, std::shared_ptr<mapPoint> map_point_ptr, double voxel_size, int max_num_points_in_voxel,
      double min_distance_points, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history) {
	Eigen::Vector3d point = map_point_ptr->getPointXYZ(false);

	short kx = static_cast<short>(point[0] / voxel_size);
	short ky = static_cast<short>(point[1] / voxel_size);
	short kz = static_cast<short>(point[2] / voxel_size);

	map_point_ptr->voxel_idx[0] = kx;
	map_point_ptr->voxel_idx[1] = ky;
	map_point_ptr->voxel_idx[2] = kz;

	voxelHashMap::iterator search = voxel_map.find(voxel(kx, ky, kz));

	if(search != voxel_map.end())
	{
		auto &voxel_block = (search.value());

		if(!voxel_block.IsFull())
		{
			double sq_dist_min_to_points = 10 * voxel_size * voxel_size;
			for (int i(0); i < voxel_block.NumPoints(); ++i)
			{
				auto &map_point_ptr_ = voxel_block.points[i];
				double sq_dist = (map_point_ptr_->getPointXYZ(false) - point).squaredNorm();

				if (sq_dist < sq_dist_min_to_points)
				{
					sq_dist_min_to_points = sq_dist;
				}
			}
			if(sq_dist_min_to_points > (min_distance_points * min_distance_points))
			{
				voxel_block.AddPoint(map_point_ptr);
				return true;
			}
			else
			{
				return false;
			}
		}
		else
		{
			return false;
		}
	}
	else
	{
		voxelBlock block(max_num_points_in_voxel);
		block.AddPoint(map_point_ptr);
		voxel_map[voxel(kx, ky, kz)] = std::move(block);

		// display
        pcl::PointXYZI point_temp;

        point_temp.x = kx >= 0 ? (kx + 0.5) * voxel_size : (kx - 0.5) * voxel_size;
        point_temp.y = ky >= 0 ? (ky + 0.5) * voxel_size : (ky - 0.5) * voxel_size;
        point_temp.z = kz >= 0 ? (ky + 0.5) * voxel_size : (kz - 0.5) * voxel_size;
        point_temp.intensity = 1;

        voxels_history->points.push_back(point_temp);
        // display

		return true;
    }
}
    static void changeHostVoxel(voxelHashMap &voxel_map, std::shared_ptr<mapPoint> map_point_ptr, double voxel_size, int max_num_points_in_voxel,
      double min_distance_points, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history) {
	Eigen::Vector3d point = map_point_ptr->getPointXYZ(false);

	short kx = static_cast<short>(point[0] / voxel_size);
	short ky = static_cast<short>(point[1] / voxel_size);
	short kz = static_cast<short>(point[2] / voxel_size);

	if (kx == map_point_ptr->voxel_idx[0] && ky == map_point_ptr->voxel_idx[1] && kz == map_point_ptr->voxel_idx[2]) return;

	voxelHashMap::iterator search_old = voxel_map.find(voxel(map_point_ptr->voxel_idx[0], map_point_ptr->voxel_idx[1], map_point_ptr->voxel_idx[2]));

	auto &voxel_block_old = (search_old.value());
	auto it_old = voxel_block_old.points.begin();

	bool has_found = false;

	while (it_old != voxel_block_old.points.end())
	{
		if ((*it_old) == map_point_ptr)
		{
			has_found = true;
			it_old = voxel_block_old.points.erase(it_old);
			break;
		}

		it_old++;
	}

	assert(has_found);

	map_point_ptr->voxel_idx[0] = kx;
	map_point_ptr->voxel_idx[1] = ky;
	map_point_ptr->voxel_idx[2] = kz;

	voxelHashMap::iterator search_new = voxel_map.find(voxel(kx, ky, kz));

	if(search_new != voxel_map.end())
	{
		auto &voxel_block_new = (search_new.value());
		voxel_block_new.AddPoint(map_point_ptr);
	}
	else
	{
		voxelBlock block_temp(max_num_points_in_voxel);
		block_temp.AddPoint(map_point_ptr);
		voxel_map[voxel(kx, ky, kz)] = std::move(block_temp);

		// display
        pcl::PointXYZI point_temp;

        point_temp.x = kx >= 0 ? (kx + 0.5) * voxel_size : (kx - 0.5) * voxel_size;
        point_temp.y = ky >= 0 ? (ky + 0.5) * voxel_size : (ky - 0.5) * voxel_size;
        point_temp.z = kz >= 0 ? (kz + 0.5) * voxel_size : (kz - 0.5) * voxel_size;
        point_temp.intensity = 1;

        voxels_history->points.push_back(point_temp);
        // display
	}
}
    static void deleteFromVoxel(voxelHashMap &voxel_map, std::shared_ptr<mapPoint> map_point_ptr) {
	voxelHashMap::iterator search = voxel_map.find(voxel(map_point_ptr->voxel_idx[0], map_point_ptr->voxel_idx[1], map_point_ptr->voxel_idx[2]));

	auto &voxel_block = (search.value());
	auto it = voxel_block.points.begin();

	bool has_found = false;

	while (it != voxel_block.points.end())
	{
		if (*it == map_point_ptr)
		{
			has_found = true;
			it = voxel_block.points.erase(it);
			break;
		}

		it++;
	}

	assert(has_found);
}
  private:
    mapManagement();
};

class feature {
  public:
    Eigen::Vector3f* dI_left;
    Eigen::Vector3f* dI_right;
    Eigen::Vector3f* dIp_left[PYR_LEVELS];
    Eigen::Vector3f* dIp_right[PYR_LEVELS];
    float* abs_squared_grad_left[PYR_LEVELS];
    float* abs_squared_grad_right[PYR_LEVELS];
    bool* mask_left;
    bool* mask_right;
    float* selected_pixels_left;
    float* selected_pixels_right;
    int frame_id;
    double timestamp;
    bool is_invalid = false;
    std::unordered_map<size_t, std::unordered_map<size_t, std::shared_ptr<feature>>> v_feat_ptr;
    void release() {
	delete[] mask_left;
	delete[] mask_right;

	delete[] selected_pixels_left;
	delete[] selected_pixels_right;

	for(int i = 0; i < pyr_levels_used; i++)
	{
		delete[] dIp_left[i];
		delete[] dIp_right[i];

		delete[] abs_squared_grad_left[i];
		delete[] abs_squared_grad_right[i];
	}

	is_invalid = true;
}
    void makeImageLeft(unsigned char* color, unsigned char* mask, std::shared_ptr<gammaPixel> gammaPixel_ptr, double timestamp_) {
	timestamp = timestamp_;

	for(int i = 0; i < pyr_levels_used; i++)
	{
		dIp_left[i] = new Eigen::Vector3f[wG[i] * hG[i]];
		abs_squared_grad_left[i] = new float[wG[i] * hG[i]];

		if (i == 0) mask_left = new bool[wG[i] * hG[i]];
	}

	dI_left = dIp_left[0];

	int w = wG[0];
	int h = hG[0];

	for(int i = 0; i < w * h; i++)
	{
		dI_left[i][0] = (float)color[i];
		mask_left[i] = mask[i] > 0 ? true : false;
	}

	for(int lvl = 0; lvl < pyr_levels_used; lvl++)
	{
		int wl = wG[lvl], hl = hG[lvl];
		Eigen::Vector3f* dI_l = dIp_left[lvl];
		float* dabs_l = abs_squared_grad_left[lvl];

		if (lvl > 0)
		{
			int lvlm1 = lvl - 1;
			int wlm1 = wG[lvlm1];

			Eigen::Vector3f* dI_lm = dIp_left[lvlm1];

			for(int y = 0; y < hl; y++)
			{
				for(int x = 0; x < wl; x++)
				{
					dI_l[x + y * wl][0] = 0.25f * (dI_lm[2 * x + 2 * y *wlm1][0] + dI_lm[2 * x + 1 + 2 * y * wlm1][0]
						+ dI_lm[2 * x + 2 * y * wlm1 + wlm1][0] + dI_lm[2 * x + 1 + 2 * y * wlm1 + wlm1][0]);
				}
			}
		}

		for(int idx = wl; idx < wl * (hl - 1); idx++)
		{
			float dx = 0.5f * (dI_l[idx + 1][0] - dI_l[idx - 1][0]);
			float dy = 0.5f * (dI_l[idx + wl][0] - dI_l[idx - wl][0]);

			if(!std::isfinite(dx)) dx = 0;
			if(!std::isfinite(dy)) dy = 0;

			dI_l[idx][1] = dx;
			dI_l[idx][2] = dy;

			dabs_l[idx] = dx * dx + dy * dy;

			if(setting_gamma_pixel_selection == 1 && gammaPixel_ptr != nullptr)
			{
				float gw = gammaPixel_ptr->getBGradOnly((float)(dI_l[idx][0]));
				dabs_l[idx] *= gw * gw;
			}
		}
	}

	selected_pixels_left = new float[wG[0] * hG[0]];
}
    void makeImageRight(unsigned char* color, unsigned char* mask, std::shared_ptr<gammaPixel> gammaPixel_ptr, double timestamp_) {
	timestamp = timestamp_;

	for(int i = 0; i < pyr_levels_used; i++)
	{
		dIp_right[i] = new Eigen::Vector3f[wG[i] * hG[i]];
		abs_squared_grad_right[i] = new float[wG[i] * hG[i]];

		if (i == 0) mask_right = new bool[wG[i] * hG[i]];
	}

	dI_right = dIp_right[0];

	int w = wG[0];
	int h = hG[0];

	for(int i = 0; i < w * h; i++)
	{
		dI_right[i][0] = (float)color[i];
		mask_right[i] = mask[i] > 0 ? true : false;
	}

	for(int lvl = 0; lvl < pyr_levels_used; lvl++)
	{
		int wl = wG[lvl], hl = hG[lvl];
		Eigen::Vector3f* dI_l = dIp_right[lvl];
		float* dabs_l = abs_squared_grad_right[lvl];

		if (lvl > 0)
		{
			int lvlm1 = lvl - 1;
			int wlm1 = wG[lvlm1];

			Eigen::Vector3f* dI_lm = dIp_right[lvlm1];

			for(int y = 0; y < hl; y++)
			{
				for(int x = 0; x < wl; x++)
				{
					dI_l[x + y * wl][0] = 0.25f * (dI_lm[2 * x + 2 * y *wlm1][0] + dI_lm[2 * x + 1 + 2 * y * wlm1][0]
						+ dI_lm[2 * x + 2 * y * wlm1 + wlm1][0] + dI_lm[2 * x + 1 + 2 * y * wlm1 + wlm1][0]);
				}
			}
		}

		for(int idx = wl; idx < wl * (hl - 1); idx++)
		{
			float dx = 0.5f * (dI_l[idx + 1][0] - dI_l[idx - 1][0]);
			float dy = 0.5f * (dI_l[idx + wl][0] - dI_l[idx - wl][0]);

			if(!std::isfinite(dx)) dx = 0;
			if(!std::isfinite(dy)) dy = 0;

			dI_l[idx][1] = dx;
			dI_l[idx][2] = dy;

			dabs_l[idx] = dx * dx + dy * dy;

			if(setting_gamma_pixel_selection == 1 && gammaPixel_ptr != nullptr)
			{
				float gw = gammaPixel_ptr->getBGradOnly((float)(dI_l[idx][0]));
				dabs_l[idx] *= gw * gw;
			}
		}
	}

	selected_pixels_right = new float[wG[0] * hG[0]];
}
};

class featureHelper {
  public:
    static void computeDisparity(std::shared_ptr<featureDatabase> db, double time_0, double time_1, double &disp_mean, double &disp_var, int &total_feats) {
	std::vector<double> disparities;
	for (auto &feat : db->getInternalData())
	{
		for (auto &cam_pairs : feat.second->timestamps)
		{
			if (cam_pairs.second.size() < 2)
				continue;

			size_t cam_id = cam_pairs.first;
			bool found_0 = false;
			bool found_1 = false;

			Eigen::Vector2f uv_0 = Eigen::Vector2f::Zero();
			Eigen::Vector2f uv_1 = Eigen::Vector2f::Zero();

			for (size_t idx = 0; idx < feat.second->timestamps.at(cam_id).size(); idx++)
			{
				double time = feat.second->timestamps.at(cam_id).at(idx);
				if ((oldest_time == -1 || time > oldest_time) && !found_0)
				{
					uv_0 = feat.second->uvs.at(cam_id).at(idx).block(0, 0, 2, 1);
					found_0 = true;
					continue;
				}
				if ((newest_time == -1 || time < newest_time) && found_0)
				{
					uv_1 = feat.second->uvs.at(cam_id).at(idx).block(0, 0, 2, 1);
					found_1 = true;
					continue;
				}
			}

			if (!found_0 || !found_1)
				continue;

			disparities.push_back((uv_1 - uv_0).norm());
		}
	}

	if (disparities.size() < 2)
	{
		disp_mean = -1;
		disp_var = -1;
		total_feats = 0;
	}

	disp_mean = 0;
	for (double disp_i : disparities)
	{
		disp_mean += disp_i;
	}

	disp_mean /= (double)disparities.size();
	disp_var = 0;
	for (double &disp_i : disparities)
	{
		disp_var += std::pow(disp_i - disp_mean, 2);
	}

	disp_var = std::sqrt(disp_var / (double)(disparities.size() - 1));
	total_feats = (int)disparities.size();
}
    static void computeDisparity(std::shared_ptr<featureDatabase> db, double &disp_mean, double &disp_var, int &total_feats,
    double newest_time = -1, double oldest_time = -1) {
	std::vector<double> disparities;
	for (auto &feat : db->getInternalData())
	{
		for (auto &cam_pairs : feat.second->timestamps)
		{
			if (cam_pairs.second.size() < 2)
				continue;

			size_t cam_id = cam_pairs.first;
			bool found_0 = false;
			bool found_1 = false;

			Eigen::Vector2f uv_0 = Eigen::Vector2f::Zero();
			Eigen::Vector2f uv_1 = Eigen::Vector2f::Zero();

			for (size_t idx = 0; idx < feat.second->timestamps.at(cam_id).size(); idx++)
			{
				double time = feat.second->timestamps.at(cam_id).at(idx);
				if ((oldest_time == -1 || time > oldest_time) && !found_0)
				{
					uv_0 = feat.second->uvs.at(cam_id).at(idx).block(0, 0, 2, 1);
					found_0 = true;
					continue;
				}
				if ((newest_time == -1 || time < newest_time) && found_0)
				{
					uv_1 = feat.second->uvs.at(cam_id).at(idx).block(0, 0, 2, 1);
					found_1 = true;
					continue;
				}
			}

			if (!found_0 || !found_1)
				continue;

			disparities.push_back((uv_1 - uv_0).norm());
		}
	}

	if (disparities.size() < 2)
	{
		disp_mean = -1;
		disp_var = -1;
		total_feats = 0;
	}

	disp_mean = 0;
	for (double disp_i : disparities)
	{
		disp_mean += disp_i;
	}

	disp_mean /= (double)disparities.size();
	disp_var = 0;
	for (double &disp_i : disparities)
	{
		disp_var += std::pow(disp_i - disp_mean, 2);
	}

	disp_var = std::sqrt(disp_var / (double)(disparities.size() - 1));
	total_feats = (int)disparities.size();
}
  private:
    featureHelper() {

}
};

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_GROWTH_POLICY_H
#define TSL_ROBIN_GROWTH_POLICY_H

#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iterator>
#include <limits>
#include <ratio>
#include <stdexcept>

#ifdef TSL_DEBUG
#    define tsl_rh_assert(expr) assert(expr)
#else
#    define tsl_rh_assert(expr) (static_cast<void>(0))
#endif

/**
 * If exceptions are enabled, throw the exception passed in parameter, otherwise call std::terminate.
 */
#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || (defined (_MSC_VER) && defined (_CPPUNWIND))) && !defined(TSL_NO_EXCEPTIONS)
#    define TSL_RH_THROW_OR_TERMINATE(ex, msg) throw ex(msg)
#else
#    define TSL_RH_NO_EXCEPTIONS
#    ifdef NDEBUG
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) std::terminate()
#    else
#        include <iostream>
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) do { std::cerr << msg << std::endl; std::terminate(); } while(0)
#    endif
#endif

#if defined(__GNUC__) || defined(__clang__)
#    define TSL_RH_LIKELY(exp) (__builtin_expect(!!(exp), true))
#else
#    define TSL_RH_LIKELY(exp) (exp)
#endif

namespace tsl {
namespace rh {

/**
 * Grow the hash table by a factor of GrowthFactor keeping the bucket count to a power of two. It allows
 * the table to use a mask operation instead of a modulo operation to map a hash to a bucket.
 *
 * GrowthFactor must be a power of two >= 2.
 */
template<std::size_t GrowthFactor>
class power_of_two_growth_policy {
public:
    /**
     * Called on the hash table creation and on rehash. The number of buckets for the table is passed in parameter.
     * This number is a minimum, the policy may update this value with a higher value if needed (but not lower).
     *
     * If 0 is given, min_bucket_count_in_out must still be 0 after the policy creation and
     * bucket_for_hash must always return 0 in this case.
     */
    explicit power_of_two_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = round_up_to_power_of_two(min_bucket_count_in_out);
            m_mask = min_bucket_count_in_out - 1;
        }
        else {
            m_mask = 0;
        }
    }

    /**
     * Return the bucket [0, bucket_count()) to which the hash belongs.
     * If bucket_count() is 0, it must always return 0.
     */
    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash & m_mask;
    }

    /**
     * Return the number of buckets that should be used on next growth.
     */
    std::size_t next_bucket_count() const {
        if((m_mask + 1) > max_bucket_count() / GrowthFactor) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return (m_mask + 1) * GrowthFactor;
    }

    /**
     * Return the maximum number of buckets supported by the policy.
     */
    std::size_t max_bucket_count() const {
        // Largest power of two.
        return (std::numeric_limits<std::size_t>::max() / 2) + 1;
    }

    /**
     * Reset the growth policy as if it was created with a bucket count of 0.
     * After a clear, the policy must always return 0 when bucket_for_hash is called.
     */
    void clear() noexcept {
        m_mask = 0;
    }

private:
    static std::size_t round_up_to_power_of_two(std::size_t value) {
        if(is_power_of_two(value)) {
            return value;
        }

        if(value == 0) {
            return 1;
        }

        --value;
        for(std::size_t i = 1; i < sizeof(std::size_t) * CHAR_BIT; i *= 2) {
            value |= value >> i;
        }

        return value + 1;
    }

    static constexpr bool is_power_of_two(std::size_t value) {
        return value != 0 && (value & (value - 1)) == 0;
    }

protected:
    static_assert(is_power_of_two(GrowthFactor) && GrowthFactor >= 2, "GrowthFactor must be a power of two >= 2.");

    std::size_t m_mask;
};

/**
 * Grow the hash table by GrowthFactor::num / GrowthFactor::den and use a modulo to map a hash
 * to a bucket. Slower but it can be useful if you want a slower growth.
 */
template<class GrowthFactor = std::ratio<3, 2>>
class mod_growth_policy {
public:
    explicit mod_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            m_mod = min_bucket_count_in_out;
        }
        else {
            m_mod = 1;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash % m_mod;
    }

    std::size_t next_bucket_count() const {
        if(m_mod == max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        const double next_bucket_count = std::ceil(double(m_mod) * REHASH_SIZE_MULTIPLICATION_FACTOR);
        if(!std::isnormal(next_bucket_count)) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(next_bucket_count > double(max_bucket_count())) {
            return max_bucket_count();
        }
        else {
            return std::size_t(next_bucket_count);
        }
    }

    std::size_t max_bucket_count() const {
        return MAX_BUCKET_COUNT;
    }

    void clear() noexcept {
        m_mod = 1;
    }

private:
    static constexpr double REHASH_SIZE_MULTIPLICATION_FACTOR = 1.0 * GrowthFactor::num / GrowthFactor::den;
    static const std::size_t MAX_BUCKET_COUNT =
            std::size_t(double(
                    std::numeric_limits<std::size_t>::max() / REHASH_SIZE_MULTIPLICATION_FACTOR
            ));

    static_assert(REHASH_SIZE_MULTIPLICATION_FACTOR >= 1.1, "Growth factor should be >= 1.1.");

    std::size_t m_mod;
};

namespace detail {

#if SIZE_MAX >= ULLONG_MAX
#define TSL_RH_NB_PRIMES 51
#elif SIZE_MAX >= ULONG_MAX
#define TSL_RH_NB_PRIMES 40
#else
#define TSL_RH_NB_PRIMES 23
#endif

static constexpr const std::array<std::size_t, TSL_RH_NB_PRIMES> PRIMES = {{
    1u, 5u, 17u, 29u, 37u, 53u, 67u, 79u, 97u, 131u, 193u, 257u, 389u, 521u, 769u, 1031u,
    1543u, 2053u, 3079u, 6151u, 12289u, 24593u, 49157u,
#if SIZE_MAX >= ULONG_MAX
    98317ul, 196613ul, 393241ul, 786433ul, 1572869ul, 3145739ul, 6291469ul, 12582917ul,
    25165843ul, 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, 1610612741ul,
    3221225473ul, 4294967291ul,
#endif
#if SIZE_MAX >= ULLONG_MAX
    6442450939ull, 12884901893ull, 25769803751ull, 51539607551ull, 103079215111ull, 206158430209ull,
    412316860441ull, 824633720831ull, 1649267441651ull, 3298534883309ull, 6597069766657ull,
#endif
}};

template<unsigned int IPrime>
static constexpr std::size_t mod(std::size_t hash) { return hash % PRIMES[IPrime]; }

// MOD_PRIME[iprime](hash) returns hash % PRIMES[iprime]. This table allows for faster modulo as the
// compiler can optimize the modulo code better with a constant known at the compilation.
static constexpr const std::array<std::size_t(*)(std::size_t), TSL_RH_NB_PRIMES> MOD_PRIME = {{
    &mod<0>, &mod<1>, &mod<2>, &mod<3>, &mod<4>, &mod<5>, &mod<6>, &mod<7>, &mod<8>, &mod<9>, &mod<10>,
    &mod<11>, &mod<12>, &mod<13>, &mod<14>, &mod<15>, &mod<16>, &mod<17>, &mod<18>, &mod<19>, &mod<20>,
    &mod<21>, &mod<22>,
#if SIZE_MAX >= ULONG_MAX
    &mod<23>, &mod<24>, &mod<25>, &mod<26>, &mod<27>, &mod<28>, &mod<29>, &mod<30>, &mod<31>, &mod<32>,
    &mod<33>, &mod<34>, &mod<35>, &mod<36>, &mod<37> , &mod<38>, &mod<39>,
#endif
#if SIZE_MAX >= ULLONG_MAX
    &mod<40>, &mod<41>, &mod<42>, &mod<43>, &mod<44>, &mod<45>, &mod<46>, &mod<47>, &mod<48>, &mod<49>,
    &mod<50>,
#endif
}};

}

/**
 * Grow the hash table by using prime numbers as bucket count. Slower than tsl::rh::power_of_two_growth_policy in
 * general but will probably distribute the values around better in the buckets with a poor hash function.
 *
 * To allow the compiler to optimize the modulo operation, a lookup table is used with constant primes numbers.
 *
 * With a switch the code would look like:
 * \code
 * switch(iprime) { // iprime is the current prime of the hash table
 *     case 0: hash % 5ul;
 *             break;
 *     case 1: hash % 17ul;
 *             break;
 *     case 2: hash % 29ul;
 *             break;
 *     ...
 * }
 * \endcode
 *
 * Due to the constant variable in the modulo the compiler is able to optimize the operation
 * by a series of multiplications, substractions and shifts.
 *
 * The 'hash % 5' could become something like 'hash - (hash * 0xCCCCCCCD) >> 34) * 5' in a 64 bits environment.
 */
class prime_growth_policy {
public:
    explicit prime_growth_policy(std::size_t& min_bucket_count_in_out) {
        auto it_prime = std::lower_bound(detail::PRIMES.begin(),
                                         detail::PRIMES.end(), min_bucket_count_in_out);
        if(it_prime == detail::PRIMES.end()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        m_iprime = static_cast<unsigned int>(std::distance(detail::PRIMES.begin(), it_prime));
        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = *it_prime;
        }
        else {
            min_bucket_count_in_out = 0;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return detail::MOD_PRIME[m_iprime](hash);
    }

    std::size_t next_bucket_count() const {
        if(m_iprime + 1 >= detail::PRIMES.size()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return detail::PRIMES[m_iprime + 1];
    }

    std::size_t max_bucket_count() const {
        return detail::PRIMES.back();
    }

    void clear() noexcept {
        m_iprime = 0;
    }

private:
    unsigned int m_iprime;

    static_assert(std::numeric_limits<decltype(m_iprime)>::max() >= detail::PRIMES.size(),
                  "The type of m_iprime is not big enough.");
};

}
}

#endif

class gammaPixel {
  public:
    float Binv[256];
    float B[256];
    gammaPixel() {
    for (int i = 0; i < 256; i++)
        Binv[i] = B[i] = i;
}
    float getBGradOnly(float color) {
    int c = color + 0.5f;
    if(c < 5) c = 5;
    if(c > 250) c = 250;
    return B[c + 1] - B[c];
}
    float getBInvGradOnly(float color) {
    int c = color + 0.5f;
    if(c < 5) c = 5;
    if(c > 250) c = 250;
    return Binv[c + 1] - Binv[c];
}
  public:
  private:
    std::function<void(const cv::Range &)> body;
};

class
preIntegrationBase
{
public:
preIntegrationBase(double sigma_w_, double sigma_wb_, double sigma_a_, double sigma_ab_, bool imu_avg_ = false) {}
void setLinearizationPoints(Eigen::Matrix<double, 3, 1> b_w_lin_, Eigen::Matrix<double, 3, 1> b_a_lin_,
		Eigen::Matrix<double, 4, 1> q_k_lin_ = Eigen::Matrix<double, 4, 1>::Zero(), Eigen::Matrix<double, 3, 1> grav_ = Eigen::Matrix<double, 3, 1>::Zero()) {}
virtual void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) = 0 {
	double delta_t = t_1 - t_0;
	dt += delta_t;
Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;
if (imu_avg) {}
Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;
double w_1 = w_hat(0, 0);
double w_2 = w_hat(1, 0);
double w_3 = w_hat(2, 0);
double mag_w = w_hat.norm();
double w_dt = mag_w * delta_t;
bool small_w = (mag_w < 0.008726646);
double dt_2 = std::pow(delta_t, 2);
double cos_wt = std::cos(w_dt);
double sin_wt = std::sin(w_dt);
Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;
Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;
Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();
if (imu_avg) {}
Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);
double f_1;
double f_2;
double f_3;
double f_4;
if (small_w) {}
else  {}
Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);
Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;
alpha_tau += beta_tau * delta_t + H_al * a_hat;
beta_tau += H_be * a_hat;
Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;
Eigen::Matrix<double, 3, 3> J_save = J_q;
J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;
H_a -= H_al;
H_a += delta_t * H_b;
H_b -= H_be;
Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
O_a += delta_t * O_b;
O_a += -H_al * R_tau_k
O_b += -H_be * R_tau_k
Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);
double df_1_dbw_1;
double df_1_dbw_2;
double df_1_dbw_3;
double df_2_dbw_1;
double df_2_dbw_2;
double df_2_dbw_3;
double df_3_dbw_1;
double df_3_dbw_2;
double df_3_dbw_3;
double df_4_dbw_1;
double df_4_dbw_2;
double df_4_dbw_3;
if (small_w) {}
else  {}
Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;
J_a += J_b * delta_t;
J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;
Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
double dt_mid = delta_t / 2.0;
double w_dt_mid = mag_w * dt_mid;
Eigen::Matrix<double, 3, 3> R_mid;
R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
R_mid = R_mid * R_tau_k;
Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
F_k1.block(0, 0, 3, 3) = - w_x;
F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();
Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
F_k2.block(0, 0, 3, 3) = - w_x;
F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();
Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
Eigen::Matrix<double, 21, 12> G_k3 = G_k2;
Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();
Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
F_k4.block(0, 0, 3, 3) = - w_x;
F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();
P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
P_big = 0.5 * (P_big + P_big.transpose());
Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);
Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
B_k.block(15, 15, 3, 3).setZero();
B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
P_big = B_k * P_big * B_k.transpose();
P_big = 0.5 * (P_big + P_big.transpose());
Discrete_J_b = B_k * Phi * Discrete_J_b;
P_meas = P_big.block(0, 0, 15, 15);
if (state_transition_jacobians) {}
R_tau_k = R_tau1_k;
q_tau_k = quatType::rotToQuat(R_tau_k);
};

This code segment functions to: Perform inertial measurement unit data pre-integration for state estimation, including covariance propagation and error state Jacobian calculations using numerical integration methods while compensating for sensor biases and gravitational effects.

```cpp
[TASK_BEGIN]
void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) override {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg) {
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ?
		Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2)/2) * w_x_2 :
		Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt/mag_w)*w_x + ((1.0-cos_wt)/(std::pow(mag_w,2.0)))*w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg) {
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}

	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1, f_2, f_3, f_4;
	if (small_w) {
		f_1 = -(std::pow(delta_t,3)/3);
		f_2 = (std::pow(delta_t,4)/8);
		f_3 = -(std::pow(delta_t,2)/2);
		f_4 = (std::pow(delta_t,3)/6);
	} else {
		f_1 = (w_dt*cos_wt - sin_wt)/std::pow(mag_w,3);
		f_2 = (std::pow(w_dt,2)-2*cos_wt-2*w_dt*sin_wt+2)/(2*std::pow(mag_w,4));
		f_3 = -(1-cos_wt)/std::pow(mag_w,2);
		f_4 = (w_dt-sin_wt)/std::pow(mag_w,3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = (dt_2/2.0)*Eigen::Matrix<double,3,3>::Identity() + f_1*w_x + f_2*w_x_2;
	Eigen::Matrix<double, 3, 3> Beta_arg = delta_t*Eigen::Matrix<double,3,3>::Identity() + f_3*w_x + f_4*w_x_2;

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ?
		Eigen::Matrix<double,3,3>::Identity() - 0.5*w_tx + (1.0/6.0)*w_tx*w_tx :
		Eigen::Matrix<double,3,3>::Identity() - ((1-cos_wt)/std::pow(w_dt,2.0))*w_tx +
		((w_dt-sin_wt)/std::pow(w_dt,3.0))*w_tx*w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	// ... (remaining covariance and Jacobian calculations)

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
[TASK_END]
```

void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) override {
	double delta_t = t_1 - t_0;
	dt += delta_t;
	if (delta_t == 0) return;
	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;
	if (imu_avg) {
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}
	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;
	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);
	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;
	bool small_w = (mag_w < 0.008726646);
	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);
	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;
	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ?
		Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2)/2) * w_x_2 :
		Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt/mag_w)*w_x + ((1.0-cos_wt)/(std::pow(mag_w,2.0)))*w_x_2;
	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();
	if (imu_avg) {
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);
	double f_1, f_2, f_3, f_4;
	if (small_w) {
		f_1 = -(std::pow(delta_t,3)/3);
		f_2 = (std::pow(delta_t,4)/8);
		f_3 = -(std::pow(delta_t,2)/2);
		f_4 = (std::pow(delta_t,3)/6);
	} else {
		f_1 = (w_dt*cos_wt - sin_wt)/std::pow(mag_w,3);
		f_2 = (std::pow(w_dt,2)-2*cos_wt-2*w_dt*sin_wt+2)/(2*std::pow(mag_w,4));
		f_3 = -(1-cos_wt)/std::pow(mag_w,2);
		f_4 = (w_dt-sin_wt)/std::pow(mag_w,3);
	}
	Eigen::Matrix<double, 3, 3> alpha_arg = (dt_2/2.0)*Eigen::Matrix<double,3,3>::Identity() + f_1*w_x + f_2*w_x_2;
	Eigen::Matrix<double, 3, 3> Beta_arg = delta_t*Eigen::Matrix<double,3,3>::Identity() + f_3*w_x + f_4*w_x_2;
	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;
	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;
	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ?
		Eigen::Matrix<double,3,3>::Identity() - 0.5*w_tx + (1.0/6.0)*w_tx*w_tx :
		Eigen::Matrix<double,3,3>::Identity() - ((1-cos_wt)/std::pow(w_dt,2.0))*w_tx +
		((w_dt-sin_wt)/std::pow(w_dt,3.0))*w_tx*w_tx;
	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;
	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;
	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);
	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}