DynamicsPriors.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <gtsam_unstable/slam/PartialPriorFactor.h>
 
#include <cassert>
 
namespace gtsam {
 
// Indices of relevant variables in the PoseRTV tangent vector:
// [ rx ry rz tx ty tz vx vy vz ]
static const size_t kRollIndex = 0;
static const size_t kPitchIndex = 1;
static const size_t kHeightIndex = 5;
static const size_t kVelocityZIndex = 8;
static const std::vector<size_t> kVelocityIndices = { 6, 7, 8 };
 
struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
 
  DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
  : Base(key, kHeightIndex, height, model)  {}
};
 
struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
 
  DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
      : Base(key, kRollIndex, wx, model)  {  }
 
  DRollPrior(Key key, const gtsam::SharedNoiseModel& model)
      : Base(key, kRollIndex, 0.0, model)  {  }
};
 
struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
 
  VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
      : Base(key, kVelocityIndices, vel, model) {}
};
 
struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
 
  DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
      : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex },
             Vector::Unit(4, 0)*height, model) {}
 
  DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
  : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, constraint, model) {
    assert(constraint.size() == 4);
  }
};
 
} // \namespace gtsam