CombinedImuFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
24 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
25 #include <boost/serialization/export.hpp>
26 #endif
27 
28 /* External or standard includes */
29 #include <ostream>
30 
31 namespace gtsam {
32 
33 using namespace std;
34 
35 //------------------------------------------------------------------------------
36 // Inner class PreintegrationCombinedParams
37 //------------------------------------------------------------------------------
38 void PreintegrationCombinedParams::print(const string& s) const {
40  cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
41  << endl;
42  cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
43  << endl;
44  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
45  << endl;
46 }
47 
48 //------------------------------------------------------------------------------
50  double tol) const {
51  auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
52  return e != nullptr && PreintegrationParams::equals(other, tol) &&
53  equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
54  tol) &&
55  equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
56  tol) &&
57  equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
58 }
59 
60 //------------------------------------------------------------------------------
61 // Inner class PreintegratedCombinedMeasurements
62 //------------------------------------------------------------------------------
63 void PreintegratedCombinedMeasurements::print(const string& s) const {
65  cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
66 }
67 
68 //------------------------------------------------------------------------------
70  const PreintegratedCombinedMeasurements& other, double tol) const {
71  return PreintegrationType::equals(other, tol)
72  && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
73 }
74 
75 //------------------------------------------------------------------------------
77  // Base class method to reset the preintegrated measurements
79  preintMeasCov_.setZero();
80 }
81 
82 //------------------------------------------------------------------------------
84  const gtsam::Matrix6& Q_init) {
85  // Base class method to reset the preintegrated measurements
87  p().biasAccOmegaInt = Q_init;
88  preintMeasCov_.setZero();
89 }
90 
91 //------------------------------------------------------------------------------
92 // sugar for derivative blocks
93 #define D_R_R(H) (H)->block<3,3>(0,0)
94 #define D_R_t(H) (H)->block<3,3>(0,3)
95 #define D_R_v(H) (H)->block<3,3>(0,6)
96 #define D_t_R(H) (H)->block<3,3>(3,0)
97 #define D_t_t(H) (H)->block<3,3>(3,3)
98 #define D_t_v(H) (H)->block<3,3>(3,6)
99 #define D_v_R(H) (H)->block<3,3>(6,0)
100 #define D_v_t(H) (H)->block<3,3>(6,3)
101 #define D_v_v(H) (H)->block<3,3>(6,6)
102 #define D_a_a(H) (H)->block<3,3>(9,9)
103 #define D_g_g(H) (H)->block<3,3>(12,12)
104 
105 //------------------------------------------------------------------------------
107  const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
108  if (dt <= 0) {
109  throw std::runtime_error(
110  "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
111  }
112 
113  // Update preintegrated measurements.
114  Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
115  Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
116  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
117 
118  // Update preintegrated measurements covariance: as in [2] we consider a first
119  // order propagation that can be seen as a prediction phase in an EKF
120  // framework. In this implementation, in contrast to [2], we consider the
121  // uncertainty of the bias selection and we keep correlation between biases
122  // and preintegrated measurements
123 
124  // Single Jacobians to propagate covariance
125  Matrix3 theta_H_omega = C.topRows<3>();
126  Matrix3 pos_H_acc = B.middleRows<3>(3);
127  Matrix3 vel_H_acc = B.bottomRows<3>();
128 
129  Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
130  Matrix3 pos_H_biasAccInit = -pos_H_acc;
131  Matrix3 vel_H_biasAccInit = -vel_H_acc;
132 
133  // overall Jacobian wrt preintegrated measurements (df/dx)
135  F.setZero();
136  F.block<9, 9>(0, 0) = A;
137  F.block<3, 3>(0, 12) = theta_H_omega;
138  F.block<3, 3>(3, 9) = pos_H_acc;
139  F.block<3, 3>(6, 9) = vel_H_acc;
140  F.block<6, 6>(9, 9) = I_6x6;
141 
142  // Update the uncertainty on the state (matrix F in [4]).
143  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
144 
145  // propagate uncertainty
146  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
147  const Matrix3& aCov = p().accelerometerCovariance;
148  const Matrix3& wCov = p().gyroscopeCovariance;
149  const Matrix3& iCov = p().integrationCovariance;
150  const Matrix6& bInitCov = p().biasAccOmegaInt;
151 
152  // first order uncertainty propagation
153  // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
154  Eigen::Matrix<double, 15, 15> G_measCov_Gt;
155  G_measCov_Gt.setZero(15, 15);
156 
157  const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
158  const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
159  const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
160  const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
161 
162  // BLOCK DIAGONAL TERMS
163  D_R_R(&G_measCov_Gt) =
164  (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) //
165  +
166  (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
167 
168  D_t_t(&G_measCov_Gt) =
169  (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) //
170  + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
171  + (dt * iCov);
172 
173  D_v_v(&G_measCov_Gt) =
174  (vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) //
175  + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
176 
177  D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
178  D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
179 
180  // OFF BLOCK DIAGONAL TERMS
181  D_R_t(&G_measCov_Gt) =
182  theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
183  D_R_v(&G_measCov_Gt) =
184  theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
185  D_t_R(&G_measCov_Gt) =
186  pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
187  D_t_v(&G_measCov_Gt) =
188  (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) +
189  (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
190  D_v_R(&G_measCov_Gt) =
191  vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
192  D_v_t(&G_measCov_Gt) =
193  (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) +
194  (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
195 
196  preintMeasCov_.noalias() += G_measCov_Gt;
197 }
198 
199 //------------------------------------------------------------------------------
200 // CombinedImuFactor methods
201 //------------------------------------------------------------------------------
203  Key vel_j, Key bias_i, Key bias_j,
205  Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
206  pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
207 }
208 
209 //------------------------------------------------------------------------------
211  return std::static_pointer_cast<gtsam::NonlinearFactor>(
213 }
214 
215 //------------------------------------------------------------------------------
216 void CombinedImuFactor::print(const string& s,
217  const KeyFormatter& keyFormatter) const {
218  cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
219  << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
220  << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << ","
221  << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>())
222  << ")\n";
223  _PIM_.print(" preintegrated measurements:");
224  this->noiseModel_->print(" noise model: ");
225 }
226 
227 //------------------------------------------------------------------------------
229  const This* e = dynamic_cast<const This*>(&other);
230  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
231 }
232 
233 //------------------------------------------------------------------------------
235  const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
236  const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
239  OptionalMatrixType H5, OptionalMatrixType H6) const {
240 
241  // error wrt bias evolution model (random walk)
242  Matrix6 Hbias_i, Hbias_j;
243  Vector6 fbias = traits<imuBias::ConstantBias>::Between(bias_j, bias_i,
244  H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();
245 
246  Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
247  Matrix93 D_r_vel_i, D_r_vel_j;
248 
249  // error wrt preintegrated measurements
250  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
251  bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
252  H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
253 
254  // if we need the jacobians
255  if (H1) {
256  H1->resize(15, 6);
257  H1->block<9, 6>(0, 0) = D_r_pose_i;
258  // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
259  H1->block<6, 6>(9, 0).setZero();
260  }
261  if (H2) {
262  H2->resize(15, 3);
263  H2->block<9, 3>(0, 0) = D_r_vel_i;
264  // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
265  H2->block<6, 3>(9, 0).setZero();
266  }
267  if (H3) {
268  H3->resize(15, 6);
269  H3->block<9, 6>(0, 0) = D_r_pose_j;
270  // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
271  H3->block<6, 6>(9, 0).setZero();
272  }
273  if (H4) {
274  H4->resize(15, 3);
275  H4->block<9, 3>(0, 0) = D_r_vel_j;
276  // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
277  H4->block<6, 3>(9, 0).setZero();
278  }
279  if (H5) {
280  H5->resize(15, 6);
281  H5->block<9, 6>(0, 0) = D_r_bias_i;
282  // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
283  H5->block<6, 6>(9, 0) = Hbias_i;
284  }
285  if (H6) {
286  H6->resize(15, 6);
287  H6->block<9, 6>(0, 0).setZero();
288  // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
289  H6->block<6, 6>(9, 0) = Hbias_j;
290  }
291 
292  // overall error
293  Vector r(15);
294  r << r_Rpv, fbias; // vector of size 15
295  return r;
296 }
297 
298 //------------------------------------------------------------------------------
299 std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
300  f._PIM_.print("combined preintegrated measurements:\n");
301  os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
302  return os;
303 }
304 
305 } // namespace gtsam
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Key F(std::uint64_t j)
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Eigen::Vector3d Vector3
Definition: Vector.h:43
#define D_g_g(H)
bool equals(const PreintegratedRotationParams &other, double tol) const override
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
void print(const std::string &s="") const override
void print(const std::string &s="") const override
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: BFloat16.h:88
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
void resetIntegration() override
Re-initialize PreintegratedCombinedMeasurements.
void print(const std::string &s="Preintegrated Measurements:") const override
#define D_R_R(H)
#define D_v_t(H)
const double dt
bool equals(const PreintegratedCombinedMeasurements &expected, double tol=1e-9) const
equals
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
#define D_R_v(H)
#define D_v_R(H)
#define D_a_a(H)
Vector9 computeErrorAndJacobians(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 6 > H3={}, OptionalJacobian< 9, 3 > H4={}, OptionalJacobian< 9, 6 > H5={}) const
virtual void print(const std::string &s="") const
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
#define D_t_R(H)
BetweenFactor< Rot3 > Between
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
#define D_t_t(H)
static const Vector3 measuredAcc
Vector evaluateError(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, const imuBias::ConstantBias &bias_j, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const override
vector of errors
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const CombinedImuFactor &)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
#define D_t_v(H)
traits
Definition: chartTesting.h:28
PreintegratedCombinedMeasurements _PIM_
static const Vector3 measuredOmega(w, 0, 0)
SharedNoiseModel noiseModel_
ofstream os("timeSchurFactors.csv")
Eigen::Matrix< double, 15, 15 > preintMeasCov_
bool equals(const PreintegratedRotationParams &other, double tol) const override
std::shared_ptr< This > shared_ptr
float * p
const G double tol
Definition: Group.h:86
#define D_v_v(H)
#define D_R_t(H)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
v setZero(3)
bool equals(const ManifoldPreintegration &other, double tol) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:02