gtsam  4.1.0
gtsam
DynamicsPriors.h
Go to the documentation of this file.
1 
10 #pragma once
11 
13 
15 
16 namespace gtsam {
17 
22 struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
24  DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
25  : Base(key, 5, height, model) { }
26 };
27 
33 struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
35 
37  DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
38  : Base(key, 0, wx, model) { }
39 
42  : Base(key, 0, 0.0, model) { }
43 };
44 
50 struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
52  VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
53  : Base(key, model) {
54  this->prior_ = vel;
55  assert(vel.size() == 3);
56  this->mask_.resize(3);
57  this->mask_[0] = 6;
58  this->mask_[1] = 7;
59  this->mask_[2] = 8;
60  this->H_ = Matrix::Zero(3, 9);
61  this->fillH();
62  }
63 };
64 
70 struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
72 
76  DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
77  : Base(key, model) {
78  this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch]
79  this->mask_.resize(4);
80  this->mask_[0] = 5; // z = height
81  this->mask_[1] = 8; // vz
82  this->mask_[2] = 0; // roll
83  this->mask_[3] = 1; // pitch
84  this->H_ = Matrix::Zero(3, 9);
85  this->fillH();
86  }
87 
91  DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
92  : Base(key, model) {
93  assert(constraint.size() == 4);
94  this->prior_ = constraint; // [z, vz, roll, pitch]
95  this->mask_.resize(4);
96  this->mask_[0] = 5; // z = height
97  this->mask_[1] = 8; // vz
98  this->mask_[2] = 0; // roll
99  this->mask_[3] = 1; // pitch
100  this->H_ = Matrix::Zero(3, 9);
101  this->fillH();
102  }
103 };
104 
105 } // \namespace gtsam
gtsam::DRollPrior::DRollPrior
DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel &model)
allows for explicit roll parameterization - uses canonical coordinate
Definition: DynamicsPriors.h:37
gtsam::DGroundConstraint::DGroundConstraint
DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel &model)
Primary constructor allows for variable height of the "floor".
Definition: DynamicsPriors.h:76
gtsam::PartialPriorFactor< PoseRTV >::mask_
std::vector< size_t > mask_
indices of values to constrain in compressed prior vector
Definition: PartialPriorFactor.h:54
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::DHeightPrior
Forces the value of the height in a PoseRTV to a specific value Dim: 1.
Definition: DynamicsPriors.h:22
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
PoseRTV.h
Pose3 with translational velocity.
gtsam::PartialPriorFactor
A class for a soft partial prior on any Lie type, with a mask over Expmap parameters.
Definition: PartialPriorFactor.h:40
gtsam::VelocityPrior
Constrains the full velocity of a state to a particular value Useful for enforcing a stationary state...
Definition: DynamicsPriors.h:50
gtsam::DRollPrior::DRollPrior
DRollPrior(Key key, const gtsam::SharedNoiseModel &model)
Forces roll to zero.
Definition: DynamicsPriors.h:41
gtsam::PartialPriorFactor< PoseRTV >::fillH
void fillH()
Constructs the jacobian matrix in place.
Definition: PartialPriorFactor.h:130
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
PartialPriorFactor.h
A simple prior factor that allows for setting a prior only on a part of linear parameters.
gtsam::DRollPrior
Forces the roll to a particular value - useful for flying robots Implied value is zero Dim: 1.
Definition: DynamicsPriors.h:33
gtsam::PartialPriorFactor< PoseRTV >::prior_
Vector prior_
measurement on tangent space parameters, in compressed form
Definition: PartialPriorFactor.h:53
gtsam::PartialPriorFactor< PoseRTV >::H_
Matrix H_
Constant Jacobian - computed at creation.
Definition: PartialPriorFactor.h:55
gtsam::DGroundConstraint
Ground constraint: forces the robot to be upright (no roll, pitch), a fixed height,...
Definition: DynamicsPriors.h:70
gtsam::DGroundConstraint::DGroundConstraint
DGroundConstraint(Key key, const Vector &constraint, const gtsam::SharedNoiseModel &model)
Fully specify vector - use only for debugging.
Definition: DynamicsPriors.h:91