From 74f83a4ef86a7e3f8d2cb92644ad4a7929963eb0 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Mon, 28 Nov 2016 11:06:27 +0100
Subject: [PATCH] constraintOdom3D added
---
src/examples/test_imu_constrained0.cpp | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 77e10258c..cf37ec887 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -126,7 +126,11 @@ int main(int argc, char** argv)
FeatureBasePtr last_feature = std::make_shared<FeatureBase>("ODOM_3D", origin_state.head(7),Eigen::Matrix7s::Identity());
//create an ODOM constraint between first and last keyframes
- ConstraintOdom3DPtr constraint_ptr = std::make_shared<ConstraintOdom3D>(last_feature, last_frame);
+ ConstraintOdom3DPtr constraintOdom_ptr = std::make_shared<ConstraintOdom3D>(last_feature, origin_frame);
+ last_feature -> addConstraint(constraintOdom_ptr);
+
+ Eigen::Vector7s expec;
+ expec = constraintOdom_ptr -> expectation();
clock_t end = clock();
double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
--
GitLab