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

New test cases for factor block difference

parent e9d70537
No related branches found
No related tags found
1 merge request!336New factor block difference
Pipeline #4886 passed
This commit is part of merge request !336. Comments created here will be created in the context of that merge request.
......@@ -15,6 +15,7 @@
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_block_difference.h"
#include "core/factor/factor_block_absolute.h"
using namespace Eigen;
......@@ -47,8 +48,14 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
KF0_ =problem_->setPrior(problem_->zeroState(), cov_prior, t0, 0.1);
KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
// KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
......@@ -68,21 +75,68 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
Feat_, KF0_->getP(), KF1_->getP()
);
KF1_->getO()->fix();
KF1_->getV()->fix();
// perturbate
KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
problem_->print(4,1,1,1);
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << report << std::endl;
problem_->print(4,1,1,1);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, EqualV)
{
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getV(), KF1_->getV()
);
// perturbate
KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, DiffP)
{
Vector3d diffP = Vector3d::Random();
Feat_->setMeasurement(diffP);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP()
);
// perturbate
KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diffP, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, DiffV)
{
Vector3d diffV = Vector3d::Random();
Feat_->setMeasurement(diffV);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getV(), KF1_->getV()
);
// perturbate
KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diffV, 1e-8);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, 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