diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp index 98a63fb5180390bad373a8cd50d3a8cd199b4972..29ef1322f7650a8086f8c9fbb6638e0cb25c3e3d 100644 --- a/test/gtest_factor_force_torque_inertial.cpp +++ b/test/gtest_factor_force_torque_inertial.cpp @@ -106,6 +106,21 @@ TEST_F(Test_FactorForceTorqueInertialPreint, constructor) FactorForceTorqueInertial f(feature, capture_origin, processor, false); WOLF_INFO("f id: ", f.id()); + + ASSERT_EQ(f.getCaptureOther(), capture_origin); +} + +TEST_F(Test_FactorForceTorqueInertialPreint, emplace) +{ + feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19), + MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6)); + factor = FactorBase::emplace<FactorForceTorqueInertial>(feature, feature, capture_origin, processor, false); + // this ^^^ will create a warning "ADDING FACTOR id TO FEATURE id THAT IS NOT CONNECTED WITH PROBLEM." + + problem->print(4, 1, 1, 1); // feature and factor not in the tree since they belong to last_ptr with no KF. + + ASSERT_EQ(factor->getCaptureOther(), capture_origin); + ASSERT_EQ(factor->getCapture(), capture_last); } int main(int argc, char **argv)