Add test emplace

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)