gtsam
navigation
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
{
30
PreintegratedRotationParams::print
(
s
);
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
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition:
NEON/PacketMath.h:3115
gtsam::PreintegratedRotationParams
Definition:
PreintegratedRotation.h:57
s
RealScalar s
Definition:
level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::PreintegrationParams
Definition:
PreintegrationParams.h:25
gtsam::equal_with_abs_tol
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
traits
Definition:
SFMdata.h:40
std
Definition:
BFloat16.h:88
gtsam::tol
const G double tol
Definition:
Group.h:79
gtsam.examples.ImuFactorISAM2Example.n_gravity
def n_gravity
Definition:
ImuFactorISAM2Example.py:31
pybind_wrapper_test_script.other
other
Definition:
pybind_wrapper_test_script.py:42
PreintegrationParams.h
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:12:46