Skip to content
Snippets Groups Projects
Commit 6aefcde5 authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

gtest for constraint_block_absolute passing

parent fb4378ac
No related branches found
No related tags found
1 merge request!149Constraint abs
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment