TEST(JanethOdometryCalibrationTestSuite, testOdometry) {
Odometry odometry;
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d::Zero());
ASSERT_EQ(odometry.getTimestamp(), 0);
ASSERT_EQ(odometry.getPoses(), std::vector<Eigen::Vector3d>(
{Eigen::Vector3d::Zero()}));
ASSERT_EQ(odometry.getTimestamps(), std::vector<double>({0}));
odometry.updateCOGDisplacement(0.5, 0, 5);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(0.5, 0, 0));
ASSERT_EQ(odometry.getTimestamp(), 5);
ASSERT_THROW(odometry.updateCOGDisplacement(0, M_PI / 4, 0),
std::runtime_error);
odometry.updateCOGDisplacement(0, M_PI / 4, 6);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(0.5, 0, M_PI / 4));
ASSERT_EQ(odometry.getTimestamp(), 6);
odometry.reset();
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d::Zero());
ASSERT_EQ(odometry.getTimestamp(), 0);
odometry.updateCOGVelocity(0.0, 0.0, 0.0);
odometry.updateCOGVelocity(0.1, 0, 10);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 0, 0));
ASSERT_EQ(odometry.getTimestamp(), 10);
odometry.updateCOGVelocity(0.0, M_PI / 16, 18);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 0, M_PI / 2));
ASSERT_EQ(odometry.getTimestamp(), 18);
odometry.insertPose(Eigen::Vector3d(1, 1, M_PI / 2), 19);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 1, M_PI / 2));
ASSERT_EQ(odometry.getTimestamp(), 19);
ASSERT_THROW(odometry.insertPose(Eigen::Vector3d(1, 1, M_PI / 2), 10),
std::runtime_error);
odometry.reset(Eigen::Vector3d(1, 2, M_PI / 6), 10);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 2, M_PI / 6));
ASSERT_EQ(odometry.getTimestamp(), 10);
}
