Go to the documentation of this file.
27 using namespace gtsam;
30 0.31686, 0.51505, 0.79645,
32 0.41733, 0.67835, -0.60471);
55 double measurement_dt(0.1);
64 Vector measurement_acc(
Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
67 InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt,
world_g,
world_rho,
world_omega_earth,
model);
69 Rot3 R1(0.487316618, 0.125253866, 0.86419557,
70 0.580273724, 0.693095498, -0.427669306,
71 -0.652537293, 0.709880342, 0.265075427);
75 Rot3 R2(0.473618898, 0.119523052, 0.872582019,
76 0.609241153, 0.67099888, -0.422594037,
77 -0.636011287, 0.731761397, 0.244979388);
100 for(
size_t i = 0;
i < 100000; ++
i) {
Linear Factor Graph where all factors are Gaussians.
gtsam::Vector3 predictionErrorVel(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
gtsam::Pose3 predictionErrorPose(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const Vector3 world_g(0.0, 0.0, 9.81)
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5))
Pose2_ Expmap(const Vector3_ &xi)
Some functions to compute numerical derivatives.
const MatrixNN & matrix() const
Return matrix.
static const Vector3 world_rho(0.0, -1.5724e-05, 0.0)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
void tictoc_finishedIteration_()
std::shared_ptr< This > shared_ptr
shared_ptr to this class
void insert(Key j, const Vector &value)
noiseModel::Gaussian::shared_ptr SharedGaussian
noiseModel::Diagonal::shared_ptr model
void g(const string &key, int i)
static enum @1096 ordering
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
gtsam::Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471)
NonlinearFactorGraph graph
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() *ECEF_omega_earth)
std::uint64_t Key
Integer nonlinear key type.
Vector evaluateError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A non-templated config holding any types of Manifold-group elements.
Inertial navigation factor (velocity in the global frame)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:08