diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index 8069140510140bf2289e712980ccc811b74e1d56..0945b328de7d8094d49624aad6a89b0af5708788 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -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);