diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp index b75b4857449fa226b15dc2a5a762a25d270f6be4..7b1d5baeb936b44521a3580a37eeb9a40e9dd6b7 100644 --- a/src/test/gtest_constraint_absolute.cpp +++ b/src/test/gtest_constraint_absolute.cpp @@ -122,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_p_check) +{ + FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); + ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( + fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(), + fea0->getMeasurement(), + data_cov.block<3,3>(3,3))) + ); + ASSERT_TRUE(problem->check(0)); +} + +TEST(ConstraintBlockAbs, ctr_bloc_abs_p_solve) +{ + FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); + ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( + fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(), + fea0->getMeasurement(), + data_cov.block<3,3>(3,3))) + ); + + // Fix frame 0, perturb frm1 + frm0->unfix(); + frm0->setState(x0); + + // solve for frm0 + std::string brief_report = ceres_mgr.solve(1); + + //only orientation is constrained + ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(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>()));