Skip to content
Snippets Groups Projects
Commit c3a5bbf7 authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

added same test for 3D case

parent 4170d1a1
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
Hide whitespace changes
Inline Side-by-side
demos/imu_static_init.yaml 0 → 100644
+ 12
− 0
View file @ c3a5bbf7
type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
keyframe_vote:
voting_active: true
voting_aux_active: false
max_time_span: 1.0 # seconds
max_buff_length: 20000 # motion deltas
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
test/CMakeLists.txt
+ 3
− 0
View file @ c3a5bbf7
......@@ -37,6 +37,9 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
......
test/gtest_imu2d_static_init.cpp
+ 38
− 126
View file @ c3a5bbf7
......@@ -23,6 +23,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
double mti_clock, tmp;
double dt;
bool second_frame;
bool third_frame;
Vector6d data;
Matrix6d data_cov;
VectorComposite x0c; // initial state composite
......@@ -52,6 +53,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
// Time and data variables
second_frame = false;
third_frame = false;
dt = 0.01;
t0.set(0);
t = t0;
......@@ -68,122 +70,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
};
//// Input odometry data and covariance
//Matrix6d data_cov = 0.1*Matrix6d::Identity();
//Matrix5d delta_cov = 0.1*Matrix5d::Identity();
//
//// Jacobian of the bias
//MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
// 0, 1, 0,
// 0, 0, 1,
// 1, 0, 0,
// 0, 1, 0 ).finished());
////preintegration bias
//Vector3d b0_preint = Vector3d::Zero();
//
//// Problem and solver
//ProblemPtr _problem_ptr_ptr = Problem::create("POV", 2);
//SolverCeres solver(problem_ptr);
//
//// Two frames
//FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
//FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
//
//// Imu2d sensor
//SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
//
////capture from frm1 to frm0
//auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
//auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
//
//TEST(imu2d_static_init, check_tree)
//{
// ASSERT_TRUE(problem_ptr->check(0));
//}
//
//
//TEST(imu2d_static_init, solve_b0)
//{
// for(int i = 0; i < 50; i++){
// // Random delta
// Vector5d delta_biased = Vector5d::Random() * 10;
// delta_biased(2) = pi2pi(delta_biased(2));
//
// // Random initial pose
// Vector5d x0 = Vector5d::Random() * 10;
// x0(2) = pi2pi(x0(2));
//
// //Random Initial bias
// Vector3d b0 = Vector3d::Random();
//
// //Corrected delta
// Vector5d delta_step = jacobian_bias*(b0-b0_preint);
// Vector5d delta = imu2d::plus(delta_biased, delta_step);
//
// // x1 groundtruth
// Vector5d x1;
// x1 = imu2d::compose(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// frm0->getV()->setZero();
// frm0->getV()->fix();
// frm1->getV()->setZero();
// frm1->getV()->fix();
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
// //WOLF_INFO("states set");
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
// FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm1, perturb frm0
// frm0->fix();
// cap0->unfix();
// frm1->fix();
// cap1->fix();
// cap0->perturb(0.1);
//
// //do static init stuff on frm1
// Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(3);
// //WOLF_INFO("zero_motion created");
// for (auto frm_pair = problem_ptr->getTrajectory()->begin();
// frm_pair != problem_ptr->getTrajectory()->end();
// frm_pair++)
// {
// //WOLF_INFO("entered for");
// if (frm_pair->second == frm1)
// break;
// auto capture_zero = CaptureBase::emplace<CaptureVoid>(frm1, frm1->getTimeStamp(), nullptr);
// auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
// "FeatureZeroOdom",
// zero_motion,
// Eigen::MatrixXd::Identity(3,3) * 0.01);
// FactorBase::emplace<FactorRelativePose2d>(feature_zero,
// feature_zero,
// frm_pair->second,
// nullptr,
// false,
// TOP_MOTION);
//
// }
//
// // solve
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
// //WOLF_INFO(cap0->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
// }
//}
TEST_F(ProcessorImu2dStaticInitTest, static)
{
Vector2d pos;
// Set the origin
x0c['P'] = Vector2d(0, 0);
x0c['O'] = Vector1d(0);
......@@ -191,6 +80,8 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
data_cov *= 1e-3;
data << 1, 2, 3, 4, 5, 6;
Vector3d bias_groundtruth;
bias_groundtruth << data(0), data(1), data(5);
//dt = 0.0001;
auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
KF0->fix();
......@@ -202,7 +93,8 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
_first_frame = nullptr;
int i = 1;
for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
int n_frames = 0;
for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){
WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
++i;
C = std::make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
......@@ -215,6 +107,7 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
WOLF_INFO("Doing the static initialization stuff");
if (not _last_frame)
{
n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame();
_first_frame = _last_frame;
......@@ -233,6 +126,7 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
// new frame
if (_last_frame != _processor_motion->getOrigin()->getFrame())
{
n_frames++;
second_frame = true;
_last_frame = _processor_motion->getOrigin()->getFrame();
......@@ -271,26 +165,44 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
WOLF_INFO("Static initialization done");
if(_first_frame)
{
WOLF_INFO("0 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
WOLF_INFO_COND(second_frame, "The second frame has been created");
WOLF_INFO("1 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
WOLF_INFO("2 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("3 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
//WOLF_INFO("0 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created");
//WOLF_INFO("1 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
//WOLF_INFO("2 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("3 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
}
//WOLF_INFO("The current problem is:");
//_problem_ptr->print(4);
WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
auto state = _problem_ptr->getState();
auto bias_est = _sensor_ptr->getIntrinsic()->getState();
auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
WOLF_INFO("Solved");
if(_first_frame)
{
WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
WOLF_INFO_COND(second_frame, "The second frame has been created");
WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
//WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created");
//WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
//WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
WOLF_INFO("Current frame is", _processor_motion->getLast()->id());
WOLF_INFO("The state is \n:", state);
WOLF_INFO("The estimated bias is: \n", bias_est);
WOLF_INFO("The preintegrated bias is: \n", bias_preint);
}
if(second_frame)
{
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
}
if (n_frames > 2)
{
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
}
WOLF_INFO("------------------------------------------------------------------------\n");
}
......
test/gtest_imu_static_init.cpp 0 → 100644
+ 217
− 0
View file @ c3a5bbf7
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "imu/internal/config.h"
#include "imu/factor/factor_imu.h"
#include "imu/math/imu_tools.h"
#include "imu/sensor/sensor_imu.h"
#include "core/capture/capture_void.h"
#include <core/factor/factor_relative_pose_3d.h>
using namespace Eigen;
using namespace wolf;
class ProcessorImuStaticInitTest : public testing::Test
{
public: //These can be accessed in fixtures
wolf::ProblemPtr _problem_ptr;
wolf::SensorBasePtr _sensor_ptr;
wolf::ProcessorMotionPtr _processor_motion;
wolf::TimeStamp t;
wolf::TimeStamp t0;
double mti_clock, tmp;
double dt;
bool second_frame;
Vector6d data;
Matrix6d data_cov;
VectorComposite x0c; // initial state composite
VectorComposite s0c; // initial sigmas composite
std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr _first_frame;
FrameBasePtr _last_frame;
SolverCeresPtr _solver;
//a new of this will be created for each new test
void SetUp() override
{
WOLF_INFO("Doing setup...");
using namespace Eigen;
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
using namespace wolf::Constants;
std::string wolf_root = _WOLF_IMU_ROOT_DIR;
// Wolf problem
_problem_ptr = Problem::create("POV", 3);
Vector7d extrinsics = (Vector7d() << 0,0,0, 0, 0,0,0).finished();
_sensor_ptr = _problem_ptr->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu_static_init.yaml");
_processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
second_frame = false;
dt = 0.01;
t0.set(0);
t = t0;
data = Vector6d::Random();
data_cov = Matrix6d::Identity();
_last_frame = nullptr;
// Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
C = make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
// Create the solver
_solver = make_shared<SolverCeres>(_problem_ptr);
}
};
TEST_F(ProcessorImuStaticInitTest, static)
{
WOLF_INFO("Starting Test");
// Set the origin
x0c['P'] = Vector3d(0, 0, 0);
x0c['O'] = Vector4d(0, 0, 0, 1);
x0c['V'] = Vector3d(0, 0, 0);
data_cov *= 1e-3;
data << 1, 2, 3, 4, 5, 6;
Vector6d bias_groundtruth;
bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
bias_groundtruth.tail(3) = data.tail(3);
//dt = 0.0001;
auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
KF0->fix();
_processor_motion->setOrigin(KF0);
WOLF_INFO("Data is: \n", data);
int size = 7;
_first_frame = nullptr;
int i = 1;
int n_frames = 0;
for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
++i;
C = std::make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
WOLF_INFO("Created new Capture");
if(second_frame){
//ASSERT_MATRIX_APPROX(_processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
}
WOLF_INFO("Processing capture");
C->process();
WOLF_INFO("Doing the static initialization stuff");
if (not _last_frame)
{
n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame();
_first_frame = _last_frame;
// impose zero velocity
_last_frame->getV()->setZero();
_last_frame->getV()->fix();
// impose zero odometry
_processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
}
else
{
assert(_processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
assert(_processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
// new frame
if (_last_frame != _processor_motion->getOrigin()->getFrame())
{
n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame();
// impose zero velocity
_last_frame->getV()->setZero();
_last_frame->getV()->fix();
// impose zero odometry
_processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
// add zero displacement and rotation capture & feature & factor with all previous frames
assert(_sensor_ptr->getProblem());
Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end();
frm_pair++)
{
if (frm_pair->second == _last_frame)
break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(_last_frame, _last_frame->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
"FeatureZeroOdom",
zero_motion,
Eigen::MatrixXd::Identity(6,6) * 0.01);
FactorBase::emplace<FactorRelativePose3d>(feature_zero,
feature_zero,
frm_pair->second,
nullptr,
false,
TOP_MOTION);
}
}
}
WOLF_INFO("Static initialization done");
if(_first_frame)
{
//WOLF_INFO("0 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created");
//WOLF_INFO("1 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
//WOLF_INFO("2 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("3 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
}
//WOLF_INFO("The current problem is:");
//_problem_ptr->print(4);
WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
auto state = _problem_ptr->getState();
auto bias_est = _sensor_ptr->getIntrinsic()->getState();
auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
WOLF_INFO("Solved");
if(_first_frame)
{
//WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created");
//WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
//WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
WOLF_INFO("Current frame is", _processor_motion->getLast()->id());
WOLF_INFO("The state is \n:", state);
WOLF_INFO("The estimated bias is: \n", bias_est);
WOLF_INFO("The preintegrated bias is: \n", bias_preint);
}
if(n_frames > 1)
{
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
}
if (n_frames > 2)
{
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
}
WOLF_INFO("------------------------------------------------------------------------\n");
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
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