SaDVIO
residuals.hpp
Go to the documentation of this file.
1 #ifndef RESIDUALS_H
2 #define RESIDUALS_H
3 
4 #include "isaeslam/data/sensors/Camera.h"
5 #include "isaeslam/data/sensors/IMU.h"
6 #include <ceres/ceres.h>
7 
8 namespace isae {
9 
13 class Motion2DFactor : public ceres::SizedCostFunction<6, 6, 6> {
14  public:
15  Motion2DFactor(const Eigen::Vector2d motion_2d, const Eigen::MatrixXd sqrt_inf, const double dt)
16  : _motion_2d(motion_2d), _sqrt_inf(sqrt_inf), _dt(dt) {
17  _dx = _motion_2d(0) * _dt;
18  _dtheta = _motion_2d(1) * _dt;
19  }
20  Motion2DFactor() {}
21 
22  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
23  Eigen::Affine3d T_fk_w = geometry::se3_doubleVec6dtoRT(parameters[0]);
24  Eigen::Affine3d T_fkp1_w = geometry::se3_doubleVec6dtoRT(parameters[1]);
25 
26  Eigen::Affine3d T_delta = Eigen::Affine3d::Identity();
27  T_delta.linear() << std::cos(_dtheta), -std::sin(_dtheta), 0, std::sin(_dtheta), std::cos(_dtheta), 0, 0, 0, 1;
28  T_delta.translation() << _dx, 0, 0;
29 
30  Eigen::Map<Vector6d> err(residuals);
31  err = _sqrt_inf * geometry::se3_RTtoVec6d(T_fk_w.inverse() * T_delta * T_fkp1_w);
32 
33  if (jacobians != NULL) {
34  if (jacobians[0] != NULL) {
35  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J_k(jacobians[0]);
36  J_k.setIdentity();
37 
38  Eigen::Affine3d T_gamma = T_delta * T_fkp1_w;
39  Eigen::Vector3d w = geometry::log_so3(T_fk_w.linear().transpose() * T_gamma.linear());
40  J_k.block<3, 3>(0, 0) = -geometry::so3_rightJacobian(w).inverse() * T_gamma.linear().transpose() *
41  T_fk_w.linear() *
42  geometry::so3_rightJacobian(geometry::log_so3(T_fk_w.linear()));
43  J_k.block<3, 3>(3, 0) =
44  T_fk_w.linear().transpose() * geometry::skewMatrix(T_gamma.translation() - T_fk_w.translation()) *
45  T_fk_w.linear() * geometry::so3_rightJacobian(geometry::log_so3(T_fk_w.linear()));
46  J_k.block<3, 3>(3, 3) = -T_fk_w.linear().transpose();
47  J_k = _sqrt_inf * J_k;
48  }
49 
50  if (jacobians[1] != NULL) {
51  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J_kp1(jacobians[1]);
52  J_kp1.setIdentity();
53 
54  Eigen::Affine3d T_beta = T_fk_w.inverse() * T_delta;
55  Eigen::Vector3d w = geometry::log_so3(T_beta.linear() * T_fkp1_w.linear());
56  J_kp1.block<3, 3>(0, 0) = geometry::so3_rightJacobian(w).inverse() *
57  geometry::so3_rightJacobian(geometry::log_so3(T_fkp1_w.linear()));
58  J_kp1.block<3, 3>(3, 3) = T_beta.linear();
59  J_kp1 = _sqrt_inf * J_kp1;
60  }
61  }
62 
63  return true;
64  }
65 
66  Eigen::Vector2d _motion_2d;
67  Eigen::MatrixXd _sqrt_inf;
68  double _dt;
69  double _dx;
70  double _dtheta;
71 };
72 
76 class Relative6DPose : public ceres::SizedCostFunction<6, 6, 6> {
77  public:
78  Relative6DPose(const Eigen::Affine3d T_w_a,
79  const Eigen::Affine3d T_w_b,
80  const Eigen::Affine3d T_a_b_prior,
81  const Eigen::MatrixXd sqrt_inf)
82  : _T_w_a(T_w_a), _T_w_b(T_w_b), _T_a_b_prior(T_a_b_prior), _sqrt_inf(sqrt_inf) {}
83  Relative6DPose() {}
84 
85  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
86  Eigen::Map<Vector6d> err(residuals);
87  Eigen::Affine3d T_w_a_up = _T_w_a * geometry::se3_doubleVec6dtoRT(parameters[0]);
88  Eigen::Affine3d T_w_b_up = _T_w_b * geometry::se3_doubleVec6dtoRT(parameters[1]);
89  Eigen::Affine3d T_b_a_prior = _T_a_b_prior.inverse();
90  Eigen::Affine3d T = T_b_a_prior * T_w_a_up.inverse() * T_w_b_up;
91  err = _sqrt_inf * geometry::se3_RTtoVec6d(T);
92 
93  if (jacobians != NULL) {
94 
95  Eigen::Vector3d tb = T_w_b_up.translation();
96  Eigen::Vector3d ta = T_w_a_up.translation();
97  Eigen::Vector3d w = geometry::log_so3(T.rotation());
98 
99  if (jacobians[0] != NULL) {
100  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobians[0]);
101  J.setIdentity();
102  Eigen::Vector3d dw = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
103 
104  // d(log(dr)) / d(taua)
105  J.block(0, 0, 3, 3) = -geometry::so3_rightJacobian(w).inverse() * T_w_b_up.rotation().transpose() *
106  T_w_a_up.rotation() * geometry::so3_rightJacobian(dw);
107 
108  // d(dt) / d(taua)
109  J.block(3, 0, 3, 3) = T_b_a_prior.rotation() * T_w_a_up.rotation().transpose() *
110  geometry::skewMatrix(tb - ta) * T_w_a_up.rotation() *
111  geometry::so3_rightJacobian(dw);
112 
113  // d(dt) / d(ta)
114  J.block(3, 3, 3, 3) = -T_b_a_prior.rotation();
115  J = _sqrt_inf * J;
116  }
117 
118  if (jacobians[1] != NULL) {
119  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobians[1]);
120  J.setIdentity();
121  Eigen::Vector3d dw = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
122 
123  // d(log(dr)) / d(taub)
124  J.block(0, 0, 3, 3) = geometry::so3_rightJacobian(w).inverse() * geometry::so3_rightJacobian(dw);
125 
126  // d(dt) / d(tb)
127  J.block(3, 3, 3, 3) = T_b_a_prior.rotation() * T_w_a_up.rotation().transpose() * T_w_b_up.rotation();
128  J = _sqrt_inf * J;
129  }
130  }
131 
132  return true;
133  }
134 
135  Eigen::Affine3d _T_w_a, _T_w_b, _T_a_b_prior;
136  Eigen::MatrixXd _sqrt_inf;
137 };
138 
145 class IMUFactor : public ceres::SizedCostFunction<9, 6, 6, 3, 3, 3, 3> {
146  public:
147  IMUFactor(const std::shared_ptr<IMU> imu_i, const std::shared_ptr<IMU> imu_j) : _imu_i(imu_i), _imu_j(imu_j) {}
148 
149  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
150 
151  // Beware: the assumption here is that the IMU frame is the main frame
152  Eigen::Affine3d T_fi_w =
153  _imu_i->getFrame()->getWorld2FrameTransform() * geometry::se3_doubleVec6dtoRT(parameters[0]);
154  Eigen::Affine3d T_fj_w =
155  _imu_j->getFrame()->getWorld2FrameTransform() * geometry::se3_doubleVec6dtoRT(parameters[1]);
156  Eigen::Vector3d v_i = _imu_i->getVelocity() + Eigen::Map<const Eigen::Vector3d>(parameters[2]);
157  Eigen::Vector3d v_j = _imu_j->getVelocity() + Eigen::Map<const Eigen::Vector3d>(parameters[3]);
158  Eigen::Vector3d d_ba = Eigen::Map<const Eigen::Vector3d>(parameters[4]);
159  Eigen::Vector3d d_bg = Eigen::Map<const Eigen::Vector3d>(parameters[5]);
160  double dtij = (_imu_j->getFrame()->getTimestamp() - _imu_i->getFrame()->getTimestamp()) * 1e-9;
161 
162  // Get the information matrix
163  Eigen::MatrixXd cov = _imu_j->getCov();
164  // cov = cov / cov(0, 0); // Normalize the covariance to have weights ~ 1
165  Eigen::Matrix<double, 9, 9> inf_sqrt =
166  Eigen::LLT<Eigen::Matrix<double, 9, 9>>(cov.inverse()).matrixL().transpose();
167 
168  // Derive the residuals
169  Eigen::Matrix3d dR = (_imu_j->getDeltaR() * geometry::exp_so3(_imu_j->_J_dR_bg * d_bg)).transpose() *
170  T_fi_w.rotation() * T_fj_w.rotation().transpose();
171  Eigen::Vector3d r_dr = geometry::log_so3(dR);
172  Eigen::Vector3d r_dv = T_fi_w.rotation() * (v_j - v_i - g * dtij) -
173  (_imu_j->getDeltaV() + _imu_j->_J_dv_bg * d_bg + _imu_j->_J_dv_ba * d_ba);
174  Eigen::Vector3d r_dp = T_fi_w.rotation() * (T_fj_w.inverse().translation() - T_fi_w.inverse().translation() -
175  v_i * dtij - 0.5 * g * dtij * dtij) -
176  (_imu_j->getDeltaP() + _imu_j->_J_dp_bg * d_bg + _imu_j->_J_dp_ba * d_ba);
177  Eigen::Map<Eigen::Matrix<double, 9, 1>> err(residuals);
178  err.block(0, 0, 3, 1) = r_dr;
179  err.block(3, 0, 3, 1) = r_dv;
180  err.block(6, 0, 3, 1) = r_dp;
181  err = inf_sqrt * err;
182 
183  if (jacobians != NULL) {
184 
185  // Jacobian wrt the pose i
186  if (jacobians[0] != NULL) {
187  Eigen::Map<Eigen::Matrix<double, 9, 6, Eigen::RowMajor>> J_dTfi(jacobians[0]);
188  J_dTfi = Eigen::Matrix<double, 9, 6, Eigen::RowMajor>::Zero();
189  Eigen::Vector3d w_dfi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
190  Eigen::Matrix3d J_r_wdfi = geometry::so3_rightJacobian(w_dfi);
191  J_dTfi.block(0, 0, 3, 3) = geometry::so3_rightJacobian(r_dr).inverse() * T_fj_w.rotation() * J_r_wdfi;
192  J_dTfi.block(3, 0, 3, 3) = -T_fi_w.rotation() * geometry::skewMatrix(v_j - v_i - g * dtij) * J_r_wdfi;
193  J_dTfi.block(6, 0, 3, 3) =
194  -T_fi_w.rotation() *
195  geometry::skewMatrix(T_fj_w.inverse().translation() - v_i * dtij - 0.5 * g * dtij * dtij) *
196  J_r_wdfi;
197  J_dTfi.block(6, 3, 3, 3) = _imu_i->getFrame()->getWorld2FrameTransform().rotation();
198  J_dTfi = inf_sqrt * J_dTfi;
199  }
200 
201  // Jacobian wrt the pose j
202  if (jacobians[1] != NULL) {
203  Eigen::Map<Eigen::Matrix<double, 9, 6, Eigen::RowMajor>> J_dTfj(jacobians[1]);
204  J_dTfj = Eigen::Matrix<double, 9, 6, Eigen::RowMajor>::Zero();
205  Eigen::Vector3d w_dfj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
206  Eigen::Matrix3d J_r_wdfj = geometry::so3_rightJacobian(w_dfj);
207  J_dTfj.block(0, 0, 3, 3) = -geometry::so3_rightJacobian(r_dr).inverse() * T_fj_w.rotation() * J_r_wdfj;
208  J_dTfj.block(6, 0, 3, 3) = -T_fi_w.rotation() * T_fj_w.rotation().transpose() *
209  geometry::skewMatrix(T_fj_w.translation()) * T_fj_w.rotation() * J_r_wdfj;
210  J_dTfj.block(6, 3, 3, 3) = -T_fi_w.rotation() * geometry::exp_so3(w_dfj).transpose();
211  J_dTfj = inf_sqrt * J_dTfj;
212  }
213 
214  // Jacobian wrt the velocity i
215  if (jacobians[2] != NULL) {
216  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dvi(jacobians[2]);
217  J_dvi = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
218  J_dvi.block(3, 0, 3, 3) = -T_fi_w.rotation();
219  J_dvi.block(6, 0, 3, 3) = -T_fi_w.rotation() * dtij;
220  J_dvi = inf_sqrt * J_dvi;
221  }
222 
223  // Jacobian wrt the velocity j
224  if (jacobians[3] != NULL) {
225  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dvj(jacobians[3]);
226  J_dvj = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
227  J_dvj.block(3, 0, 3, 3) = T_fi_w.rotation();
228  J_dvj = inf_sqrt * J_dvj;
229  }
230 
231  // Jacobian wrt the bias of the accelerometer
232  if (jacobians[4] != NULL) {
233  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dba(jacobians[4]);
234  J_dba = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
235  J_dba.block(3, 0, 3, 3) = -_imu_j->_J_dv_ba;
236  J_dba.block(6, 0, 3, 3) = -_imu_j->_J_dp_ba;
237  J_dba = inf_sqrt * J_dba;
238  }
239 
240  // Jacobian wrt the bias of the gyroscope
241  if (jacobians[5] != NULL) {
242  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dbg(jacobians[5]);
243  J_dbg = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
244  J_dbg.block(0, 0, 3, 3) = -geometry::so3_rightJacobian(r_dr).inverse() * dR.transpose() *
245  geometry::so3_rightJacobian(_imu_j->_J_dR_bg * d_bg) * _imu_j->_J_dR_bg;
246  J_dbg.block(3, 0, 3, 3) = -_imu_j->_J_dv_bg;
247  J_dbg.block(6, 0, 3, 3) = -_imu_j->_J_dp_bg;
248  J_dbg = inf_sqrt * J_dbg;
249  }
250  }
251 
252  return true;
253  }
254 
255  std::shared_ptr<IMU> _imu_i, _imu_j;
256 };
257 
261 class IMUBiasFactor : public ceres::SizedCostFunction<6, 3, 3, 3, 3> {
262 
263  public:
264  IMUBiasFactor(const std::shared_ptr<IMU> imu_i, const std::shared_ptr<IMU> imu_j) : _imu_i(imu_i), _imu_j(imu_j) {}
265 
266  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
267  Eigen::Vector3d d_bai = Eigen::Map<const Eigen::Vector3d>(parameters[0]);
268  Eigen::Vector3d d_bgi = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
269  Eigen::Vector3d d_baj = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
270  Eigen::Vector3d d_bgj = Eigen::Map<const Eigen::Vector3d>(parameters[3]);
271  double dtij = (_imu_j->getFrame()->getTimestamp() - _imu_i->getFrame()->getTimestamp()) * 1e-9;
272 
273  double sigma2_dba = dtij * _imu_i->getbAccNoise() * _imu_i->getbAccNoise(); // / _imu_j->getCov()(0, 0);
274  Eigen::Matrix3d sqrt_inf_ba = Eigen::Matrix3d::Identity() * (1 / std::sqrt(sigma2_dba));
275  double sigma2_dbg = dtij * _imu_i->getbGyrNoise() * _imu_i->getbGyrNoise(); // / _imu_j->getCov()(0, 0);
276  Eigen::Matrix3d sqrt_inf_bg = Eigen::Matrix3d::Identity() * (1 / std::sqrt(sigma2_dbg));
277 
278  Eigen::Map<Eigen::Matrix<double, 6, 1>> err(residuals);
279  err.block(0, 0, 3, 1) = sqrt_inf_ba * (_imu_j->getBa() + d_baj - _imu_i->getBa() - d_bai);
280  err.block(3, 0, 3, 1) = sqrt_inf_bg * (_imu_j->getBg() + d_bgj - _imu_i->getBg() - d_bgi);
281 
282  if (jacobians != NULL) {
283 
284  if (jacobians[0] != NULL) {
285  Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> J_dba(jacobians[0]);
286  J_dba = Eigen::Matrix<double, 6, 3, Eigen::RowMajor>::Zero();
287  J_dba.block(0, 0, 3, 3) = -sqrt_inf_ba;
288  }
289 
290  if (jacobians[1] != NULL) {
291  Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> J_dbg(jacobians[1]);
292  J_dbg = Eigen::Matrix<double, 6, 3, Eigen::RowMajor>::Zero();
293  J_dbg.block(3, 0, 3, 3) = -sqrt_inf_bg;
294  }
295 
296  if (jacobians[2] != NULL) {
297  Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> J_dba(jacobians[2]);
298  J_dba = Eigen::Matrix<double, 6, 3, Eigen::RowMajor>::Zero();
299  J_dba.block(0, 0, 3, 3) = sqrt_inf_ba;
300  }
301 
302  if (jacobians[3] != NULL) {
303  Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> J_dbg(jacobians[3]);
304  J_dbg = Eigen::Matrix<double, 6, 3, Eigen::RowMajor>::Zero();
305  J_dbg.block(3, 0, 3, 3) = sqrt_inf_bg;
306  }
307  }
308 
309  return true;
310  }
311 
312  std::shared_ptr<IMU> _imu_i, _imu_j;
313 };
314 
319 class IMUFactorInit : public ceres::SizedCostFunction<9, 2, 3, 3, 3, 3, 1> {
320  public:
321  IMUFactorInit(const std::shared_ptr<IMU> imu_i, const std::shared_ptr<IMU> imu_j) : _imu_i(imu_i), _imu_j(imu_j) {}
322 
323  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
324 
325  // Beware: the assumption here is that the IMU frame is the main frame
326  Eigen::Vector3d w_w_i = Eigen::Vector3d(parameters[0][0], parameters[0][1], 0);
327  Eigen::Matrix3d R_w_i = geometry::exp_so3(w_w_i);
328  Eigen::Vector3d v_i = _imu_i->getVelocity() + Eigen::Map<const Eigen::Vector3d>(parameters[1]);
329  Eigen::Vector3d v_j = _imu_j->getVelocity() + Eigen::Map<const Eigen::Vector3d>(parameters[2]);
330  Eigen::Vector3d d_ba = Eigen::Map<const Eigen::Vector3d>(parameters[3]);
331  Eigen::Vector3d d_bg = Eigen::Map<const Eigen::Vector3d>(parameters[4]);
332  double lambda = *parameters[5];
333  Eigen::Affine3d T_fi_w = _imu_i->getFrame()->getWorld2FrameTransform();
334  Eigen::Affine3d T_fj_w = _imu_j->getFrame()->getWorld2FrameTransform();
335  double dtij = (_imu_j->getFrame()->getTimestamp() - _imu_i->getFrame()->getTimestamp()) * 1e-9;
336 
337  // Get the information matrix
338  Eigen::MatrixXd cov = _imu_j->getCov();
339  Eigen::Matrix<double, 9, 9> inf_sqrt =
340  Eigen::LLT<Eigen::Matrix<double, 9, 9>>(cov.inverse()).matrixL().transpose();
341 
342  // Derive the residuals
343  Eigen::Matrix3d dR = (_imu_j->getDeltaR() * geometry::exp_so3(_imu_j->_J_dR_bg * d_bg)).transpose() *
344  T_fi_w.rotation() * T_fj_w.rotation().transpose();
345  Eigen::Vector3d r_dr = geometry::log_so3(dR);
346  Eigen::Vector3d r_dv = T_fi_w.rotation() * R_w_i * ((v_j - v_i) - g * dtij) -
347  (_imu_j->getDeltaV() + _imu_j->_J_dv_bg * d_bg + _imu_j->_J_dv_ba * d_ba);
348  Eigen::Vector3d r_dp =
349  T_fi_w.rotation() * R_w_i *
350  (std::exp(lambda) * (T_fj_w.inverse().translation() - T_fi_w.inverse().translation()) - v_i * dtij -
351  0.5 * g * dtij * dtij) -
352  (_imu_j->getDeltaP() + _imu_j->_J_dp_bg * d_bg + _imu_j->_J_dp_ba * d_ba);
353  Eigen::Map<Eigen::Matrix<double, 9, 1>> err(residuals);
354  err.block(0, 0, 3, 1) = r_dr;
355  err.block(3, 0, 3, 1) = r_dv;
356  err.block(6, 0, 3, 1) = r_dp;
357  err = inf_sqrt * err;
358 
359  if (jacobians != NULL) {
360 
361  // Jacobian wrt the gravity orientation
362  if (jacobians[0] != NULL) {
363  Eigen::Map<Eigen::Matrix<double, 9, 2, Eigen::RowMajor>> J_Rwi(jacobians[0]);
364  J_Rwi = Eigen::Matrix<double, 9, 2, Eigen::RowMajor>::Zero();
365  J_Rwi.block(3, 0, 3, 2) = -T_fi_w.rotation() * R_w_i * geometry::skewMatrix((v_j - v_i) - g * dtij) *
366  geometry::so3_rightJacobian(w_w_i).block(0, 0, 3, 2);
367  J_Rwi.block(6, 0, 3, 2) = -T_fi_w.rotation() * R_w_i *
368  geometry::skewMatrix(std::exp(lambda) * (T_fj_w.inverse().translation() -
369  T_fi_w.inverse().translation()) -
370  v_i * dtij - 0.5 * g * dtij * dtij) *
371  geometry::so3_rightJacobian(w_w_i).block(0, 0, 3, 2);
372  J_Rwi = inf_sqrt * J_Rwi;
373  }
374 
375  // Jacobian wrt the velocity i
376  if (jacobians[1] != NULL) {
377  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_vi(jacobians[1]);
378  J_vi = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
379  J_vi.block(3, 0, 3, 3) = -(T_fi_w.rotation() * R_w_i);
380  J_vi.block(6, 0, 3, 3) = -(T_fi_w.rotation() * R_w_i) * dtij;
381  J_vi = inf_sqrt * J_vi;
382  }
383 
384  // Jacobian wrt the velocity j
385  if (jacobians[2] != NULL) {
386  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_vj(jacobians[2]);
387  J_vj = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
388  J_vj.block(3, 0, 3, 3) = T_fi_w.rotation() * R_w_i;
389  J_vj = inf_sqrt * J_vj;
390  }
391 
392  // Jacobian wrt the bias of the accelerometer
393  if (jacobians[3] != NULL) {
394  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dba(jacobians[3]);
395  J_dba = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
396  J_dba.block(3, 0, 3, 3) = -_imu_j->_J_dv_ba;
397  J_dba.block(6, 0, 3, 3) = -_imu_j->_J_dp_ba;
398  J_dba = inf_sqrt * J_dba;
399  }
400 
401  // Jacobian wrt the bias of the gyroscope
402  if (jacobians[4] != NULL) {
403  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dbg(jacobians[4]);
404  J_dbg = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
405  J_dbg.block(0, 0, 3, 3) = -geometry::so3_rightJacobian(r_dr).inverse() * dR.transpose() *
406  geometry::so3_rightJacobian(_imu_j->_J_dR_bg * d_bg) * _imu_j->_J_dR_bg;
407  J_dbg.block(3, 0, 3, 3) = -_imu_j->_J_dv_bg;
408  J_dbg.block(6, 0, 3, 3) = -_imu_j->_J_dp_bg;
409  J_dbg = inf_sqrt * J_dbg;
410  }
411 
412  // Jacobian wrt the scale
413  if (jacobians[5] != NULL) {
414  Eigen::Map<Eigen::Matrix<double, 9, 1>> J_scale(jacobians[5]);
415  J_scale = Eigen::Matrix<double, 9, 1>::Zero();
416  J_scale.block(6, 0, 3, 1) =
417  T_fi_w.rotation() * R_w_i * (T_fj_w.inverse().translation() - T_fi_w.inverse().translation());
418  J_scale = inf_sqrt * J_scale;
419  }
420  }
421 
422  return true;
423  }
424 
425  std::shared_ptr<IMU> _imu_i, _imu_j;
426 };
427 
431 class IMUFactorInitBis : public ceres::SizedCostFunction<9, 2, 3, 3, 1> {
432  public:
433  IMUFactorInitBis(const std::shared_ptr<IMU> imu_i, const std::shared_ptr<IMU> imu_j)
434  : _imu_i(imu_i), _imu_j(imu_j) {}
435 
436  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
437 
438  // Beware: the assumption here is that the IMU frame is the main frame
439  Eigen::Vector3d w_w_i = Eigen::Vector3d(parameters[0][0], parameters[0][1], 0);
440  Eigen::Matrix3d R_w_i = geometry::exp_so3(w_w_i);
441  Eigen::Vector3d d_ba = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
442  Eigen::Vector3d d_bg = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
443  double lambda = *parameters[3];
444  Eigen::Vector3d v_i = _imu_i->getVelocity();
445  Eigen::Vector3d v_j = _imu_j->getVelocity();
446  Eigen::Affine3d T_fi_w = _imu_i->getFrame()->getWorld2FrameTransform();
447  Eigen::Affine3d T_fj_w = _imu_j->getFrame()->getWorld2FrameTransform();
448  double dtij = (_imu_j->getFrame()->getTimestamp() - _imu_i->getFrame()->getTimestamp()) * 1e-9;
449 
450  // Get the information matrix
451  Eigen::MatrixXd cov = _imu_j->getCov();
452  Eigen::Matrix<double, 9, 9> inf_sqrt =
453  Eigen::LLT<Eigen::Matrix<double, 9, 9>>(cov.inverse()).matrixL().transpose();
454 
455  // Derive the residuals
456  Eigen::Matrix3d dR = (_imu_j->getDeltaR() * geometry::exp_so3(_imu_j->_J_dR_bg * d_bg)).transpose() *
457  T_fi_w.rotation() * T_fj_w.rotation().transpose();
458  Eigen::Vector3d r_dr = geometry::log_so3(dR);
459  Eigen::Vector3d r_dv = T_fi_w.rotation() * R_w_i * (std::exp(lambda) * (v_j - v_i) - g * dtij) -
460  (_imu_j->getDeltaV() + _imu_j->_J_dv_bg * d_bg + _imu_j->_J_dv_ba * d_ba);
461  Eigen::Vector3d r_dp =
462  T_fi_w.rotation() * R_w_i *
463  (std::exp(lambda) * (T_fj_w.inverse().translation() - T_fi_w.inverse().translation()) -
464  std::exp(lambda) * v_i * dtij - 0.5 * g * dtij * dtij) -
465  (_imu_j->getDeltaP() + _imu_j->_J_dp_bg * d_bg + _imu_j->_J_dp_ba * d_ba);
466  Eigen::Map<Eigen::Matrix<double, 9, 1>> err(residuals);
467  err.block(0, 0, 3, 1) = r_dr;
468  err.block(3, 0, 3, 1) = r_dv;
469  err.block(6, 0, 3, 1) = r_dp;
470  err = inf_sqrt * err;
471 
472  if (jacobians != NULL) {
473 
474  // Jacobian wrt the gravity orientation
475  if (jacobians[0] != NULL) {
476  Eigen::Map<Eigen::Matrix<double, 9, 2, Eigen::RowMajor>> J_Rwi(jacobians[0]);
477  J_Rwi = Eigen::Matrix<double, 9, 2, Eigen::RowMajor>::Zero();
478  J_Rwi.block(3, 0, 3, 2) = -T_fi_w.rotation() * R_w_i *
479  geometry::skewMatrix(std::exp(lambda) * (v_j - v_i) - g * dtij) *
480  geometry::so3_rightJacobian(w_w_i).block(0, 0, 3, 2);
481  J_Rwi.block(6, 0, 3, 2) =
482  -T_fi_w.rotation() * R_w_i *
483  geometry::skewMatrix(std::exp(lambda) * (T_fj_w.inverse().translation() -
484  T_fi_w.inverse().translation() - v_i * dtij) -
485  0.5 * g * dtij * dtij) *
486  geometry::so3_rightJacobian(w_w_i).block(0, 0, 3, 2);
487  J_Rwi = inf_sqrt * J_Rwi;
488  }
489 
490  // Jacobian wrt the bias of the accelerometer
491  if (jacobians[1] != NULL) {
492  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dba(jacobians[1]);
493  J_dba = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
494  J_dba.block(3, 0, 3, 3) = -_imu_j->_J_dv_ba;
495  J_dba.block(6, 0, 3, 3) = -_imu_j->_J_dp_ba;
496  J_dba = inf_sqrt * J_dba;
497  }
498 
499  // Jacobian wrt the bias of the gyroscope
500  if (jacobians[2] != NULL) {
501  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::RowMajor>> J_dbg(jacobians[2]);
502  J_dbg = Eigen::Matrix<double, 9, 3, Eigen::RowMajor>::Zero();
503  J_dbg.block(0, 0, 3, 3) = -geometry::so3_rightJacobian(r_dr).inverse() * dR.transpose() *
504  geometry::so3_rightJacobian(_imu_j->_J_dR_bg * d_bg) * _imu_j->_J_dR_bg;
505  J_dbg.block(3, 0, 3, 3) = -_imu_j->_J_dv_bg;
506  J_dbg.block(6, 0, 3, 3) = -_imu_j->_J_dp_bg;
507  J_dbg = inf_sqrt * J_dbg;
508  }
509 
510  // Jacobian wrt the scale
511  if (jacobians[3] != NULL) {
512  Eigen::Map<Eigen::Matrix<double, 9, 1>> J_scale(jacobians[3]);
513  J_scale = Eigen::Matrix<double, 9, 1>::Zero();
514  J_scale.block(3, 0, 3, 1) = T_fi_w.rotation() * R_w_i * (v_j - v_i);
515  J_scale.block(6, 0, 3, 1) =
516  T_fi_w.rotation() * R_w_i *
517  (T_fj_w.inverse().translation() - T_fi_w.inverse().translation() - v_i * dtij);
518  }
519  }
520 
521  return true;
522  }
523 
524  std::shared_ptr<IMU> _imu_i, _imu_j;
525 };
526 
530 class Landmark3DPrior : public ceres::SizedCostFunction<3, 3> {
531  public:
532  Landmark3DPrior(const Eigen::Vector3d prior, const Eigen::Vector3d lmk, const Eigen::Matrix3d sqrt_inf)
533  : _prior(prior), _lmk(lmk), _sqrt_inf(sqrt_inf) {}
534  Landmark3DPrior() {}
535 
536  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
537  Eigen::Map<Eigen::Vector3d> err(residuals);
538  err = _sqrt_inf * (_lmk + Eigen::Map<const Eigen::Vector3d>(parameters[0]) - _prior);
539 
540  if (jacobians != NULL) {
541  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J(jacobians[0]);
542  J = _sqrt_inf;
543  }
544 
545  return true;
546  }
547 
548  Eigen::Vector3d _prior, _lmk;
549  Eigen::Matrix3d _sqrt_inf;
550 };
551 
555 class LandmarkToLandmarkFactor : public ceres::SizedCostFunction<3, 3, 3> {
556  public:
557  LandmarkToLandmarkFactor(const Eigen::Vector3d delta,
558  const Eigen::Vector3d lmk0,
559  const Eigen::Vector3d lmk1,
560  const Eigen::Matrix3d sqrt_inf)
561  : _delta(delta), _lmk0(lmk0), _lmk1(lmk1), _sqrt_inf(sqrt_inf) {}
562  LandmarkToLandmarkFactor() {}
563 
564  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
565  Eigen::Map<Eigen::Vector3d> err(residuals);
566  err = _sqrt_inf * ((_lmk0 + Eigen::Map<const Eigen::Vector3d>(parameters[0])) -
567  (_lmk1 + Eigen::Map<const Eigen::Vector3d>(parameters[1])) - _delta);
568 
569  if (jacobians != NULL) {
570 
571  if (jacobians[0] != NULL) {
572  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J_l0(jacobians[0]);
573  J_l0 = _sqrt_inf;
574  }
575 
576  if (jacobians[1] != NULL) {
577  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J_l1(jacobians[1]);
578  J_l1 = -_sqrt_inf;
579  }
580  }
581 
582  return true;
583  }
584  Eigen::Vector3d _delta, _lmk0, _lmk1;
585  Eigen::Matrix3d _sqrt_inf;
586 };
587 
591 class PoseToLandmarkFactor : public ceres::SizedCostFunction<3, 6, 3> {
592  public:
593  PoseToLandmarkFactor(const Eigen::Vector3d delta,
594  const Eigen::Affine3d T_f_w,
595  const Eigen::Vector3d t_w_lmk,
596  const Eigen::Matrix3d sqrt_inf)
597  : _delta(delta), _t_w_lmk(t_w_lmk), _T_f_w(T_f_w), _sqrt_inf(sqrt_inf) {}
598  PoseToLandmarkFactor() {}
599 
600  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
601  Eigen::Map<Eigen::Vector3d> err(residuals);
602  Eigen::Affine3d T_f_w = _T_f_w * geometry::se3_doubleVec6dtoRT(parameters[0]);
603  Eigen::Vector3d t_w_lmk = _t_w_lmk + Eigen::Map<const Eigen::Vector3d>(parameters[1]);
604 
605  err = _sqrt_inf * (T_f_w * t_w_lmk - _delta);
606 
607  if (jacobians != NULL) {
608 
609  if (jacobians[0] != NULL) {
610  Eigen::Map<Eigen::Matrix<double, 3, 6, Eigen::RowMajor>> J_f(jacobians[0]);
611  Eigen::Vector3d w_df = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
612  Eigen::Matrix3d dR = geometry::exp_so3(w_df);
613  J_f.block(0, 0, 3, 3) = _sqrt_inf * _T_f_w.rotation() *
614  (-dR * geometry::skewMatrix(t_w_lmk) * geometry::so3_rightJacobian(w_df));
615  J_f.block(0, 3, 3, 3) = _sqrt_inf * _T_f_w.rotation();
616  }
617 
618  if (jacobians[1] != NULL) {
619  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J_l(jacobians[1]);
620  J_l = _sqrt_inf * T_f_w.rotation();
621  }
622  }
623 
624  return true;
625  }
626  Eigen::Vector3d _delta, _t_w_lmk;
627  Eigen::Affine3d _T_f_w;
628  Eigen::Matrix3d _sqrt_inf;
629 };
630 
634 class PosePriordx : public ceres::SizedCostFunction<6, 6> {
635  public:
636  PosePriordx(const Eigen::Affine3d T, const Eigen::Affine3d T_prior, const Eigen::MatrixXd sqrt_inf)
637  : _T(T), _T_prior(T_prior), _sqrt_inf(sqrt_inf) {}
638  PosePriordx() {}
639 
640  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
641  Eigen::Map<Vector6d> err(residuals);
642  Eigen::Affine3d T = _T * geometry::se3_doubleVec6dtoRT(parameters[0]);
643  err = _sqrt_inf * geometry::se3_RTtoVec6d(T * _T_prior.inverse());
644 
645  if (jacobians != NULL) {
646  Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobians[0]);
647  J.setIdentity();
648  Eigen::Vector3d dw = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
649 
650  Eigen::Vector3d w = geometry::log_so3(T.rotation() * _T_prior.rotation().transpose());
651  J.block(0, 0, 3, 3) =
652  geometry::so3_rightJacobian(w).inverse() * _T_prior.rotation() * geometry::so3_rightJacobian(dw);
653  J.block(3, 0, 3, 3) = T.rotation() *
654  geometry::skewMatrix(_T_prior.rotation().transpose() * _T_prior.translation()) *
655  geometry::so3_rightJacobian(dw);
656  J.block(3, 3, 3, 3) = _T.rotation();
657  J = _sqrt_inf * J;
658  }
659 
660  return true;
661  }
662 
663  Eigen::Affine3d _T, _T_prior;
664  Eigen::MatrixXd _sqrt_inf;
665 };
666 
670 class IMUPriordx : public ceres::SizedCostFunction<15, 6, 3, 3, 3> {
671  public:
672  IMUPriordx(const Eigen::Affine3d T,
673  const Eigen::Affine3d T_prior,
674  const Eigen::Vector3d v,
675  const Eigen::Vector3d v_prior,
676  const Eigen::Vector3d ba,
677  const Eigen::Vector3d ba_prior,
678  const Eigen::Vector3d bg,
679  const Eigen::Vector3d bg_prior,
680  const Eigen::MatrixXd sqrt_inf)
681  : _T(T), _T_prior(T_prior), _v(v), _v_prior(v_prior), _ba(ba), _ba_prior(ba_prior), _bg(bg),
682  _bg_prior(bg_prior), _sqrt_inf(sqrt_inf) {}
683  IMUPriordx() {}
684 
685  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
686  Eigen::Map<Eigen::Matrix<double, 15, 1>> err(residuals);
687  Eigen::Affine3d T = _T * geometry::se3_doubleVec6dtoRT(parameters[0]);
688  err.block(0, 0, 6, 1) = geometry::se3_RTtoVec6d(T * _T_prior.inverse());
689  err.block(6, 0, 3, 1) = _v + Eigen::Map<const Eigen::Vector3d>(parameters[1]) - _v_prior;
690  err.block(9, 0, 3, 1) = _ba + Eigen::Map<const Eigen::Vector3d>(parameters[2]) - _ba_prior;
691  err.block(12, 0, 3, 1) = _bg + Eigen::Map<const Eigen::Vector3d>(parameters[3]) - _bg_prior;
692  err = _sqrt_inf * err;
693 
694  if (jacobians != NULL) {
695 
696  if (jacobians[0] != NULL) {
697  Eigen::Map<Eigen::Matrix<double, 15, 6, Eigen::RowMajor>> J(jacobians[0]);
698  J.setZero();
699  Eigen::Vector3d dw = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
700 
701  Eigen::Vector3d w = geometry::log_so3(T.rotation() * _T_prior.rotation().transpose());
702  J.block(0, 0, 3, 3) =
703  geometry::so3_rightJacobian(w).inverse() * _T_prior.rotation() * geometry::so3_rightJacobian(dw);
704  J.block(3, 0, 3, 3) = T.rotation() *
705  geometry::skewMatrix(_T_prior.rotation().transpose() * _T_prior.translation()) *
706  geometry::so3_rightJacobian(dw);
707  J.block(3, 3, 3, 3) = _T.rotation();
708  J = _sqrt_inf * J;
709  }
710 
711  if (jacobians[1] != NULL) {
712  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_v(jacobians[1]);
713  J_v.setZero();
714  J_v.block(6, 0, 3, 3) = Eigen::Matrix3d::Identity();
715  }
716 
717  if (jacobians[2] != NULL) {
718  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_ba(jacobians[2]);
719  J_ba.setZero();
720  J_ba.block(9, 0, 3, 3) = Eigen::Matrix3d::Identity();
721  }
722 
723  if (jacobians[3] != NULL) {
724  Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> J_bg(jacobians[3]);
725  J_bg.setZero();
726  J_bg.block(12, 0, 3, 3) = Eigen::Matrix3d::Identity();
727  }
728  }
729 
730  return true;
731  }
732 
733  Eigen::Affine3d _T, _T_prior;
734  Eigen::Vector3d _v, _v_prior, _ba, _ba_prior, _bg, _bg_prior;
735  Eigen::MatrixXd _sqrt_inf;
736 };
737 
741 class scalePrior : public ceres::SizedCostFunction<1, 1> {
742  public:
743  scalePrior(const double sqrt_inf) : _sqrt_inf(sqrt_inf) {}
744  scalePrior() {}
745 
746  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
747  residuals[0] = _sqrt_inf * (1 - parameters[0][0]);
748 
749  if (jacobians != NULL) {
750  jacobians[0][0] = -_sqrt_inf;
751  }
752 
753  return true;
754  }
755  double _sqrt_inf;
756 };
757 
758 } // namespace isae
759 
760 #endif // RESIDUALS_H
isae::geometry::log_so3
Eigen::Vector3d log_so3(const Eigen::Matrix3d &M)
Compute the logarithm of the SO(3) manifold.
Definition: geometry.h:178
isae::IMUFactor::IMUFactor
IMUFactor(const std::shared_ptr< IMU > imu_i, const std::shared_ptr< IMU > imu_j)
Definition: residuals.hpp:147
isae::PosePriordx::PosePriordx
PosePriordx(const Eigen::Affine3d T, const Eigen::Affine3d T_prior, const Eigen::MatrixXd sqrt_inf)
Definition: residuals.hpp:636
isae::IMUFactor::_imu_j
std::shared_ptr< IMU > _imu_j
The first and the last IMU of the pre-integration.
Definition: residuals.hpp:255
isae::LandmarkToLandmarkFactor
3D relative translation factor between 3D punctual landmarks
Definition: residuals.hpp:555
isae::IMUFactorInitBis::_imu_i
std::shared_ptr< IMU > _imu_i
Definition: residuals.hpp:524
isae::IMUBiasFactor::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:266
isae::geometry::se3_doubleVec6dtoRT
Eigen::Affine3d se3_doubleVec6dtoRT(const double *pose)
Compute the Exponential of the SO(3)xT(3) composite manifold from a double*.
Definition: geometry.h:238
isae::IMUPriordx::_ba_prior
Eigen::Vector3d _ba_prior
Definition: residuals.hpp:734
isae::LandmarkToLandmarkFactor::_delta
Eigen::Vector3d _delta
Definition: residuals.hpp:584
isae::Motion2DFactor::_dx
double _dx
The forward displacement.
Definition: residuals.hpp:69
isae::Motion2DFactor
A cost function to impose a constraint on 2D motion.
Definition: residuals.hpp:13
isae::IMUBiasFactor::IMUBiasFactor
IMUBiasFactor(const std::shared_ptr< IMU > imu_i, const std::shared_ptr< IMU > imu_j)
Definition: residuals.hpp:264
isae::Landmark3DPrior::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:536
isae::IMUPriordx::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:685
isae::Landmark3DPrior::_lmk
Eigen::Vector3d _lmk
Prior and current 3D positions.
Definition: residuals.hpp:548
isae::LandmarkToLandmarkFactor::LandmarkToLandmarkFactor
LandmarkToLandmarkFactor(const Eigen::Vector3d delta, const Eigen::Vector3d lmk0, const Eigen::Vector3d lmk1, const Eigen::Matrix3d sqrt_inf)
Definition: residuals.hpp:557
isae::geometry::skewMatrix
Eigen::Matrix3d skewMatrix(const Eigen::MatrixBase< Derived > &w)
Computes the skew matrix of a given 3D vector.
Definition: geometry.h:20
isae::PosePriordx::_sqrt_inf
Eigen::MatrixXd _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:664
isae::IMUPriordx::IMUPriordx
IMUPriordx(const Eigen::Affine3d T, const Eigen::Affine3d T_prior, const Eigen::Vector3d v, const Eigen::Vector3d v_prior, const Eigen::Vector3d ba, const Eigen::Vector3d ba_prior, const Eigen::Vector3d bg, const Eigen::Vector3d bg_prior, const Eigen::MatrixXd sqrt_inf)
Definition: residuals.hpp:672
isae::Motion2DFactor::Motion2DFactor
Motion2DFactor(const Eigen::Vector2d motion_2d, const Eigen::MatrixXd sqrt_inf, const double dt)
Definition: residuals.hpp:15
isae::PosePriordx::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:640
isae::Motion2DFactor::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:22
isae::IMUFactorInitBis::IMUFactorInitBis
IMUFactorInitBis(const std::shared_ptr< IMU > imu_i, const std::shared_ptr< IMU > imu_j)
Definition: residuals.hpp:433
isae::Landmark3DPrior::Landmark3DPrior
Landmark3DPrior(const Eigen::Vector3d prior, const Eigen::Vector3d lmk, const Eigen::Matrix3d sqrt_inf)
Definition: residuals.hpp:532
isae::scalePrior::_sqrt_inf
double _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:755
isae::Landmark3DPrior
A factor for absolute 3D position constraint on a punctual landmark.
Definition: residuals.hpp:530
isae::Landmark3DPrior::_prior
Eigen::Vector3d _prior
Definition: residuals.hpp:548
isae::PoseToLandmarkFactor::PoseToLandmarkFactor
PoseToLandmarkFactor(const Eigen::Vector3d delta, const Eigen::Affine3d T_f_w, const Eigen::Vector3d t_w_lmk, const Eigen::Matrix3d sqrt_inf)
Definition: residuals.hpp:593
isae::LandmarkToLandmarkFactor::_lmk0
Eigen::Vector3d _lmk0
Definition: residuals.hpp:584
isae::scalePrior::scalePrior
scalePrior()
Definition: residuals.hpp:744
isae::geometry::exp_so3
Eigen::Matrix3d exp_so3(const Eigen::Vector3d &v)
Compute the exponential of the SO(3) manifold.
Definition: geometry.h:154
isae::IMUPriordx::_sqrt_inf
Eigen::MatrixXd _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:735
isae::IMUFactorInitBis
An experimental IMU factor refining only roll, pitch, biases and scale.
Definition: residuals.hpp:431
isae::Motion2DFactor::_dtheta
double _dtheta
The angular displacement.
Definition: residuals.hpp:70
isae::IMUPriordx::_T_prior
Eigen::Affine3d _T_prior
Pose prior and current value.
Definition: residuals.hpp:733
isae::IMUBiasFactor::_imu_j
std::shared_ptr< IMU > _imu_j
The first and the last IMU of the pre-integration.
Definition: residuals.hpp:312
isae::Motion2DFactor::_motion_2d
Eigen::Vector2d _motion_2d
The velocity on the 2D plane.
Definition: residuals.hpp:66
isae::Relative6DPose::_sqrt_inf
Eigen::MatrixXd _sqrt_inf
Square root of the information matrix.
Definition: residuals.hpp:136
isae::IMUBiasFactor::_imu_i
std::shared_ptr< IMU > _imu_i
Definition: residuals.hpp:312
isae::PoseToLandmarkFactor::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:600
isae::Relative6DPose::_T_a_b_prior
Eigen::Affine3d _T_a_b_prior
All the poses needed.
Definition: residuals.hpp:135
isae::LandmarkToLandmarkFactor::LandmarkToLandmarkFactor
LandmarkToLandmarkFactor()
Definition: residuals.hpp:562
isae::IMUBiasFactor
IMU bias cost function that takes into account random walk behaviour.
Definition: residuals.hpp:261
isae::Relative6DPose::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:85
isae::IMUFactor
A cost function for IMU pre-integration factor, parameters are delta updates.
Definition: residuals.hpp:145
isae::PosePriordx::PosePriordx
PosePriordx()
Definition: residuals.hpp:638
isae::IMUFactorInit::IMUFactorInit
IMUFactorInit(const std::shared_ptr< IMU > imu_i, const std::shared_ptr< IMU > imu_j)
Definition: residuals.hpp:321
isae::PoseToLandmarkFactor
Relative 3D translation between a 6DoF frame and a punctual landmark.
Definition: residuals.hpp:591
isae::IMUPriordx
An absolute prior on an Inertial state, including pose, velocity and biases.
Definition: residuals.hpp:670
isae::PoseToLandmarkFactor::_delta
Eigen::Vector3d _delta
Definition: residuals.hpp:626
IMU.h
isae::IMUFactorInitBis::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:436
isae::scalePrior
Prior on a 1D parameter (e.g. scale)
Definition: residuals.hpp:741
isae::PoseToLandmarkFactor::_sqrt_inf
Eigen::Matrix3d _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:628
isae::geometry::se3_RTtoVec6d
Vector6d se3_RTtoVec6d(Eigen::Affine3d RT)
Compute the logarithm of the SO(3)xT(3) composite manifold.
Definition: geometry.h:203
isae::IMUPriordx::_bg_prior
Eigen::Vector3d _bg_prior
Prior and current value for velocity and biases.
Definition: residuals.hpp:734
isae::LandmarkToLandmarkFactor::_lmk1
Eigen::Vector3d _lmk1
Prior on relative translation and positions of the landmarks.
Definition: residuals.hpp:584
isae::Relative6DPose::Relative6DPose
Relative6DPose()
Definition: residuals.hpp:83
isae
Definition: AFeature2D.h:8
isae::IMUPriordx::IMUPriordx
IMUPriordx()
Definition: residuals.hpp:683
isae::PoseToLandmarkFactor::_t_w_lmk
Eigen::Vector3d _t_w_lmk
The prior delta and the landmark 3D position.
Definition: residuals.hpp:626
isae::Landmark3DPrior::Landmark3DPrior
Landmark3DPrior()
Definition: residuals.hpp:534
isae::Motion2DFactor::_sqrt_inf
Eigen::MatrixXd _sqrt_inf
The square root informatino matrix of the measurement.
Definition: residuals.hpp:67
Camera.h
isae::IMUPriordx::_bg
Eigen::Vector3d _bg
Definition: residuals.hpp:734
isae::IMUFactorInitBis::_imu_j
std::shared_ptr< IMU > _imu_j
The first and the last IMU of the pre-integration.
Definition: residuals.hpp:524
isae::IMUFactorInit::_imu_j
std::shared_ptr< IMU > _imu_j
The first and the last IMU of the pre-integration.
Definition: residuals.hpp:425
isae::Motion2DFactor::Motion2DFactor
Motion2DFactor()
Definition: residuals.hpp:20
isae::Motion2DFactor::_dt
double _dt
The integration time.
Definition: residuals.hpp:68
isae::PoseToLandmarkFactor::_T_f_w
Eigen::Affine3d _T_f_w
The pose of the linked frame.
Definition: residuals.hpp:627
isae::LandmarkToLandmarkFactor::_sqrt_inf
Eigen::Matrix3d _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:585
isae::IMUFactor::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:149
isae::IMUFactorInit
Pre integration IMU factor for initialization, the parameters optimized are: scale,...
Definition: residuals.hpp:319
isae::PosePriordx
Absolute 6DoF pose prior factor.
Definition: residuals.hpp:634
isae::IMUPriordx::_v_prior
Eigen::Vector3d _v_prior
Definition: residuals.hpp:734
isae::PosePriordx::_T_prior
Eigen::Affine3d _T_prior
The 6DoF pose prior and its current value.
Definition: residuals.hpp:663
isae::IMUFactor::_imu_i
std::shared_ptr< IMU > _imu_i
Definition: residuals.hpp:255
isae::Relative6DPose::_T_w_b
Eigen::Affine3d _T_w_b
Definition: residuals.hpp:135
isae::IMUPriordx::_v
Eigen::Vector3d _v
Definition: residuals.hpp:734
isae::IMUPriordx::_T
Eigen::Affine3d _T
Definition: residuals.hpp:733
isae::IMUFactorInit::_imu_i
std::shared_ptr< IMU > _imu_i
Definition: residuals.hpp:425
isae::geometry::so3_rightJacobian
Eigen::Matrix3d so3_rightJacobian(const Eigen::Vector3d &w)
Compute the right jacobian on SO(3) manifold.
Definition: geometry.h:42
isae::Relative6DPose::Relative6DPose
Relative6DPose(const Eigen::Affine3d T_w_a, const Eigen::Affine3d T_w_b, const Eigen::Affine3d T_a_b_prior, const Eigen::MatrixXd sqrt_inf)
Definition: residuals.hpp:78
isae::PoseToLandmarkFactor::PoseToLandmarkFactor
PoseToLandmarkFactor()
Definition: residuals.hpp:598
isae::scalePrior::scalePrior
scalePrior(const double sqrt_inf)
Definition: residuals.hpp:743
isae::IMUFactorInit::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:323
isae::IMUPriordx::_ba
Eigen::Vector3d _ba
Definition: residuals.hpp:734
isae::PosePriordx::_T
Eigen::Affine3d _T
Definition: residuals.hpp:663
isae::LandmarkToLandmarkFactor::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:564
isae::Landmark3DPrior::_sqrt_inf
Eigen::Matrix3d _sqrt_inf
Square root information matrix.
Definition: residuals.hpp:549
isae::Relative6DPose::_T_w_a
Eigen::Affine3d _T_w_a
Definition: residuals.hpp:135
isae::Relative6DPose
A cost function for 6DoF relative pose constraint, the parameters are the delta pose of the two relat...
Definition: residuals.hpp:76
isae::scalePrior::Evaluate
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
Definition: residuals.hpp:746
Generated by   doxygen 1.8.17