diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index 77e10258c759f205ec8ffe351e2562f077b5ea03..cf37ec8879c6a8ac14161e563e1a9416ad60683d 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;