From 1ef7bb5a063575758262603941e2d4c7ea8f48f7 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Fri, 15 Dec 2017 13:51:13 +0100
Subject: [PATCH] gtest constraintBlockAbs -> test p
---
src/test/gtest_constraint_absolute.cpp | 31 ++++++++++++++++++++++++++
1 file changed, 31 insertions(+)
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index b75b48574..7b1d5baeb 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>()));
--
GitLab