Fixed gtest processor inertial kinematics

test/gtest_processor_inertial_kinematics.cpp

@@ -69,9 +69,6 @@ class FactorInertialKinematics_2KF : public testing::Test
         // solver_->getSolverOptions().max_num_iterations = 1e4;
 
         //===================================================== INITIALIZATION
-        x_origin_.resize(19);
-        x_origin_ << ZERO3, 0,0,0,1, ZERO3, ZERO3, ZERO3, ZERO3;
-        P_origin_ = pow(1e-3, 2) * Eigen::MatrixXd::Identity(18,18);
         t_.set(0.0);
 
         // SENSOR + PROCESSOR INERTIAL KINEMATICS
@@ -100,19 +97,14 @@ class FactorInertialKinematics_2KF : public testing::Test
         // auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
 
         // Set origin of the problem
-        // KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
-        KF0_ = problem_->emplaceFrame(KEY, t_, x_origin_);
-
-        ///////////////////////////////////////////////////
-        // Prior pose factor
-        CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF0_, t_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
-        pose_prior_capture->emplaceFeatureAndFactor();
-
-        ///////////////////////////////////////////////////
-        // Prior velocity factor
-        CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
-        FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
-        FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF0_->getV(), nullptr, false);
+        x_origin_.resize(19);
+        x_origin_ << ZERO3, 0,0,0,1, ZERO3, ZERO3, ZERO3, ZERO3;
+        VectorXd std_vec = pow(1e-3, 2)*VectorXd::Ones(18); 
+        VectorComposite prior(x_origin_,  "POVCDL", {3,4,3,3,3,3});
+        VectorComposite prior_std(std_vec, "POVCDL", {3,3,3,3,3,3});
+        problem_->setPriorFactor(prior, prior_std, t_, 0.001);
+        KF0_ = problem_->getTrajectory()->getLastFrame();
+
         // Set origin of processor Imu
         std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);