MSCKF::marginalize wrong index in p_f_G_vec ?

[chanochh]

I saw an out of bounds crash while running in Debug mode.

In msckf.h line 419 assumes that feature_tracks_to_residualize_[iter]
corresponds to p_f_G_vec[iter]

However, the loop in line 352, which push_backs to p_f_G_vec
has a continue instruction in line 358, which skips the push_back,
So the this might be false

It also seems that p_f_G_vec is not needed at all, because line 370 saves the p_f_G inside
the track itself (if it is valid)

(current commit d51c9ee)