From 6aefcde58710f5b7dabf34d330c23e864d2919d5 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Fri, 15 Dec 2017 13:42:54 +0100
Subject: [PATCH] gtest for constraint_block_absolute passing
---
src/test/gtest_constraint_absolute.cpp | 32 ++++++++++++++++++++++++++
1 file changed, 32 insertions(+)
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 1056326f8..b75b48574 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);
--
GitLab