ImuMeasurement.h

Go to the documentation of this file.

#pragma once

#include <tests/Measurement.h>

namespace gtsam {

class ImuMeasurement : public Measurement {
 public:
  enum Name { BODY = 0, RF_FOOT = 1, RH_FOOT = 2 };

  Name name;               
  Eigen::Vector3d I_a_WI;  
  Eigen::Vector3d I_w_WI;  

  ImuMeasurement()
      : Measurement("ImuMeasurement"), I_a_WI{0, 0, 0}, I_w_WI{0, 0, 0} {}

  ~ImuMeasurement() override {}

  friend std::ostream& operator<<(std::ostream& stream,
                                  const ImuMeasurement& meas);
};

std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas) {
  stream << "IMU Measurement at time = " << meas.time << " : \n"
         << "dt    : " << meas.dt << "\n"
         << "I_a_WI: " << meas.I_a_WI.transpose() << "\n"
         << "I_w_WI: " << meas.I_w_WI.transpose() << "\n";
  return stream;
}

}  // namespace gtsam