Factor_ImuCPIv1.cpp
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #include "Factor_ImuCPIv1.h"
23 
24 #include "utils/quat_ops.h"
25 
26 using namespace ov_init;
27 
28 Factor_ImuCPIv1::Factor_ImuCPIv1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta,
29  Eigen::Vector4d &q_KtoK1, Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q,
30  Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha, Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha,
31  Eigen::Matrix<double, 15, 15> &covariance) {
32  // Save measurements
33  this->alpha = alpha;
34  this->beta = beta;
35  this->q_breve = q_KtoK1;
36  this->dt = deltatime;
37  this->grav_save = grav;
38 
39  // Linearization points
40  this->b_a_lin_save = ba_lin;
41  this->b_w_lin_save = bg_lin;
42 
43  // Save bias jacobians
44  this->J_q = J_q;
45  this->J_a = J_alpha;
46  this->J_b = J_beta;
47  this->H_a = H_alpha;
48  this->H_b = H_beta;
49 
50  // Check that we have a valid covariance matrix that we can get the information of
51  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(covariance.rows(), covariance.rows());
52  Eigen::MatrixXd information = covariance.llt().solve(I);
53  if (std::isnan(information.norm())) {
54  std::cerr << "P - " << std::endl << covariance << std::endl << std::endl;
55  std::cerr << "Pinv - " << std::endl << covariance.inverse() << std::endl << std::endl;
56  std::exit(EXIT_FAILURE);
57  }
58 
59  // Get square-root of our information matrix
60  Eigen::LLT<Eigen::MatrixXd> lltOfI(information);
61  sqrtI_save = lltOfI.matrixL().transpose();
62 
63  // Set the number of measurements, and the block sized
64  set_num_residuals(15);
65  mutable_parameter_block_sizes()->push_back(4); // q_GtoI1
66  mutable_parameter_block_sizes()->push_back(3); // bg_1
67  mutable_parameter_block_sizes()->push_back(3); // v_I1inG
68  mutable_parameter_block_sizes()->push_back(3); // ba_1
69  mutable_parameter_block_sizes()->push_back(3); // p_I1inG
70  mutable_parameter_block_sizes()->push_back(4); // q_GtoI2
71  mutable_parameter_block_sizes()->push_back(3); // bg_2
72  mutable_parameter_block_sizes()->push_back(3); // v_I2inG
73  mutable_parameter_block_sizes()->push_back(3); // ba_2
74  mutable_parameter_block_sizes()->push_back(3); // p_I2inG
75 }
76 
77 bool Factor_ImuCPIv1::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
78 
79  // Get the local variables (these would be different if we relinearized)
80  Eigen::Vector3d gravity = grav_save;
81  Eigen::Matrix<double, 15, 15> sqrtI = sqrtI_save;
82  Eigen::Vector3d b_w_lin = b_w_lin_save;
83  Eigen::Vector3d b_a_lin = b_a_lin_save;
84 
85  // Get the state estimates from the ceres parameters.
86  // Each clone is stored in the canonical VINS format: q, bw, v, ba, p
87  Eigen::Vector4d q_1 = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
88  Eigen::Matrix3d R_1 = ov_core::quat_2_Rot(q_1);
89  Eigen::Vector4d q_2 = Eigen::Map<const Eigen::Vector4d>(parameters[5]);
90 
91  // Get bias_w
92  Eigen::Vector3d b_w1 = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
93  Eigen::Vector3d b_w2 = Eigen::Map<const Eigen::Vector3d>(parameters[6]);
94 
95  // Get bias_a
96  Eigen::Vector3d b_a1 = Eigen::Map<const Eigen::Vector3d>(parameters[3]);
97  Eigen::Vector3d b_a2 = Eigen::Map<const Eigen::Vector3d>(parameters[8]);
98 
99  // Get velocity
100  Eigen::Vector3d v_1 = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
101  Eigen::Vector3d v_2 = Eigen::Map<const Eigen::Vector3d>(parameters[7]);
102 
103  // Get positions
104  Eigen::Vector3d p_1 = Eigen::Map<const Eigen::Vector3d>(parameters[4]);
105  Eigen::Vector3d p_2 = Eigen::Map<const Eigen::Vector3d>(parameters[9]);
106 
107  // Get the change in clone 1's biases from the linearization points
108  Eigen::Vector3d dbw = b_w1 - b_w_lin;
109  Eigen::Vector3d dba = b_a1 - b_a_lin;
110 
111  // Quaternion associated with the bias w correction
112  // Eigen::Vector4d q_b= rot_2_quat(Exp(-J_q*dbw));
113  Eigen::Vector4d q_b;
114  q_b.block(0, 0, 3, 1) = 0.5 * J_q * dbw;
115  q_b(3, 0) = 1.0;
116  q_b = q_b / q_b.norm();
117 
118  // Relative orientation from state estimates
119  Eigen::Vector4d q_1_to_2 = ov_core::quat_multiply(q_2, ov_core::Inv(q_1));
120 
121  // Intermediate quaternions for error/jacobian computation
122  Eigen::Vector4d q_res_minus = ov_core::quat_multiply(q_1_to_2, ov_core::Inv(q_breve));
123  Eigen::Vector4d q_res_plus = ov_core::quat_multiply(q_res_minus, q_b);
124 
125  //================================================================================
126  //================================================================================
127  //================================================================================
128 
129  // Computer residual
130  Eigen::Matrix<double, 15, 1> res;
131  res.block(0, 0, 3, 1) = 2 * q_res_plus.block(0, 0, 3, 1);
132  res.block(3, 0, 3, 1) = b_w2 - b_w1;
133  res.block(6, 0, 3, 1) = R_1 * (v_2 - v_1 + gravity * dt) - J_b * dbw - H_b * dba - beta;
134  res.block(9, 0, 3, 1) = b_a2 - b_a1;
135  res.block(12, 0, 3, 1) = R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)) - J_a * dbw - H_a * dba - alpha;
136  res = sqrtI * res;
137 
138  // Store residuals
139  for (int i = 0; i < res.rows(); i++) {
140  residuals[i] = res(i, 0);
141  }
142 
143  //================================================================================
144  //================================================================================
145  //================================================================================
146 
147  // Compute jacobians if asked
148  if (jacobians) {
149 
150  // Stores the total preintegrated jacobian into one spot
151  Eigen::Matrix<double, 15, 30> Jacobian = Eigen::Matrix<double, 15, 30>::Zero();
152 
153  // Quick identity
154  Eigen::Matrix<double, 3, 3> eye = Eigen::MatrixXd::Identity(3, 3);
155  Eigen::Matrix<double, 4, 1> q_meas_plus = ov_core::quat_multiply(ov_core::Inv(q_breve), q_b);
156 
157  // Dtheta wrt theta 1
158  Jacobian.block(0, 0, 3, 3) = -((q_1_to_2(3, 0) * eye - ov_core::skew_x(q_1_to_2.block(0, 0, 3, 1))) *
159  (q_meas_plus(3, 0) * eye + ov_core::skew_x(q_meas_plus.block(0, 0, 3, 1))) -
160  q_1_to_2.block(0, 0, 3, 1) * q_meas_plus.block(0, 0, 3, 1).transpose());
161 
162  // Dtheta wrt theta 2
163  Jacobian.block(0, 15, 3, 3) = q_res_plus(3, 0) * eye + ov_core::skew_x(q_res_plus.block(0, 0, 3, 1));
164 
165  // Dtheta wrt bw 1
166  Jacobian.block(0, 3, 3, 3) = (q_res_minus(3, 0) * eye - ov_core::skew_x(q_res_minus.block(0, 0, 3, 1))) * J_q;
167 
168  // Dbw wrt bw1 and bw2
169  Jacobian.block(3, 3, 3, 3) = -eye;
170  Jacobian.block(3, 18, 3, 3) = eye;
171 
172  // Dvelocity wrt theta 1
173  Jacobian.block(6, 0, 3, 3) = ov_core::skew_x(R_1 * (v_2 - v_1 + gravity * dt));
174 
175  // Dvelocity wrt v 1
176  Jacobian.block(6, 6, 3, 3) = -R_1;
177 
178  // Dvelocity wrt v 2
179  Jacobian.block(6, 21, 3, 3) = R_1;
180 
181  // Dvelocity wrt bw 1
182  Jacobian.block(6, 3, 3, 3) = -J_b;
183 
184  // Dvelocity wrt ba 1
185  Jacobian.block(6, 9, 3, 3) = -H_b;
186 
187  // Dbw wrt ba1 and ba2
188  Jacobian.block(9, 9, 3, 3) = -eye;
189  Jacobian.block(9, 24, 3, 3) = eye;
190 
191  // Dposition wrt theta 1
192  Jacobian.block(12, 0, 3, 3) = ov_core::skew_x(R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)));
193  // Dposition wrt v 1
194  Jacobian.block(12, 6, 3, 3) = -R_1 * dt;
195  // Dposition wrt p 1
196  Jacobian.block(12, 12, 3, 3) = -R_1;
197 
198  // Dposition wrt p 2
199  Jacobian.block(12, 27, 3, 3) = R_1;
200 
201  // Dposition wrt bw 1
202  Jacobian.block(12, 3, 3, 3) = -J_a;
203  // Dposition wrt ba 1
204  Jacobian.block(12, 9, 3, 3) = -H_a;
205 
206  // Apply sqrt info
207  Jacobian = sqrtI * Jacobian;
208 
209  // Now store jacobians
210  // Th1
211  if (jacobians[0]) {
212  Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> J_th1(jacobians[0], 15, 4);
213  J_th1.block(0, 0, 15, 3) = Jacobian.block(0, 0, 15, 3);
214  J_th1.block(0, 3, 15, 1).setZero();
215  }
216  // Th2
217  if (jacobians[5]) {
218  Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> J_th2(jacobians[5], 15, 4);
219  J_th2.block(0, 0, 15, 3) = Jacobian.block(0, 15, 15, 3);
220  J_th2.block(0, 3, 15, 1).setZero();
221  }
222  // bw1
223  if (jacobians[1]) {
224  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_bw1(jacobians[1], 15, 3);
225  J_bw1.block(0, 0, 15, 3) = Jacobian.block(0, 3, 15, 3);
226  }
227  // bw2
228  if (jacobians[6]) {
229  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_bw2(jacobians[6], 15, 3);
230  J_bw2.block(0, 0, 15, 3) = Jacobian.block(0, 18, 15, 3);
231  }
232  // v1
233  if (jacobians[2]) {
234  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_v1(jacobians[2], 15, 3);
235  J_v1.block(0, 0, 15, 3) = Jacobian.block(0, 6, 15, 3);
236  }
237  // v2
238  if (jacobians[7]) {
239  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_v2(jacobians[7], 15, 3);
240  J_v2.block(0, 0, 15, 3) = Jacobian.block(0, 21, 15, 3);
241  }
242  // ba1
243  if (jacobians[3]) {
244  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_ba1(jacobians[3], 15, 3);
245  J_ba1.block(0, 0, 15, 3) = Jacobian.block(0, 9, 15, 3);
246  }
247  // ba2
248  if (jacobians[8]) {
249  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_ba2(jacobians[8], 15, 3);
250  J_ba2.block(0, 0, 15, 3) = Jacobian.block(0, 24, 15, 3);
251  }
252  // p1
253  if (jacobians[4]) {
254  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_p1(jacobians[4], 15, 3);
255  J_p1.block(0, 0, 15, 3) = Jacobian.block(0, 12, 15, 3);
256  }
257  // p2
258  if (jacobians[9]) {
259  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_p2(jacobians[9], 15, 3);
260  J_p2.block(0, 0, 15, 3) = Jacobian.block(0, 27, 15, 3);
261  }
262  }
263  return true;
264 }
ov_init::Factor_ImuCPIv1::b_a_lin_save
Eigen::Vector3d b_a_lin_save
Definition: Factor_ImuCPIv1.h:42
ov_core::skew_x
Eigen::Matrix< double, 3, 3 > skew_x(const Eigen::Matrix< double, 3, 1 > &w)
quat_ops.h
ov_init::Factor_ImuCPIv1::J_q
Eigen::Matrix3d J_q
Definition: Factor_ImuCPIv1.h:45
ov_init::Factor_ImuCPIv1::Evaluate
bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override
Error residual and Jacobian calculation.
Definition: Factor_ImuCPIv1.cpp:77
ov_init::Factor_ImuCPIv1::J_a
Eigen::Matrix3d J_a
Definition: Factor_ImuCPIv1.h:46
ov_core::quat_multiply
Eigen::Matrix< double, 4, 1 > quat_multiply(const Eigen::Matrix< double, 4, 1 > &q, const Eigen::Matrix< double, 4, 1 > &p)
ov_init::Factor_ImuCPIv1::dt
double dt
Definition: Factor_ImuCPIv1.h:38
ov_init::Factor_ImuCPIv1::grav_save
Eigen::Vector3d grav_save
Definition: Factor_ImuCPIv1.h:55
ov_init::Factor_ImuCPIv1::H_a
Eigen::Matrix3d H_a
Definition: Factor_ImuCPIv1.h:48
Factor_ImuCPIv1.h
ov_init::Factor_ImuCPIv1::J_b
Eigen::Matrix3d J_b
Definition: Factor_ImuCPIv1.h:47
ov_init
State initialization code.
Definition: Factor_GenericPrior.h:27
ov_init::Factor_ImuCPIv1::H_b
Eigen::Matrix3d H_b
Definition: Factor_ImuCPIv1.h:49
ov_core::quat_2_Rot
Eigen::Matrix< double, 3, 3 > quat_2_Rot(const Eigen::Matrix< double, 4, 1 > &q)
ov_init::Factor_ImuCPIv1::beta
Eigen::Vector3d beta
Definition: Factor_ImuCPIv1.h:36
ov_init::Factor_ImuCPIv1::alpha
Eigen::Vector3d alpha
Definition: Factor_ImuCPIv1.h:35
ov_init::Factor_ImuCPIv1::sqrtI_save
Eigen::Matrix< double, 15, 15 > sqrtI_save
Definition: Factor_ImuCPIv1.h:52
ov_init::Factor_ImuCPIv1::b_w_lin_save
Eigen::Vector3d b_w_lin_save
Definition: Factor_ImuCPIv1.h:41
ov_init::Factor_ImuCPIv1::Factor_ImuCPIv1
Factor_ImuCPIv1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta, Eigen::Vector4d &q_KtoK1, Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q, Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha, Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, Eigen::Matrix< double, 15, 15 > &covariance)
Default constructor.
Definition: Factor_ImuCPIv1.cpp:28
ov_core::Inv
Eigen::Matrix< double, 4, 1 > Inv(Eigen::Matrix< double, 4, 1 > q)
ov_init::Factor_ImuCPIv1::q_breve
Eigen::Vector4d q_breve
Definition: Factor_ImuCPIv1.h:37

ov_init
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:51