diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index de081e23f24f8e4eabbc0819116647b05e651c15..9087c5567cfedc60c00b04420244801f15584215 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -224,11 +224,12 @@ class FactorInertialKinematics_2KF : public testing::Test
P_origin_ = pow(1e-3, 2) * Matrix9d::Identity();
// Set origin of the problem
- KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
+ //KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
// Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KFA_, "Vel0", tA_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KFA_->getV());
+ //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KFA_, "Vel0", tA_);
+ //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
+ //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KFA_->getV());
+ KFA_ = problem_->setPriorFactor(x_origin_, P_origin_, tA_, 0.005);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 566cc84a6a484a0fc7d00b059433ee1ca81b4a76..a8987ab5d3bfa158db00520b44ba1149f89b9aa1 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -98,11 +98,13 @@ class FactorInertialKinematics_1KF : public testing::Test
// Set origin of the problem
- KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
+ //KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
// Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+ //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
+ //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
+ //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+
+ KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index da8bfeb01d4fee99b029b0ca79bab23f032b9678..5d71965d8ff765624b232a5e7d367d205f3aa3e8 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -221,21 +221,12 @@ public:
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
- // - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors isMotion since the flag prior_is_set_ is not true when doing installProcessor (later)
+ // - Manually set problem prior
+ // - call setOrigin on processors isMotion
x_origin_.resize(19);
x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
- P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
-
- KF1_ = problem_->emplaceFrame(KEY, 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();
- ///////////////////////////////////////////////////
- // Prior velocity factor
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1_, "Vel0", t0_);
- 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, KF1_->getV());
+ P_origin_ = pow(1e-3, 2) * Eigen::MatrixXd::Identity(18,18);
+ KF1_ = problem_->setPriorFactor(x_origin_, P_origin_, t0_, 0.1);
initializeData1();
@@ -249,7 +240,7 @@ public:
auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin0->process();
proc_ftpreint_->setOrigin(KF1_);
- problem_->setPriorIsSet(true); // We handle the whole initialization manually here
+ //problem_->setPriorIsSet(true); // We handle the whole initialization manually here
// T1
CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
@@ -304,7 +295,7 @@ public:
Eigen::Matrix6d Q_bi = 1e-8 * Eigen::Matrix6d::Identity();
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic());
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
// Add loose absolute factor on initial bp bias since dynamical trajectories
// able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
@@ -312,7 +303,7 @@ public:
CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
// FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic());
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(),nullptr,false);
}
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 7195fb6ba309f3685d6821cd6f9434b05070345e..2c398bfcdeae98724e795a1a1c87125ca6245243 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -69,7 +69,8 @@ class FactorInertialKinematics_2KF : public testing::Test
x_origin_.resize(19);
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
x_origin_ << zero3, 0,0,0,1, zero3, zero3, zero3, zero3;
- P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+ //P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+ P_origin_ = pow(1e-3, 2) * Eigen::MatrixXd::Identity(18,18);
t_.set(0.0);
// SENSOR + PROCESSOR INERTIAL KINEMATICS
@@ -98,12 +99,9 @@ class FactorInertialKinematics_2KF : public testing::Test
// auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
// Set origin of the problem
- KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
-
+ KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
+ // Set origin of processor Imu
+ std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);
}
virtual void TearDown(){}