DynamicsPriors.h

Go to the documentation of this file.

#pragma once

#include <gtsam_unstable/slam/PartialPriorFactor.h>

#include <gtsam_unstable/dynamics/PoseRTV.h>

namespace gtsam {

struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
  DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
  : Base(key, 5, height, model)  {  }
};

struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;

  DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
  : Base(key, 0, wx, model)  {  }

  DRollPrior(Key key, const gtsam::SharedNoiseModel& model)
  : Base(key, 0, 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, model) {
    this->prior_ = vel;
    assert(vel.size() == 3);
    this->mask_.resize(3);
    this->mask_[0] = 6;
    this->mask_[1] = 7;
    this->mask_[2] = 8;
    this->H_ = Matrix::Zero(3, 9);
    this->fillH();
  }
};

struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
  typedef gtsam::PartialPriorFactor<PoseRTV> Base;

  DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
  : Base(key, model) {
    this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch]
    this->mask_.resize(4);
    this->mask_[0] = 5; // z = height
    this->mask_[1] = 8; // vz
    this->mask_[2] = 0; // roll
    this->mask_[3] = 1; // pitch
    this->H_ = Matrix::Zero(3, 9);
    this->fillH();
  }

  DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
  : Base(key, model) {
    assert(constraint.size() == 4);
    this->prior_ = constraint; // [z, vz, roll, pitch]
    this->mask_.resize(4);
    this->mask_[0] = 5; // z = height
    this->mask_[1] = 8; // vz
    this->mask_[2] = 0; // roll
    this->mask_[3] = 1; // pitch
    this->H_ = Matrix::Zero(3, 9);
    this->fillH();
  }
};

} // \namespace gtsam