From 05312c25a2d4051858ded0669290d9a1b83a0942 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Mon, 21 Nov 2016 15:18:32 +0100
Subject: [PATCH] fix : intialize time for origin
---
src/examples/test_imu_constraintAHP.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/examples/test_imu_constraintAHP.cpp b/src/examples/test_imu_constraintAHP.cpp
index 10234479b..127b096b3 100644
--- a/src/examples/test_imu_constraintAHP.cpp
+++ b/src/examples/test_imu_constraintAHP.cpp
@@ -55,9 +55,9 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PVQBB_3D);
- Eigen::VectorXs extrinsics(7);
- extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, shared_ptr<IntrinsicsBase>());
+ Eigen::VectorXs IMU_extrinsics(7);
+ IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+ SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>());
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
@@ -65,6 +65,8 @@ int main(int argc, char** argv)
Eigen::Vector6s data_;
Scalar mpu_clock = 0;
+ t.set(mpu_clock);
+
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0, 0,0,0,1, 0,0,.001, 0,0,.002; // Try some non-zero biases
--
GitLab