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

added all gtests

parent ea0091ac
No related branches found
No related tags found
3 merge requests!43imu2d,!42Merge imu2d,!30WIP: slanted imu2d
......@@ -91,345 +91,56 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
}
}
//TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
//{
// for(int i = 0; i < 50; i++){
// // Random delta
// Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
// delta(2) = pi2pi(delta(2));
//
// // Random initial pose
// Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
// x0(2) = pi2pi(x0(2));
//
// // x1 groundtruth
// Vector5d x1;
// x1 = imu2d::composeOverState(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm0 and biases, perturb frm1
// frm0->unfix();
// cap0->fix();
// frm1->fix();
// cap1->fix();
// frm0->perturb(0.01);
//
// // solve
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
// //WOLF_INFO(frm1->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
//}
//}
//
//TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
//{
// for(int i = 0; i < 50; i++){
// // Random delta
// Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
// delta(2) = pi2pi(delta(2));
//
// // Random initial pose
// Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
// x0(2) = pi2pi(x0(2));
//
// // x1 groundtruth
// Vector5d x1;
// x1 = imu2d::composeOverState(x0, delta, 1);
//
// //0 Initial bias
// Vector3d b0 = Vector3d::Zero();
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
// frm0->fix();
// cap0->fix();
// frm1->fix();
// cap1->unfix();
// cap1->perturb(0.01);
//
// // solve to estimate bias at cap1
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
// //WOLF_INFO(cap1->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
//}
//}
//
//TEST(FactorImu2dWithGravity, 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::composeOverState(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm1, perturb frm0
// frm0->fix();
// cap0->unfix();
// frm1->fix();
// cap1->fix();
// cap0->perturb(0.1);
//
// // 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(FactorImu2dWithGravity, solve_b1)
//{
// for(int i = 0; i < 50; i++){
// // Random delta
// Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
// delta(2) = pi2pi(delta(2));
//
// // Random initial pose
// Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
// x0(2) = pi2pi(x0(2));
//
// // x1 groundtruth
// Vector5d x1;
// x1 = imu2d::composeOverState(x0, delta, 1);
//
// //0 Initial bias
// Vector3d b0 = Vector3d::Random();
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm0 and frm1, unfix bias, perturb cap1
// frm0->fix();
// cap0->fix();
// frm1->fix();
// cap1->unfix();
// cap1->perturb(0.01);
//
// // solve to estimate bias at cap1
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
// //WOLF_INFO(cap1->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
//}
//}
//
//TEST(FactorImu2dWithGravity, solve_f0)
//{
// 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::composeOverState(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm1, perturb frm0
// frm0->unfix();
// cap0->fix();
// frm1->fix();
// cap1->fix();
// frm0->perturb(0.1);
//
// // solve
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
// //WOLF_INFO(frm0->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
//}
//}
//
//TEST(FactorImu2dWithGravity, solve_f1)
//{
// 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::composeOverState(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm1, perturb frm0
// frm0->fix();
// cap0->fix();
// frm1->unfix();
// cap1->fix();
// frm1->perturb(0.1);
//
// // solve
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
// //WOLF_INFO(frm0->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
//}
//}
//
//TEST(FactorImu2dWithGravity, solve_f1_b1)
//{
// 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::composeOverState(x0, delta, 1);
//
// // Set states
// frm0->setState(x0);
// frm1->setState(x1);
// cap0->getStateBlock('I')->setState(b0);
// cap1->getStateBlock('I')->setState(b0);
//
// // feature & factor with delta measurement
// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
// FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
//
// // Fix frm1, perturb frm0
// frm0->fix();
// cap0->fix();
// frm1->unfix();
// cap1->unfix();
// frm1->perturb(0.1);
// cap1->perturb(0.1);
//
// // solve
// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
//
// ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
// ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
// //WOLF_INFO(frm0->getStateVector());
//
// // remove feature (and factor) for the next loop
// fea1->remove();
// }
//}
TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
{
for(int i = 0; i < 50; i++){
// Random delta
Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
delta(2) = pi2pi(delta(2));
TEST(FactorImu2dWithGravity, bias_zero_solve_G)
// Random initial pose
Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
x0(2) = pi2pi(x0(2));
//Random gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
// x1 groundtruth
Vector5d x1;
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1 and biases, perturb frm0
frm0->unfix();
cap0->fix();
frm1->fix();
cap1->fix();
sensor->getStateBlock('G')->fix();
frm0->perturb(0.01);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
//WOLF_INFO(frm1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
{
for(int i = 0; i < 50; i++){
//WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
// Random delta
Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
delta(2) = pi2pi(delta(2));
......@@ -445,17 +156,353 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_G)
// x1 groundtruth
Vector5d x1;
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
//0 Initial bias
Vector3d b0 = Vector3d::Zero();
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm0 and biases, perturb frm1
// Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
frm0->fix();
cap0->fix();
frm1->fix();
sensor->getStateBlock('G')->fix();
cap1->unfix();
cap1->perturb(0.01);
// solve to estimate bias at cap1
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
//WOLF_INFO(cap1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, 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 gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
//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::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1, perturb frm0
frm0->fix();
cap0->unfix();
frm1->fix();
cap1->fix();
sensor->getStateBlock('G')->fix();
cap0->perturb(0.1);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
//WOLF_INFO(cap0->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, solve_b1)
{
for(int i = 0; i < 50; i++){
// Random delta
Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
delta(2) = pi2pi(delta(2));
// Random initial pose
Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
x0(2) = pi2pi(x0(2));
//Random gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
// x1 groundtruth
Vector5d x1;
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
//0 Initial bias
Vector3d b0 = Vector3d::Random();
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm0 and frm1, unfix bias, perturb cap1
frm0->fix();
cap0->fix();
frm1->fix();
sensor->getStateBlock('G')->fix();
cap1->unfix();
cap1->perturb(0.01);
// solve to estimate bias at cap1
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
//WOLF_INFO(cap1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, solve_f0)
{
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 gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
//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::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1, perturb frm0
frm0->unfix();
cap0->fix();
frm1->fix();
cap1->fix();
sensor->getStateBlock('G')->fix();
frm0->perturb(0.1);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
//WOLF_INFO(frm0->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, solve_f1)
{
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 gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
//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::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1, perturb frm0
frm0->fix();
cap0->fix();
frm1->unfix();
cap1->fix();
sensor->getStateBlock('G')->fix();
frm1->perturb(0.1);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
//WOLF_INFO(frm0->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, solve_f1_b1)
{
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 gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
//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::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1, perturb frm0
frm0->fix();
cap0->fix();
sensor->getStateBlock('G')->fix();
frm1->unfix();
cap1->unfix();
frm1->perturb(0.1);
cap1->perturb(0.1);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
//WOLF_INFO(frm0->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorImu2dWithGravity, bias_zero_solve_G)
{
for(int i = 0; i < 50; i++){
//WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
// Random delta
Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
delta(2) = pi2pi(delta(2));
// Random initial pose
Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
x0(2) = pi2pi(x0(2));
//Random gravity
//Vector2d g = Vector2d::Random()*5;
Vector2d g = Vector2d::Zero();
//Zero bias
Vector3d b0 = Vector3d::Zero();
// x1 groundtruth
Vector5d x1;
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
frm0->setState(x0);
frm1->setState(x1);
cap0->getStateBlock('I')->setState(b0);
cap1->getStateBlock('I')->setState(b0);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frames and biases, perturb g
frm0->fix();
cap0->fix();
frm1->fix();
......@@ -465,14 +512,13 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_G)
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
//WOLF_INFO(frm1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
}
TEST(FactorImu2dWithGravity, solve_G)
{
......@@ -485,13 +531,13 @@ TEST(FactorImu2dWithGravity, solve_G)
Vector5d x0 = Vector5d::Random() * 10;
x0(2) = pi2pi(x0(2));
//Random Initial bias
Vector3d b0 = Vector3d::Random();
//Random gravity
Vector2d g = Vector2d::Random()*5;
//Vector2d g = Vector2d::Zero();
//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);
......@@ -511,7 +557,7 @@ TEST(FactorImu2dWithGravity, solve_G)
auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
// Fix frm1, perturb frm0
// Fix frames and captures, perturb g
frm0->fix();
cap0->fix();
frm1->fix();
......@@ -520,14 +566,14 @@ TEST(FactorImu2dWithGravity, solve_G)
sensor->getStateBlock('G')->perturb(1);
// solve
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
//WOLF_INFO(cap0->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
}
int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment