gtsam 4.1.1
gtsam
DynamicsPriors.h
Go to the documentation of this file.
1
10#pragma once
11
14
15namespace gtsam {
16
17// Indices of relevant variables in the PoseRTV tangent vector:
18// [ rx ry rz tx ty tz vx vy vz ]
19static const size_t kRollIndex = 0;
20static const size_t kPitchIndex = 1;
21static const size_t kHeightIndex = 5;
22static const size_t kVelocityZIndex = 8;
23static const std::vector<size_t> kVelocityIndices = { 6, 7, 8 };
24
29struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
31
32 DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
33 : Base(key, kHeightIndex, height, model) {}
34};
35
41struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
43
45 DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
46 : Base(key, kRollIndex, wx, model) { }
47
50 : Base(key, kRollIndex, 0.0, model) { }
51};
52
58struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
60
61 VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
62 : Base(key, kVelocityIndices, vel, model) {}
63};
64
72
76 DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
77 : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex },
78 Vector::Unit(4, 0)*height, model) {}
79
83 DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
84 : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, constraint, model) {
85 assert(constraint.size() == 4);
86 }
87};
88
89} // \namespace gtsam
A simple prior factor that allows for setting a prior only on a part of linear parameters.
Pose3 with translational velocity.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
This is the base class for all factor types.
Definition: Factor.h:56
A class for a soft partial prior on any Lie type, with a mask over Expmap parameters.
Definition: PartialPriorFactor.h:38
Forces the value of the height (z) in a PoseRTV to a specific value.
Definition: DynamicsPriors.h:29
Forces the roll to a particular value - useful for flying robots Implied value is zero Dim: 1.
Definition: DynamicsPriors.h:41
DRollPrior(Key key, const gtsam::SharedNoiseModel &model)
Forces roll to zero.
Definition: DynamicsPriors.h:49
DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel &model)
allows for explicit roll parameterization - uses canonical coordinate
Definition: DynamicsPriors.h:45
Constrains the full velocity of a state to a particular value Useful for enforcing a stationary state...
Definition: DynamicsPriors.h:58
Ground constraint: forces the robot to be upright (no roll, pitch), a fixed height,...
Definition: DynamicsPriors.h:70
DGroundConstraint(Key key, const Vector &constraint, const gtsam::SharedNoiseModel &model)
Fully specify vector - use only for debugging.
Definition: DynamicsPriors.h:83
DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel &model)
Primary constructor allows for variable height of the "floor".
Definition: DynamicsPriors.h:76