12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactored code for ImuMeasurements

release/4.3a0
Varun Agrawal 2020-12-14 14:32:04 -05:00
parent 6543fb6c7c
commit 8b9f917f43
2 changed files with 32 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
tests/ImuMeasurement.h
View File

@ -2,11 +2,11 @@
#include <tests/Measurement.h> #include <tests/Measurement.h>
namespace drs { namespace gtsam {
/// /**
/// \brief Contains data from the IMU mesaurements. *\brief Contains data from the IMU mesaurements.
/// */
class ImuMeasurement : public Measurement { class ImuMeasurement : public Measurement {
public: public:
enum Name { BODY = 0, RF_FOOT = 1, RH_FOOT = 2 }; enum Name { BODY = 0, RF_FOOT = 1, RH_FOOT = 2 };
@ -15,9 +15,21 @@ class ImuMeasurement : public Measurement {
Eigen::Vector3d I_a_WI; ///< Raw acceleration from the IMU (m/s/s) Eigen::Vector3d I_a_WI; ///< Raw acceleration from the IMU (m/s/s)
Eigen::Vector3d I_w_WI; ///< Raw angular velocity from the IMU (rad/s) Eigen::Vector3d I_w_WI; ///< Raw angular velocity from the IMU (rad/s)
ImuMeasurement()
: Measurement("ImuMeasurement"), I_a_WI{0, 0, 0}, I_w_WI{0, 0, 0} {}
virtual ~ImuMeasurement() override {} virtual ~ImuMeasurement() override {}
ImuMeasurement();
friend std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas); friend std::ostream& operator<<(std::ostream& stream,
const ImuMeasurement& meas);
}; };
} // namespace drs 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

24
tests/Measurement.h
View File

@ -3,21 +3,23 @@
#include <Eigen/Core> #include <Eigen/Core>
#include <string> #include <string>
namespace drs { namespace gtsam {
/// /**
/// \brief This is the base class for all measurement types. * \brief This is the base class for all measurement types.
/// */
class Measurement { class Measurement {
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
uint64_t dt; ///< Time since the last message of this type (nanoseconds). size_t dt; ///< Time since the last message of this type (nanoseconds).
uint64_t time; ///< ROS time message recieved (nanoseconds). size_t time; ///< ROS time message recieved (nanoseconds).
std::string type; ///< The type of message (to enable dynamic/static casting). ///< The type of message (to enable dynamic/static casting).
std::string type;
Measurement() : dt(0), time(0), type("UNDEFINED") {}
Measurement(std::string _type) : dt(0), time(0), type(_type) {}
virtual ~Measurement() {} virtual ~Measurement() {}
Measurement();
Measurement(std::string _type);
}; };
} // namespace drs } // namespace gtsam