From c39002c753a2a997daaec33760254aeebcadb69e Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Fri, 15 Dec 2017 14:40:50 +0100
Subject: [PATCH] test for constraint_quaternion_absolute in
gtest_constraint_absolute.cpp
---
src/test/gtest_constraint_absolute.cpp | 28 ++++++++++++++++++++++++++
1 file changed, 28 insertions(+)
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 143a593e0..e706f0616 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -11,6 +11,7 @@
#include "constraint_absolute_orientation.h"
#include "constraint_absolute_velocity.h"
#include "constraint_block_absolute.h"
+#include "constraint_quaternion_absolute.h"
#include "capture_motion.h"
#include "ceres_wrapper/ceres_manager.h"
@@ -184,6 +185,33 @@ TEST(ConstraintBlockAbs, ctr_bloc_abs_v_solve)
ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
}
+TEST(ConstraintQuatAbs, ctr_bloc_abs_o_check)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
+ );
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ConstraintQuatAbs, ctr_bloc_abs_o_solve)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
+ );
+
+ // 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().segment<4>(3), pose10.segment<4>(3), 1e-6);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
--
GitLab