From bb37012ae0418cb41b1389a104e4da200bbe7ef3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr> Date: Wed, 5 Feb 2020 14:43:31 +0100 Subject: [PATCH] Corrected kinematic error covariance computation, still 0 eigenvalues --- test/gtest_factor_inertial_kinematics.cpp | 137 +++++++++++++--------- 1 file changed, 79 insertions(+), 58 deletions(-) diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp index 10a3b12..9f7cba8 100644 --- a/test/gtest_factor_inertial_kinematics.cpp +++ b/test/gtest_factor_inertial_kinematics.cpp @@ -22,38 +22,42 @@ #include <core/feature/feature_base.h> // IMU -// #include "imu/internal/config.h" -// #include "imu/capture/capture_IMU.h" -// #include "imu/processor/processor_IMU.h" -// #include "imu/sensor/sensor_IMU.h" +#include "IMU/internal/config.h" +#include "IMU/capture/capture_IMU.h" +#include "IMU/processor/processor_IMU.h" +#include "IMU/sensor/sensor_IMU.h" // BODYDYNAMICS #include "bodydynamics/feature/feature_inertial_kinematics.h" #include "bodydynamics/factor/factor_inertial_kinematics.h" -using namespace wolf; - +#include <Eigen/Eigenvalues> -Eigen::Matrix9d computeKinCov(const Eigen::Matrix3d& Qp, - const Eigen::Matrix3d& Qv, - const Eigen::Matrix3d& Qw, - const Eigen::Vector3d& w_unb, - const Eigen::Vector3d& p_unb) +using namespace wolf; +using namespace Eigen; +using namespace std; + +Matrix9d computeKinCov(const Matrix3d& Qp, + const Matrix3d& Qv, + const Matrix3d& Qw, + const Vector3d& w_unb, + const Vector3d& p_unb, + const Matrix3d& Iq) { - Eigen::Matrix9d cov; cov.setZero(); + Matrix9d cov; cov.setZero(); - Eigen::Matrix3d wx = skew(w_unb); - Eigen::Matrix3d px = skew(p_unb); + Matrix3d wx = skew(w_unb); + Matrix3d px = skew(p_unb); // Starting from top left, to the right and then next row cov.topLeftCorner<3,3>() = Qp; cov.block<3,3>(0,3) = Qp * wx; - // cov.topRightCorner<3,3>() = Eigen::Matrix3d::Identity(); + // cov.topRightCorner<3,3>() = Matrix3d::Identity(); cov.block<3,3>(3,0) = cov.block<3,3>(0,3).conjugate(); - cov.block<3,3>(3,3) = -wx * Qp * wx + Qv - px * Qw * px; - cov.block<3,3>(3,6) = -Qv; - // cov.block<3,3>(6,0) = Eigen::Matrix3d::Identity(); - cov.block<3,3>(6,3) = -Qv; - cov.block<3,3>(6,6) = Qv; + cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px; + cov.block<3,3>(3,6) = -Qv*Iq; + // cov.block<3,3>(6,0) = Matrix3d::Identity(); + cov.block<3,3>(6,3) = -Iq*Qv; + cov.block<3,3>(6,6) = Iq*Qv*Iq; return cov; } @@ -67,16 +71,16 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test CeresManager* ceres_manager; ProcessorBasePtr processor; // ProcessorIMUPtr processor_imu; - Eigen::VectorXd expected_final_state; - Eigen::VectorXd x_origin; - Eigen::MatrixXd P_origin; + VectorXd expected_final_state; + VectorXd x_origin; + MatrixXd P_origin; FrameBasePtr KF0; virtual void SetUp() { - // using std::shared_ptr; - // using std::make_shared; - // using std::static_pointer_cast; + // using shared_ptr; + // using make_shared; + // using static_pointer_cast; //===================================================== SETTING PROBLEM // WOLF PROBLEM @@ -90,12 +94,12 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test ceres_manager = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU ---- ALL OF THAT JUST TO HAVE A PROCESSOR MOTION AT HAND... - // std::string wolf_root = _WOLF_IMU_ROOT_DIR; + // string wolf_root = _WOLF_IMU_ROOT_DIR; - // SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Eigen::Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + // SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); // processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); - // sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - // processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); + // sen_imu = static_pointer_cast<SensorIMU>(sen0_ptr); + // processor_imu = static_pointer_cast<ProcessorIMU>(processor); //===================================================== INITIALIZATION x_origin.resize(10); @@ -111,51 +115,68 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test KF0->getO()->fix(); KF0->getV()->fix(); - Eigen::Vector3d zero3; zero3 << 0,0,0; - Eigen::Vector6d zero6; zero6 << zero3, zero3; - StateBlockPtr sbc = std::make_shared<StateBlock>(zero3); - StateBlockPtr sbd = std::make_shared<StateBlock>(zero3); - StateBlockPtr sbL = std::make_shared<StateBlock>(zero3); - StateBlockPtr sbb = std::make_shared<StateBlock>(zero3); - StateBlockPtr sbbimu = std::make_shared<StateBlock>(zero6); + Vector3d zero3; zero3 << 0,0,0; + Vector6d zero6; zero6 << zero3, zero3; + StateBlockPtr sbc = make_shared<StateBlock>(zero3); + StateBlockPtr sbd = make_shared<StateBlock>(zero3); + StateBlockPtr sbL = make_shared<StateBlock>(zero3); + StateBlockPtr sbb = make_shared<StateBlock>(zero3); + StateBlockPtr sbbimu = make_shared<StateBlock>(zero6); KF0->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD); KF0->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD); KF0->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD); KF0->addStateBlock("B", sbb); problem->notifyStateBlock(sbb,ADD); - std::cout << KF0->getStateBlock("L")->getState() << std::endl; + cout << KF0->getStateBlock("L")->getState() << endl; // =============== SET KIN MEASUREMENT AND COVARIANCE // Measurements - Eigen::Vector3d pBC_meas; pBC_meas << 1,1,1; - Eigen::Vector3d bias_p; bias_p << 0,0,0; + Vector3d pBC_meas; pBC_meas << 0,0,0; + Vector3d bias_p; bias_p << 0,0,0; - Eigen::Vector3d vBC_meas; vBC_meas << 1,1,1; + Vector3d vBC_meas; vBC_meas << 0,0,0; - Eigen::Vector3d w_meas; w_meas << 0,0,0; - Eigen::Vector3d bias_w; bias_w << 0,0,0; + Vector3d w_meas; w_meas << 0,0,0; + Vector3d bias_w; bias_w << 0,0,0; // momentum parameters - Eigen::Matrix3d Iq; Iq.setIdentity(); - Eigen::Vector3d Lq; Lq.setIdentity(); + Matrix3d Iq; Iq.setIdentity(); + Vector3d Lq; Lq.setIdentity(); + + Matrix3d Qp; Qp.setIdentity(); + Matrix3d Qv; Qv.setIdentity(); + Matrix3d Qw; Qw.setIdentity(); + Matrix9d Qkin = computeKinCov(Qp, Qv, Qw, pBC_meas-bias_p, w_meas-bias_w, Iq); + + cout << "Qp\n" << Qp << endl; + cout << "Qv\n" << Qv << endl; + cout << "Qw\n" << Qw << endl; + cout << "Qkin\n" << Qkin << endl; + + ASSERT_TRUE(Qkin.cols() == Qkin.rows()); + ASSERT_MATRIX_APPROX(Qkin, Qkin.conjugate(), 1e-5); - Eigen::Matrix3d Qp; Qp.setIdentity(); - Eigen::Matrix3d Qv; Qv.setIdentity(); - Eigen::Matrix3d Qw; Qw.setIdentity(); - Eigen::Matrix9d Qkin = computeKinCov(Qp, Qv, Qw, pBC_meas-bias_p, w_meas-bias_w); + SelfAdjointEigenSolver<Matrix9d> eigensolver(Qkin); + if (eigensolver.info() != Success) abort(); + cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl; + cout << "Here's a matrix whose columns are eigenvectors of A \n" + << "corresponding to these eigenvalues:\n" + << eigensolver.eigenvectors() << endl; + - std::cout << "Qp\n" << Qp << std::endl; - std::cout << "Qv\n" << Qv << std::endl; - std::cout << "Qw\n" << Qw << std::endl; - std::cout << "Qkin\n" << Qkin << std::endl; + // LLT<Matrix9d> lltOfQkin(Qkin); // compute the Cholesky decomposition of A + // if(lltOfQkin.info() == NumericalIssue) + // { + // throw runtime_error("Possibly non semi-positive definitie matrix!"); + // } // =============== CREATE CAPURE/FEATURE/FACTOR - Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas; + Vector9d meas; meas << pBC_meas, vBC_meas, w_meas; CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0, "CaptureInertialKinematics", t, nullptr); - // auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); - // auto feat_in = std::static_pointer_cast<FeatureInertialKinematics>(feat); + auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + // auto feat_in = static_pointer_cast<FeatureInertialKinematics>(feat); // sbbimu->fix(); // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat, feat, sbbimu); @@ -169,8 +190,8 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test TEST_F(FactorInertialKinematics_ZeroMvt,ZeroMvt) { problem->print(3,0,1,1); - std::cout << "SOLVING" << std::endl; - // std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + cout << "SOLVING" << endl; + // string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; // ASSERT_MATRIX_APPROX(KF0->getState(), KF0->getState(), 1e-5); ASSERT_TRUE(true); -- GitLab