diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index aea20d6cf0695a2b9e727ef9b8be20b89ae49d31..a8f74f3499037d32032f7c79f2c3354add9d0a3a 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -25,7 +25,7 @@
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/capture/capture_pose.h>
@@ -362,8 +362,7 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("POVCDL", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
@@ -584,7 +583,7 @@ int main (int argc, char **argv) {
ts_since_last_kf += dt;
if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << report << std::endl;
ts_since_last_kf = 0;
}
@@ -598,7 +597,7 @@ int main (int argc, char **argv) {
///////////////////////////////////////////
problem->print(4,true,true,true);
if (solve_end){
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
problem->print(4,true,true,true);
std::cout << report << std::endl;
}
@@ -735,4 +734,4 @@ int main (int argc, char **argv) {
file_est.close();
-}
\ No newline at end of file
+}
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index a9ee309013ece4ae91883318d49b83b4243af4f6..c04ecaa5b08a38b6a2306cc1a15ee159b475002d 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -105,14 +105,14 @@ FactorForceTorque::FactorForceTorque(
_processor_ptr,
_apply_loss_function,
_status,
- _feat->getFrame()->getStateBlock("C"), // COM position, current
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name), current
- _feat->getFrame()->getStateBlock("L"), // Angular momentum, current
- _frame_other->getStateBlock("C"), // COM position, previous
- _frame_other->getStateBlock("D"), // COM velocity (bad name), previous
- _frame_other->getStateBlock("L"), // Angular momentum, previous
+ _feat->getFrame()->getStateBlock('C'), // COM position, current
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
+ _frame_other->getStateBlock('C'), // COM position, previous
+ _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
+ _frame_other->getStateBlock('L'), // Angular momentum, previous
_sb_bp_other, // BC relative position bias, previous
- _frame_other->getStateBlock("O") // Base orientation, previous
+ _frame_other->getStateBlock('O') // Base orientation, previous
),
sb_bp_other_(_sb_bp_other)
{
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
index 99fb9e3610cccd1d62c64b5928fbb34c114eb6ce..080a31b8622cfe73c8a5b50cd5d45edb52c21ea1 100644
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -169,15 +169,15 @@ inline FactorForceTorquePreint::FactorForceTorquePreint(
_processor_ptr,
_apply_loss_function,
_status,
- _cap_origin_ptr->getFrame()->getStateBlock("C"),
- _cap_origin_ptr->getFrame()->getStateBlock("D"),
- _cap_origin_ptr->getFrame()->getStateBlock("L"),
+ _cap_origin_ptr->getFrame()->getStateBlock('C'),
+ _cap_origin_ptr->getFrame()->getStateBlock('D'),
+ _cap_origin_ptr->getFrame()->getStateBlock('L'),
_cap_origin_ptr->getFrame()->getO(),
_cap_ikin_other->getSensorIntrinsic(),
_cap_gyro_other->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getStateBlock("C"),
- _ftr_ptr->getFrame()->getStateBlock("D"),
- _ftr_ptr->getFrame()->getStateBlock("L"),
+ _ftr_ptr->getFrame()->getStateBlock('C'),
+ _ftr_ptr->getFrame()->getStateBlock('D'),
+ _ftr_ptr->getFrame()->getStateBlock('L'),
_ftr_ptr->getFrame()->getO()
),
dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index cf3662361fdd33e23b1ef0c406e06972010609a4..082737f0081a32c44645e1d6b08c55c0296e602a 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -79,10 +79,10 @@ FactorInertialKinematics::FactorInertialKinematics(
_feat->getFrame()->getP(),
_feat->getFrame()->getO(),
_feat->getFrame()->getV(),
- _feat->getFrame()->getStateBlock("C"), // COM position
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name)
- _feat->getFrame()->getStateBlock("L"), // Angular momentum
- _feat->getCapture()->getStateBlock("I"), // body-to-COM relative position bias
+ _feat->getFrame()->getStateBlock('C'), // COM position
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name)
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum
+ _feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
_sb_bias_imu
)
{
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 1fe5892fda0d695e4699c6afaf6cf7544c3adbcc..f19ebad7b376baf948c2f1ebc459add7027ab089 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -63,7 +63,7 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
if (time_ok) {
CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel);
auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first;
- if (cap_angvel && cap_angvel->getStateBlock("I")){ // TODO: or only cap_angvel?
+ if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_angvel?
// cast incoming capture to the range-and-bearing type, add it to the keyframe
auto kf = buffer_pack_kf_it->second->key_frame;
auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second);
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index ab6d15e87ad2dee321fb31fa05b914f12e7037c2..86c908418f7d77e21f6ffc8ee3a31b1956822fbb 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -17,7 +17,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
{
assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0.");
assert(getIntrinsic()->getState().size() == 3 && "Wrong extrinsics size! Should be 3.");
- setStateBlockDynamic("I", true);
+ setStateBlockDynamic('I', true);
}
SensorInertialKinematics::~SensorInertialKinematics()
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 47e95dd52a4793426418735e580c7b7342425749..d529469ae5a1801ff365149b7c53ce26d3bf13d5 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -32,7 +32,7 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
@@ -121,15 +121,15 @@ Matrix9d computeKinCov(const Matrix3d& Qp,
}
-void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
+void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
if(!KF->getStateBlock(sb_name)->isFixed())
{
- if (sb_name == "O")
+ if (sb_name == 'O')
{
double max_rad_pert = M_PI / 3;
Vector3d do_pert = max_rad_pert*Vector3d::Random();
Quaterniond curr_state_q(KF->getO()->getState().data());
- KF->getStateBlock("O")->setState((curr_state_q * exp_q(do_pert)).coeffs());
+ KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
}
else
{
@@ -152,10 +152,10 @@ FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, Vec
Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
{
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);
- StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock("I", sbbimu, problem); // Simulating IMU
+ 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);
+ StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
return KF;
}
@@ -167,7 +167,7 @@ class FactorInertialKinematics_2KF : public testing::Test
wolf::TimeStamp tA_;
wolf::TimeStamp tB_;
ProblemPtr problem_;
- CeresManagerPtr ceres_manager_;
+ SolverCeresPtr solver_;
VectorComposite x_origin_; // initial state
VectorComposite s_origin_; // initial sigmas for prior
SensorInertialKinematicsPtr SIK_;
@@ -202,11 +202,10 @@ class FactorInertialKinematics_2KF : public testing::Test
SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e3;
- ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_->getSolverOptions().max_num_iterations = 1e3;
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
// INSTALL Imu SENSOR
// Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
@@ -227,16 +226,16 @@ class FactorInertialKinematics_2KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = zero3;
bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("C", sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("D", sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("L", sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock("I", sbbimuA, problem_);
+ StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
+ StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
+ StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
+ StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
// Fix the one we cannot estimate
// KFA_->getP()->fix();
// KFA_->getO()->fix();
// KFA_->getV()->fix();
- KFA_->getStateBlock("I")->fix(); // Imu
+ KFA_->getStateBlock('I')->fix(); // Imu
// INERTIAL KINEMATICS FACTOR
// Measurements
@@ -254,7 +253,7 @@ class FactorInertialKinematics_2KF : public testing::Test
Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
- CIKA_->getStateBlock("I")->fix(); // IK bias
+ CIKA_->getStateBlock('I')->fix(); // IK bias
FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
@@ -268,7 +267,7 @@ class FactorInertialKinematics_2KF : public testing::Test
// KFB_->getP()->fix();
KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFB_->getStateBlock("I")->fix(); // Imu
+ KFB_->getStateBlock('I')->fix(); // Imu
// // ================ PROCESS Imu DATA
@@ -286,7 +285,7 @@ class FactorInertialKinematics_2KF : public testing::Test
CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
CIKB_->getSensorIntrinsic()->fix(); // IK bias
FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock("I"), nullptr, false);
+ auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -458,7 +457,7 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
// KFB_->getP()->fix();
// KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFC_->getStateBlock("I")->fix();
+ KFC_->getStateBlock('I')->fix();
// ================ FACTOR INERTIAL KINEMATICS ON KFB
Vector3d pBC_measC = zero3;
@@ -470,9 +469,9 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
- CIKC_->getStateBlock("I")->fix(); // IK bias
+ CIKC_->getStateBlock('I')->fix(); // IK bias
auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock("I"), nullptr, false);
+ FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -520,9 +519,9 @@ class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinemati
FactorBase::emplace<FactorOdom3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false);
// With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
- CIKB_->getStateBlock("I")->unfix();
+ CIKB_->getStateBlock('I')->unfix();
// However this test is not sufficient to observe the bias at KFA
- // CIKA_->getStateBlock("I")->unfix(); // this is not observable
+ // CIKA_->getStateBlock('I')->unfix(); // this is not observable
}
};
@@ -546,27 +545,27 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
@@ -576,27 +575,27 @@ TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
@@ -606,27 +605,27 @@ TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -637,27 +636,27 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -668,27 +667,27 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -696,135 +695,135 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
{
// problem_->print(4,true,true,true);
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
// {
-// string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// perturbateAllIfUnFixed(KFA_);
// perturbateAllIfUnFixed(KFB_);
// perturbateAllIfUnFixed(KFC_);
// // problem_->print(4,true,true,true);
-// report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+// report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// // std::cout << report << std::endl;
// // problem_->print(4,true,true,true);
-// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
-// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("C")->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("D")->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock("I")->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
// }
TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
{
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 33b375ecaea8833c68d4c972d64b8d47b8c659c6..8dfb0f528b5389f9703d2244ecac5529b4006f09 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -32,7 +32,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
@@ -67,7 +67,7 @@ class FactorInertialKinematics_1KF : public testing::Test
wolf::TimeStamp t_;
ProblemPtr problem_;
// SensorImuPtr sen_imu;
- CeresManagerPtr ceres_manager_;
+ SolverCeresPtr solver_;
// ProcessorBasePtr processor;
// ProcessorImuPtr processor_imu;
VectorComposite x_origin_; // initial state
@@ -88,11 +88,10 @@ class FactorInertialKinematics_1KF : public testing::Test
problem_ = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.max_num_iterations = 1e4;
- ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
//===================================================== INITIALIZATION
x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
@@ -112,13 +111,13 @@ class FactorInertialKinematics_1KF : public testing::Test
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
sbbimu_ = make_shared<StateBlock>(bias_imu_);
- KF0_->addStateBlock("C", sbc, problem_);
- KF0_->addStateBlock("D", sbd, problem_);
- KF0_->addStateBlock("L", sbL, problem_);
- KF0_->addStateBlock("I", sbbimu_, problem_); // IMU bias
+ KF0_->addStateBlock('C', sbc, problem_);
+ KF0_->addStateBlock('D', sbd, problem_);
+ KF0_->addStateBlock('L', sbL, problem_);
+ KF0_->addStateBlock('I', sbbimu_, problem_); // IMU bias
// Fix the one we cannot estimate
- KF0_->getStateBlock("I")->fix(); // Imu
+ KF0_->getStateBlock('I')->fix(); // Imu
// =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// Measurements
@@ -149,8 +148,8 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas = ZERO3;
@@ -174,8 +173,8 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
@@ -199,8 +198,8 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
{
FactorInertialKinematics_1KF::SetUp();
// Fix the COM position
- CIK0_->getStateBlock("I")->unfix();
- KF0_->getStateBlock("C")->fix();
+ CIK0_->getStateBlock('I')->unfix();
+ KF0_->getStateBlock('C')->fix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
@@ -226,10 +225,10 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
bias_p_ = ZERO3;
- CIK0_->getStateBlock("I")->setState(bias_p_);
+ CIK0_->getStateBlock('I')->setState(bias_p_);
Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
@@ -252,7 +251,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// public:
// wolf::TimeStamp t_;
// ProblemPtr problem_;
-// CeresManager* ceres_manager_;
+// SolverCeres* solver_;
// VectorXd x_origin_;
// MatrixXd P_origin_;
// FrameBasePtr KF0_;
@@ -279,7 +278,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
// // ceres_options.max_line_search_step_contraction = 1e-3;
// // ceres_options.max_num_iterations = 1e4;
-// ceres_manager_ = new CeresManager(problem_, ceres_options);
+// solver_ = new SolverCeres(problem_, ceres_options);
//
// //===================================================== INITIALIZATION
// x_origin_.resize(10);
@@ -372,12 +371,12 @@ TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at("P"), 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
@@ -389,11 +388,11 @@ TEST_F(FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt)
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
@@ -404,12 +403,12 @@ TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
Eigen::Vector3d bp_exp; bp_exp << 1,0,0;
KF0_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIK0_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ // ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIK0_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
@@ -420,11 +419,11 @@ TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 08fe7503940af68f8f93942a0ee693d54b45534c..d2c378112faff3c61c74a930fa1290b593e1b562 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -15,7 +15,7 @@
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
// #include <core/ceres_wrapper/qr_manager.h>
@@ -66,7 +66,7 @@ public:
wolf::TimeStamp t0_;
ProblemPtr problem_;
SensorImuPtr sen_imu_;
- CeresManagerPtr ceres_manager_;
+ SolverCeresPtr solver_;
SensorInertialKinematicsPtr sen_ikin_;
SensorForceTorquePtr sen_ft_;
ProcessorImuPtr proc_imu_;
@@ -81,11 +81,10 @@ public:
problem_ = Problem::create("POVCDL", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.max_num_iterations = 1e4;
- ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
@@ -542,38 +541,38 @@ public:
TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
{
// // TEST FIRST PURE INTEGRATION
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// THEN SOLVE
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -583,22 +582,22 @@ TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
{
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4,true,true,true);
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -612,39 +611,39 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -657,38 +656,38 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -699,7 +698,7 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
{
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
@@ -707,19 +706,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -730,7 +729,7 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
{
problem_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
@@ -738,19 +737,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -763,14 +762,14 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
// Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
// Vector4d q_diff; q_diff << 1,0,0,0;
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
-
-// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 1e-6);
-// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('O')->getState(), q_diff, 1e-6);
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), Lc_diff, 1e-6);
// }
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index c8a95d15f0d356222e29e2b7a9e952518d151443..137b96f341e41d6c678165352d75143bbc124101 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -15,7 +15,7 @@
// WOLF
#include <core/problem/problem.h>
-#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
#include <core/capture/capture_pose.h>
@@ -50,7 +50,7 @@ class FactorInertialKinematics_2KF : public testing::Test
wolf::TimeStamp t_;
ProblemPtr problem_;
SensorImuPtr sen_imu_;
- CeresManagerPtr ceres_manager_;
+ SolverCeresPtr solver_;
SensorInertialKinematicsPtr sen_ikin_;
VectorXd x_origin_;
MatrixXd P_origin_;
@@ -63,11 +63,10 @@ class FactorInertialKinematics_2KF : public testing::Test
problem_ = Problem::create("POVCDL", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- // ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.max_num_iterations = 1e4;
- ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ // solver_->getSolverOptions().max_num_iterations = 1e4;
//===================================================== INITIALIZATION
x_origin_.resize(19);
@@ -191,7 +190,7 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
KF0_->perturb();
- std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
std::cout << report << std::endl;
problem_->print(4,true,true,true);
@@ -201,17 +200,17 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
FrameBasePtr KF1 = problem_->getTrajectory()->getLastKeyFrame();
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("V")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("P")->getState(), pdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("V")->getState(), vdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("C")->getState(), pdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("D")->getState(), vdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('P')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('V')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('C')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('D')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('L')->getState(), ZERO3, 1e-6);
}