diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp index 1056326f82fe75bd6b86ff4130e2fbbfeab917eb..b75b4857449fa226b15dc2a5a762a25d270f6be4 100644 --- a/src/test/gtest_constraint_absolute.cpp +++ b/src/test/gtest_constraint_absolute.cpp @@ -10,6 +10,7 @@ #include "constraint_absolute_position.h" #include "constraint_absolute_orientation.h" #include "constraint_absolute_velocity.h" +#include "constraint_block_absolute.h" #include "capture_motion.h" #include "ceres_wrapper/ceres_manager.h" @@ -121,6 +122,37 @@ TEST(ConstraintAbsV, ctr_abs_v_solve) ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6); } +TEST(ConstraintBlockAbs, ctr_bloc_abs_v_check) +{ + FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); + ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( + fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr(), + fea0->getMeasurement(), + data_cov.bottomRightCorner<3,3>())) + ); + ASSERT_TRUE(problem->check(0)); +} + +TEST(ConstraintBlockAbs, ctr_bloc_abs_v_solve) +{ + FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); + ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( + fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr(), + fea0->getMeasurement(), + data_cov.bottomRightCorner<3,3>())) + ); + + // Fix frame 0, perturb frm1 + frm0->unfix(); + frm0->setState(x0); + + // solve for frm0 + std::string brief_report = ceres_mgr.solve(1); + + //only velocity is constrained + ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);