diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d32d5c6216c857ad703c34aa1a10f4b29d6c4dc0..db82caf1fc34b83b2509fe7d4f3ac4d6cc9e8c82 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -47,13 +47,9 @@ add_library(dummy ${SRC_DUMMY}) wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp) target_link_libraries(gtest_capture_base ${PROJECT_NAME}) -# CaptureBase class test -#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp) -#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME}) - -# FactorBlockDifference class test -wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp) -target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME}) +# FactorBase class test +wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp) +target_link_libraries(gtest_factor_base ${PROJECT_NAME}) # FactorAutodiff class test wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) @@ -169,6 +165,10 @@ target_link_libraries(gtest_factor_absolute ${PROJECT_NAME}) wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp) target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME}) +# FactorBlockDifference class test +wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp) +target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME}) + # FactorOdom3d class test wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp) target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME}) diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af --- /dev/null +++ b/test/gtest_factor_base.cpp @@ -0,0 +1,120 @@ +/* + * gtest_factor_base.cpp + * + * Created on: Apr 2, 2020 + * Author: jsola + */ + + +#include "core/utils/utils_gtest.h" +#include "core/utils/logging.h" + +#include "core/factor/factor_base.h" + +using namespace wolf; +using namespace Eigen; + +class FactorBaseTest : public testing::Test +{ + public: + FrameBasePtr F0,F1; + CaptureBasePtr C0,C1; + FeatureBasePtr f0,f1; + LandmarkBasePtr L0,L1; + + virtual void SetUp() + { + F0 = std::make_shared<FrameBase>(0.0, nullptr); + F1 = std::make_shared<FrameBase>(1.0, nullptr); + C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr); + C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr); + f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + f1 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + L0 = std::make_shared<LandmarkBase>("Lmk", nullptr); + L1 = std::make_shared<LandmarkBase>("Lmk", nullptr); + } +// virtual void TearDown(){} +}; + +class FactorDummy : public FactorBase +{ + public: + FactorDummy(const FrameBasePtr& _frame_other, + const CaptureBasePtr& _capture_other, + const FeatureBasePtr& _feature_other, + const LandmarkBasePtr& _landmark_other) : + FactorBase("Dummy", + _frame_other, + _capture_other, + _feature_other, + _landmark_other, + nullptr, + false) + { + // + } + FactorDummy(const FrameBasePtrList& _frame_other_list, + const CaptureBasePtrList& _capture_other_list, + const FeatureBasePtrList& _feature_other_list, + const LandmarkBasePtrList& _landmark_other_list) : + FactorBase("Dummy", + _frame_other_list, + _capture_other_list, + _feature_other_list, + _landmark_other_list, + nullptr, + false) + { + // + } + virtual ~FactorDummy() = default; + + virtual std::string getTopology() const override {return "DUMMY";} + virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;} + virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {} + virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;} + virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;} + virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;} + virtual unsigned int getSize() const override {return 0;} + +}; + +TEST_F(FactorBaseTest, constructor_from_pointers) +{ + FactorDummy fac(nullptr,C0,f0,nullptr); + + ASSERT_TRUE(fac.getFrameOtherList().empty()); + + ASSERT_EQ(fac.getCaptureOtherList().size(), 1); + + ASSERT_EQ(fac.getFeatureOtherList().size(), 1); + + ASSERT_TRUE(fac.getLandmarkOtherList().empty()); +} + +TEST_F(FactorBaseTest, constructor_from_lists) +{ + FactorDummy fac({},{C0},{f0,f1},{}); + + ASSERT_TRUE(fac.getFrameOtherList().empty()); + + ASSERT_EQ(fac.getCaptureOtherList().size(), 1); + + ASSERT_EQ(fac.getFeatureOtherList().size(), 2); + + ASSERT_TRUE(fac.getLandmarkOtherList().empty()); +} + + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + // restrict to a group of tests only + //::testing::GTEST_FLAG(filter) = "TestInit.*"; + + // restrict to this test only + //::testing::GTEST_FLAG(filter) = "TestInit.testName"; + + return RUN_ALL_TESTS(); +}