DynamicsPriors.h
Go to the documentation of this file.
1 
10 #pragma once
11 
14 
15 namespace gtsam {
16 
17 // Indices of relevant variables in the PoseRTV tangent vector:
18 // [ rx ry rz tx ty tz vx vy vz ]
19 static const size_t kRollIndex = 0;
20 static const size_t kPitchIndex = 1;
21 static const size_t kHeightIndex = 5;
22 static const size_t kVelocityZIndex = 8;
23 static const std::vector<size_t> kVelocityIndices = { 6, 7, 8 };
24 
29 struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
31 
33  : Base(key, kHeightIndex, height, model) {}
34 };
35 
41 struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
43 
46  : Base(key, kRollIndex, wx, model) { }
47 
50  : Base(key, kRollIndex, 0.0, model) { }
51 };
52 
58 struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
60 
62  : Base(key, kVelocityIndices, vel, model) {}
63 };
64 
70 struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
72 
77  : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex },
78  Vector::Unit(4, 0)*height, model) {}
79 
84  : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, constraint, model) {
85  assert(constraint.size() == 4);
86  }
87 };
88 
89 } // \namespace gtsam
static const std::vector< size_t > kVelocityIndices
static const size_t kPitchIndex
gtsam::PartialPriorFactor< PoseRTV > Base
noiseModel::Diagonal::shared_ptr model
static const Velocity3 vel(0.4, 0.5, 0.6)
gtsam::PartialPriorFactor< PoseRTV > Base
static const size_t kVelocityZIndex
DRollPrior(Key key, const gtsam::SharedNoiseModel &model)
DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel &model)
A simple prior factor that allows for setting a prior only on a part of linear parameters.
Eigen::VectorXd Vector
Definition: Vector.h:38
static const size_t kRollIndex
DGroundConstraint(Key key, const Vector &constraint, const gtsam::SharedNoiseModel &model)
VelocityPrior(Key key, const gtsam::Vector &vel, const gtsam::SharedNoiseModel &model)
gtsam::PartialPriorFactor< PoseRTV > Base
Pose3 with translational velocity.
traits
Definition: chartTesting.h:28
DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel &model)
static const size_t kHeightIndex
DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel &model)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
gtsam::PartialPriorFactor< PoseRTV > Base
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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