diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp index 93974c8dfdc87c7feb38de864fde89ccbac75f41..63f9fe29983b2fafbf79c2609f76d3a7b54566d1 100644 --- a/src/test/gtest_feature_imu.cpp +++ b/src/test/gtest_feature_imu.cpp @@ -143,11 +143,20 @@ TEST_F(FeatureIMU_test, access_members) { using namespace wolf; - Eigen::VectorXs delta(16); + Eigen::VectorXs delta(10); //dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98 - delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98, 0,0,0, 0,0,0; + delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98; ASSERT_TRUE((feat_imu->dp_preint_ - delta.head<3>()).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)) << "feat_imu->dp_preint_ : " << feat_imu->dp_preint_.transpose() << ",\t delta.head<3>() :" << delta.head<3>().transpose() << ",\n delta_preint : " << delta_preint.transpose() << std::endl; + EXPECT_TRUE((feat_imu->dv_preint_ - delta.tail<3>()).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)) << "feat_imu->dv_preint_ : " << feat_imu->dv_preint_.transpose() << ",\t delta.tail<3>() :" << delta.tail<3>().transpose() << + ",\n (feat_imu->dv_preint_ - delta.tail<3>() : " << (feat_imu->dv_preint_ - delta.tail<3>()).transpose() << std::endl; + ASSERT_TRUE((feat_imu->dv_preint_ - delta.tail<3>()).isMuchSmallerThan(1, wolf::Constants::EPS)) << "feat_imu->dv_preint_ : " << feat_imu->dv_preint_.transpose() << ",\t delta.tail<3>() :" << delta.tail<3>().transpose() << + ",\n delta_preint : " << delta_preint.transpose() << std::endl; + + Eigen::Vector4s quat; + quat << feat_imu->dq_preint_.x(), feat_imu->dq_preint_.y(), feat_imu->dq_preint_.z(), feat_imu->dq_preint_.w(); + ASSERT_TRUE((quat - delta.segment<4>(3)).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)) << "feat_imu->dq_preint_ : " << quat.transpose() << ",\t delta.segment<4>(3) :" << delta.segment<4>(3).transpose() << + ",\n delta_preint : " << delta_preint.transpose() << std::endl; } TEST_F(FeatureIMU_test, addConstraint)