fix : intialize time for origin

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