From a7068c169f3947c77cad8c1cd2d8f7a8ec93c929 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Tue, 13 Dec 2016 14:42:46 +0100
Subject: [PATCH] change data for preintegration
---
src/test/gtest_feature_imu.cpp | 21 +++++++++++++++------
1 file changed, 15 insertions(+), 6 deletions(-)
diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp
index 5d630429c..40d88feda 100644
--- a/src/test/gtest_feature_imu.cpp
+++ b/src/test/gtest_feature_imu.cpp
@@ -45,13 +45,12 @@ class FeatureIMU_test : public testing::Test
// Time and data variables
TimeStamp t;
Eigen::Vector6s data_;
- Scalar mpu_clock = 0;
- t.set(mpu_clock);
+ t.set(0);
// Set the origin
Eigen::VectorXs x0(16);
- x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
+ x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
//create a keyframe at origin
@@ -65,9 +64,10 @@ class FeatureIMU_test : public testing::Test
imu_ptr->setFramePtr(origin_frame);
//process data
- mpu_clock = 0.001003;
- data_ << 0.579595, -0.143701, 9.939331, 0.127445, 0.187814, -0.055003;
- t.set(mpu_clock);
+ data_ << 2, 0, 9.8, 0, 0, 0;
+ t.set(0.1);
+ // Expected state after one integration
+ //x << 0.01,0,0, 0,0,0,1, 0.2,0,0, 0,0,0, 0,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
// assign data to capture
imu_ptr->setData(data_);
imu_ptr->setTimeStamp(t);
@@ -135,6 +135,15 @@ TEST_F(FeatureIMU_test, check_frame)
//previous_frame->addConstrainedBy(constraint_imu);
}
+TEST_F(FeatureIMU_test, access_members)
+{
+ using namespace wolf;
+
+ // Expected state after one integration
+ Eigen::VectorXs x(16);
+ x << 0.01,0,0, 0,0,0,1, 0.2,0,0, 0,0,0, 0,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
+}
+
TEST_F(FeatureIMU_test, addConstraint)
{
using namespace wolf;
--
GitLab