Skip to content
Snippets Groups Projects
Commit d22f42c6 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch '14-follow-core-422' into 'devel'

Resolve "Follow core 422"

Closes #14

See merge request !14
parents 7fc268e7 a24ac20e
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!14Resolve "Follow core 422"
......@@ -512,7 +512,7 @@ int main (int argc, char **argv) {
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
// - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
// - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
TimeStamp t0(t_arr[0]);
VectorXd x_origin; x_origin.resize(19);
x_origin << p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0], c_traj_arr[0], dc_traj_arr[0], L_traj_arr[0];
......
......@@ -298,7 +298,7 @@ int main (int argc, char **argv) {
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
// - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
// - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
......
......@@ -314,7 +314,7 @@ int main (int argc, char **argv) {
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
// - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
// - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
MatrixXd P_origin(9,9); P_origin.setIdentity();
P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
......
......@@ -222,7 +222,7 @@ public:
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually set problem prior
// - call setOrigin on processors isMotion
// - call setOrigin on processors MotionProvider
setOriginState();
MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
KF1_ = problem_->emplaceFrame( t0_, x_origin_);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment