diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 5952b69555d0f5230bf9f7e3519a19e2da6db639..50f96e712de13d3d923461eed39bbbd558a0643a 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -285,14 +285,6 @@ public:
}
void buildDataVectors(){
- std::cout << "\n\nOOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
- std::cout << pbc_ << std::endl;
pbc_meas_ = pbc_ + bias_pbc_;
acc_gyr_meas_ << acc_meas_, w_meas_;
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 225cf9bbcc849aed32fb1d62230098be6a8694df..89e2c81bd873a3c8edc2e7a10200e3b7894537ab 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -18,6 +18,7 @@
#include <core/ceres_wrapper/ceres_manager.h>
#include <core/math/rotations.h>
#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
@@ -39,6 +40,9 @@ using namespace wolf;
using namespace Eigen;
using namespace std;
+const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
+
class FactorInertialKinematics_2KF : public testing::Test
{
@@ -67,9 +71,7 @@ class FactorInertialKinematics_2KF : public testing::Test
//===================================================== INITIALIZATION
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();
+ 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);
@@ -79,29 +81,49 @@ class FactorInertialKinematics_2KF : public testing::Test
intr_ik->std_vbc = 0.1;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
- // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/test/sensor_inertial_kinematics.yaml"); // TODO: does not work!
sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu.yaml");
+ ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
// auto processor_imu = std::static_pointer_cast<ProcessorImu>(proc);
ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = 0.01;
+ params_ik->std_bp_drift = 0.0000001;
ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
+ // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
// 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_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
+ KF0_ = problem_->emplaceFrame(KEY, x_origin_, t_);
+
+ ///////////////////////////////////////////////////
+ // 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, KF0_->getV(), nullptr, false);
// Set origin of processor Imu
std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);
+
+ ////////////////////////////////////////////
+ // Add absolute factor on Imu biases to simulate a fix()
+ Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_);
+ FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+
}
virtual void TearDown(){}
@@ -132,18 +154,18 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
// sen_imu_->getIntrinsic()->fix();
// T0
+ auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
+ sen_ikin_->process(CIKin0);
CaptureImuPtr CImu0 = std::make_shared<CaptureImu>(t0, sen_imu_, acc_gyr_meas, acc_gyr_cov);
sen_imu_->process(CImu0);
- CImu0->getSensorIntrinsic()->fix();
- auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
- sen_ikin_->process(CIKin0);
+
CIKin0->getSensorIntrinsic()->fix();
// T1
CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas, acc_gyr_cov);
sen_imu_->process(CImu1);
- CImu1->getSensorIntrinsic()->fix();
+ // CImu1->getSensorIntrinsic()->fix();
auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin0, Iq, Lq);
sen_ikin_->process(CIKin1);
@@ -152,7 +174,7 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
// T2
CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas, acc_gyr_cov);
- CImu2->getSensorIntrinsic()->fix();
+ // CImu2->getSensorIntrinsic()->fix();
sen_imu_->process(CImu2);
auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin0, Iq, Lq);
@@ -161,23 +183,36 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
// T3
CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas, acc_gyr_cov);
- CImu3->getSensorIntrinsic()->fix();
+ // CImu3->getSensorIntrinsic()->fix();
sen_imu_->process(CImu3);
CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin0, Iq, Lq);
sen_ikin_->process(CIKin3);
CIKin3->getSensorIntrinsic()->fix();
+ KF0_->perturb();
- KF0_->getStateBlock("C")->setState(Eigen::Vector3d::Random());
- KF0_->getStateBlock("D")->setState(Eigen::Vector3d::Random());
- KF0_->getStateBlock("L")->setState(Eigen::Vector3d::Random());
+ std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::cout << report << std::endl;
+ problem_->print(4,true,true,true);
+ Vector3d pdiff; pdiff << 4.5, 0, 0;
+ Vector3d vdiff; vdiff << 3, 0, 0;
- problem_->print(4,true,true,true);
- // std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
+ 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(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);
}