LeggedEstimatorFactors.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

  
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/navigation/LeggedEstimator.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
namespace gtsam {

inline int leggedFootBlockStart(size_t foot) {
  return 9 + 3 * static_cast<int>(foot);
}

inline Vector3 extendedPoseContactPrediction(const ExtendedPose3d& state,
                                             size_t footColumn,
                                             OptionalMatrixType H = {}) {
  Matrix36 prediction_H_pose;
  const Pose3 pose(state.rotation(), state.x(0));
  const Point3 foothold = state.x(footColumn);
  const Vector3 prediction = pose.transformTo(foothold, prediction_H_pose);
 
  if (H) {
    H->setZero(3, static_cast<Eigen::Index>(state.dim()));
    H->block(0, 0, 3, 6) = prediction_H_pose;
    const int start = leggedFootBlockStart(footColumn - 2);
    H->block(0, start, 3, 3) = I_3x3;
  }
 
  return prediction;
}

class ExtendedPoseContactFactor : public NoiseModelFactorN<ExtendedPose3d> {
  using Base = NoiseModelFactorN<ExtendedPose3d>;
 
 public:
  using Base::evaluateError;

  ExtendedPoseContactFactor(Key key, size_t footColumn,
                            const Point3& measurement,
                            const SharedNoiseModel& model)
      : Base(model, key), footColumn_(footColumn), measurement_(measurement) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new ExtendedPoseContactFactor(*this)));
  }

  Vector evaluateError(const ExtendedPose3d& state,
                       OptionalMatrixType H) const override {
    const Vector3 prediction =
        extendedPoseContactPrediction(state, footColumn_, H);
 
    return prediction - measurement_;
  }
 
 private:
  size_t footColumn_;
  Point3 measurement_;
};

class ExtendedPoseHeightFactor : public NoiseModelFactorN<ExtendedPose3d> {
  using Base = NoiseModelFactorN<ExtendedPose3d>;
 
 public:
  using Base::evaluateError;

  ExtendedPoseHeightFactor(Key key, size_t footColumn, double terrainHeight,
                           const SharedNoiseModel& model)
      : Base(model, key),
        footColumn_(footColumn),
        terrainHeight_(terrainHeight) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new ExtendedPoseHeightFactor(*this)));
  }

  Vector evaluateError(const ExtendedPose3d& state,
                       OptionalMatrixType H) const override {
    if (H) {
      H->setZero(1, static_cast<Eigen::Index>(state.dim()));
      const Matrix3 R = state.rotation().matrix();
      const int start = leggedFootBlockStart(footColumn_ - 2);
      H->block(0, start, 1, 3) = R.row(2);
    }
 
    return Vector1(state.x(footColumn_).z() - terrainHeight_);
  }
 
 private:
  size_t footColumn_;
  double terrainHeight_;
};

class NavStatePointContactFactor
    : public NoiseModelFactorT<Vector3, NavState, Point3> {
  using Base = NoiseModelFactorT<Vector3, NavState, Point3>;
 
 public:
  using Base::evaluateError;

  NavStatePointContactFactor(Key navKey, Key pointKey,
                             const Point3& measurement,
                             const SharedNoiseModel& model)
      : Base(model, navKey, pointKey), measurement_(measurement) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new NavStatePointContactFactor(*this)));
  }

  Vector3 evaluateError(const NavState& navState, const Point3& foothold,
                        OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    Matrix36 prediction_H_pose;
    Matrix3 prediction_H_foothold;
    const Vector3 prediction = navState.pose().transformTo(
        foothold, prediction_H_pose, prediction_H_foothold);
 
    if (H1) {
      H1->setZero(3, 9);
      H1->block(0, 0, 3, 6) = prediction_H_pose;
    }
    if (H2) {
      *H2 = prediction_H_foothold;
    }
 
    return prediction - measurement_;
  }
 
 private:
  Point3 measurement_;
};

class Pose3PointContactFactor
    : public NoiseModelFactorT<Vector3, Pose3, Point3> {
  using Base = NoiseModelFactorT<Vector3, Pose3, Point3>;
 
 public:
  using Base::evaluateError;

  Pose3PointContactFactor(Key poseKey, Key pointKey, const Point3& measurement,
                          const SharedNoiseModel& model)
      : Base(model, poseKey, pointKey), measurement_(measurement) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new Pose3PointContactFactor(*this)));
  }

  Vector3 evaluateError(const Pose3& pose, const Point3& foothold,
                        OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    return pose.transformTo(foothold, H1, H2) - measurement_;
  }
 
 private:
  Point3 measurement_;
};

class PointHeightFactor : public NoiseModelFactorN<Point3> {
  using Base = NoiseModelFactorN<Point3>;
 
 public:
  using Base::evaluateError;

  PointHeightFactor(Key key, double terrainHeight,
                    const SharedNoiseModel& model)
      : Base(model, key), terrainHeight_(terrainHeight) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new PointHeightFactor(*this)));
  }

  Vector evaluateError(const Point3& foothold,
                       OptionalMatrixType H) const override {
    if (H) {
      H->resize(1, 3);
      *H = Matrix13{{0.0, 0.0, 1.0}};
    }
    return Vector1(foothold.z() - terrainHeight_);
  }
 
 private:
  double terrainHeight_;
};
 
}  // namespace gtsam