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