PreintegrationParams.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 
22 #include "PreintegrationParams.h"
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 //------------------------------------------------------------------------------
29 void PreintegrationParams::print(const string& s) const {
31  cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
32  << endl;
33  cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
34  << endl;
35  if (omegaCoriolis && use2ndOrderCoriolis)
36  cout << "Using 2nd-order Coriolis" << endl;
37  cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
38 }
39 
40 //------------------------------------------------------------------------------
41 bool PreintegrationParams::equals(const PreintegratedRotationParams& other,
42  double tol) const {
43  auto e = dynamic_cast<const PreintegrationParams*>(&other);
44  return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
45  use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
46  equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
47  tol) &&
48  equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
49  tol) &&
50  equal_with_abs_tol(n_gravity, e->n_gravity, tol);
51 }
52 
53 } // namespace gtsam
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
const G double tol
Definition: Group.h:86


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