Skip to content
Snippets Groups Projects
Commit bb37012a authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Corrected kinematic error covariance computation, still 0 eigenvalues

parent a86bb561
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_inertial_kinematics.cpp
+ 79
− 58
View file @ bb37012a
...@@ -22,38 +22,42 @@ ...@@ -22,38 +22,42 @@
#include <core/feature/feature_base.h> #include <core/feature/feature_base.h>
// IMU // IMU
// #include "imu/internal/config.h" #include "IMU/internal/config.h"
// #include "imu/capture/capture_IMU.h" #include "IMU/capture/capture_IMU.h"
// #include "imu/processor/processor_IMU.h" #include "IMU/processor/processor_IMU.h"
// #include "imu/sensor/sensor_IMU.h" #include "IMU/sensor/sensor_IMU.h"
// BODYDYNAMICS // BODYDYNAMICS
#include "bodydynamics/feature/feature_inertial_kinematics.h" #include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h" #include "bodydynamics/factor/factor_inertial_kinematics.h"
using namespace wolf; #include <Eigen/Eigenvalues>
Eigen::Matrix9d computeKinCov(const Eigen::Matrix3d& Qp, using namespace wolf;
const Eigen::Matrix3d& Qv, using namespace Eigen;
const Eigen::Matrix3d& Qw, using namespace std;
const Eigen::Vector3d& w_unb,
const Eigen::Vector3d& p_unb) 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); Matrix3d wx = skew(w_unb);
Eigen::Matrix3d px = skew(p_unb); Matrix3d px = skew(p_unb);
// Starting from top left, to the right and then next row // Starting from top left, to the right and then next row
cov.topLeftCorner<3,3>() = Qp; cov.topLeftCorner<3,3>() = Qp;
cov.block<3,3>(0,3) = Qp * wx; 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,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,3) = -wx*Qp*wx + Qv - px*Qw*px;
cov.block<3,3>(3,6) = -Qv; cov.block<3,3>(3,6) = -Qv*Iq;
// cov.block<3,3>(6,0) = Eigen::Matrix3d::Identity(); // cov.block<3,3>(6,0) = Matrix3d::Identity();
cov.block<3,3>(6,3) = -Qv; cov.block<3,3>(6,3) = -Iq*Qv;
cov.block<3,3>(6,6) = Qv; cov.block<3,3>(6,6) = Iq*Qv*Iq;
return cov; return cov;
} }
...@@ -67,16 +71,16 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test ...@@ -67,16 +71,16 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test
CeresManager* ceres_manager; CeresManager* ceres_manager;
ProcessorBasePtr processor; ProcessorBasePtr processor;
// ProcessorIMUPtr processor_imu; // ProcessorIMUPtr processor_imu;
Eigen::VectorXd expected_final_state; VectorXd expected_final_state;
Eigen::VectorXd x_origin; VectorXd x_origin;
Eigen::MatrixXd P_origin; MatrixXd P_origin;
FrameBasePtr KF0; FrameBasePtr KF0;
virtual void SetUp() virtual void SetUp()
{ {
// using std::shared_ptr; // using shared_ptr;
// using std::make_shared; // using make_shared;
// using std::static_pointer_cast; // using static_pointer_cast;
//===================================================== SETTING PROBLEM //===================================================== SETTING PROBLEM
// WOLF PROBLEM // WOLF PROBLEM
...@@ -90,12 +94,12 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test ...@@ -90,12 +94,12 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test
ceres_manager = new CeresManager(problem, ceres_options); ceres_manager = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU ---- ALL OF THAT JUST TO HAVE A PROCESSOR MOTION AT HAND... // 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"); // processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
// sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); // sen_imu = static_pointer_cast<SensorIMU>(sen0_ptr);
// processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); // processor_imu = static_pointer_cast<ProcessorIMU>(processor);
//===================================================== INITIALIZATION //===================================================== INITIALIZATION
x_origin.resize(10); x_origin.resize(10);
...@@ -111,51 +115,68 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test ...@@ -111,51 +115,68 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test
KF0->getO()->fix(); KF0->getO()->fix();
KF0->getV()->fix(); KF0->getV()->fix();
Eigen::Vector3d zero3; zero3 << 0,0,0; Vector3d zero3; zero3 << 0,0,0;
Eigen::Vector6d zero6; zero6 << zero3, zero3; Vector6d zero6; zero6 << zero3, zero3;
StateBlockPtr sbc = std::make_shared<StateBlock>(zero3); StateBlockPtr sbc = make_shared<StateBlock>(zero3);
StateBlockPtr sbd = std::make_shared<StateBlock>(zero3); StateBlockPtr sbd = make_shared<StateBlock>(zero3);
StateBlockPtr sbL = std::make_shared<StateBlock>(zero3); StateBlockPtr sbL = make_shared<StateBlock>(zero3);
StateBlockPtr sbb = std::make_shared<StateBlock>(zero3); StateBlockPtr sbb = make_shared<StateBlock>(zero3);
StateBlockPtr sbbimu = std::make_shared<StateBlock>(zero6); StateBlockPtr sbbimu = make_shared<StateBlock>(zero6);
KF0->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD); KF0->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD);
KF0->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD); KF0->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD);
KF0->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD); KF0->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD);
KF0->addStateBlock("B", sbb); problem->notifyStateBlock(sbb,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 // =============== SET KIN MEASUREMENT AND COVARIANCE
// Measurements // Measurements
Eigen::Vector3d pBC_meas; pBC_meas << 1,1,1; Vector3d pBC_meas; pBC_meas << 0,0,0;
Eigen::Vector3d bias_p; bias_p << 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; Vector3d w_meas; w_meas << 0,0,0;
Eigen::Vector3d bias_w; bias_w << 0,0,0; Vector3d bias_w; bias_w << 0,0,0;
// momentum parameters // momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity(); Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq; Lq.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(); SelfAdjointEigenSolver<Matrix9d> eigensolver(Qkin);
Eigen::Matrix3d Qv; Qv.setIdentity(); if (eigensolver.info() != Success) abort();
Eigen::Matrix3d Qw; Qw.setIdentity(); cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
Eigen::Matrix9d Qkin = computeKinCov(Qp, Qv, Qw, pBC_meas-bias_p, w_meas-bias_w); 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; // LLT<Matrix9d> lltOfQkin(Qkin); // compute the Cholesky decomposition of A
std::cout << "Qv\n" << Qv << std::endl; // if(lltOfQkin.info() == NumericalIssue)
std::cout << "Qw\n" << Qw << std::endl; // {
std::cout << "Qkin\n" << Qkin << std::endl; // throw runtime_error("Possibly non semi-positive definitie matrix!");
// }
// =============== CREATE CAPURE/FEATURE/FACTOR // =============== 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); 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 = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
// auto feat_in = std::static_pointer_cast<FeatureInertialKinematics>(feat); // auto feat_in = static_pointer_cast<FeatureInertialKinematics>(feat);
// sbbimu->fix(); // sbbimu->fix();
// auto fac = FactorBase::emplace<FactorInertialKinematics>(feat, feat, sbbimu); // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat, feat, sbbimu);
...@@ -169,8 +190,8 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test ...@@ -169,8 +190,8 @@ class FactorInertialKinematics_ZeroMvt : public testing::Test
TEST_F(FactorInertialKinematics_ZeroMvt,ZeroMvt) TEST_F(FactorInertialKinematics_ZeroMvt,ZeroMvt)
{ {
problem->print(3,0,1,1); problem->print(3,0,1,1);
std::cout << "SOLVING" << std::endl; cout << "SOLVING" << endl;
// std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; // string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// ASSERT_MATRIX_APPROX(KF0->getState(), KF0->getState(), 1e-5); // ASSERT_MATRIX_APPROX(KF0->getState(), KF0->getState(), 1e-5);
ASSERT_TRUE(true); ASSERT_TRUE(true);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment