Merge branch '7-adapt-to-std-set-and-std-map-instead-of-std-list-in-wolf-nodes' into devel

demos/mcapi_povcdl_estimation.cpp

@@ -432,7 +432,7 @@ int main (int argc, char **argv) {
     P_origin.block<3,3>(0,0)   = pow(std_prior_p, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(3,3)   = pow(std_prior_o, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(6,6)   = pow(std_prior_v, 2) * Matrix3d::Identity();
-    FrameBasePtr KF1 = problem->emplaceFrame(KEY, x_origin, t0);
+    FrameBasePtr KF1 = problem->emplaceKeyFrame( x_origin, t0);
     // Prior pose factor
     CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
     pose_prior_capture->emplaceFeatureAndFactor();

test/gtest_factor_force_torque.cpp

@@ -151,7 +151,7 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
 FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
                               Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
 {
-    FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), wolf::KEY, t, "POV", x_origin);
+    FrameBasePtr KF = FrameBase::emplaceKeyFrame<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
     StateBlockPtr sbc = make_shared<StateBlock>(c);  KF->addStateBlock('C', sbc, problem);
     StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
     StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);

test/gtest_feature_force_torque_preint_WIP.cpp

@@ -111,7 +111,7 @@ class FeatureForceTorquePreint_test : public testing::Test
     //emplace Frame
         ts_ = problem_->getProcessorIsMotion()->getBuffer().back().ts_;
         state_vec_ = problem_->getProcessorIsMotion()->getCurrentState();
-        last_frame_ = problem_->emplaceFrame(KEY, state_vec_, ts_);
+        last_frame_ = problem_->emplaceKeyFrame( state_vec_, ts_);
 
     //emplace a feature
         delta_preint_ = problem_->getProcessorIsMotion()->getMotion().delta_integr_;

test/gtest_processor_force_torque_preint.cpp

@@ -203,7 +203,7 @@ public:
         // - call setOrigin on processors isMotion
         setOriginState();
         MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
-        KF1_ = problem_->emplaceFrame(KEY, t0_, x_origin_);
+        KF1_ = problem_->emplaceKeyFrame( t0_, x_origin_);
         // Prior pose factor
         CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
         pose_prior_capture->emplaceFeatureAndFactor();
@@ -269,7 +269,7 @@ public:
         setOdomData();
         Matrix6d rel_pose_cov = 1e-6 * Matrix6d::Identity();
 
-        KF2_ = problem_->getTrajectory()->getLastKeyFrame();
+        KF2_ = problem_->getTrajectory()->getLastFrame();
         CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
         FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
         FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false);

test/gtest_processor_inertial_kinematics.cpp

@@ -101,7 +101,7 @@ class FactorInertialKinematics_2KF : public testing::Test
 
         // Set origin of the problem
         // KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
-        KF0_ = problem_->emplaceFrame(KEY, t_, x_origin_);
+        KF0_ = problem_->emplaceKeyFrame( t_, x_origin_);
 
         ///////////////////////////////////////////////////
         // Prior pose factor
@@ -197,7 +197,7 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
     Vector3d pdiff; pdiff << 4.5, 0, 0;
     Vector3d vdiff; vdiff << 3, 0, 0;
 
-    FrameBasePtr KF1 = problem_->getTrajectory()->getLastKeyFrame();
+    FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
 
 
     ASSERT_MATRIX_APPROX(KF0_->getStateBlock('P')->getState(), ZERO3, 1e-6);