From e2030a46a6f6a92ecb33c8482dbc31bba8ed1bdf Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Mon, 28 Nov 2016 15:20:27 +0100
Subject: [PATCH] create constraintIMU between keyframes
---
src/examples/test_imu_constrained0.cpp | 21 +++++++++++++++++++--
1 file changed, 19 insertions(+), 2 deletions(-)
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index cf37ec887..76abfee3a 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -88,17 +88,33 @@ int main(int argc, char** argv)
const int keyframe_spacing = 10;
int last_keyframe_dt = 0;
Eigen::VectorXs state_vec;
- FrameBasePtr last_frame;
+ Eigen::VectorXs delta_preint;
+ FrameIMUPtr last_frame;
+ FrameIMUPtr previous_frame;
+ Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
while(!data_file.eof()){
if(last_keyframe_dt >= keyframe_spacing){
+ previous_frame = last_frame; //to constraint the new frame and link it to previous one
ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
-
last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
//FrameBasePtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts_.get(),std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ //create a feature
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
+
+ //create a constraintIMU
+ //wolf_problem_ptr_->getProcessorMotionPtr()->emplaceConstraint(feat_imu, previous_frame);
+ ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, previous_frame);
+ feat_imu->addConstraint(constraint_imu);
+ previous_frame->addConstrainedBy(constraint_imu);
+
+ //reset origin of motion to new frame
+ wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(last_frame);
last_keyframe_dt = 0;
}
else
@@ -128,6 +144,7 @@ int main(int argc, char** argv)
//create an ODOM constraint between first and last keyframes
ConstraintOdom3DPtr constraintOdom_ptr = std::make_shared<ConstraintOdom3D>(last_feature, origin_frame);
last_feature -> addConstraint(constraintOdom_ptr);
+ origin_frame -> addConstrainedBy(constraintOdom_ptr);
Eigen::Vector7s expec;
expec = constraintOdom_ptr -> expectation();
--
GitLab