diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h index 5eda3bbccd9dbd03e776aad2801587f78c4dc939..eed7e421b4bc81bc58b9a0391eebd3ef61579dc3 100644 --- a/include/vision/factor/factor_pixel_hp.h +++ b/include/vision/factor/factor_pixel_hp.h @@ -152,7 +152,7 @@ inline void FactorPixelHp::expectation(const T* const _frame_p, // landmark dir vector in C frame /* note: transforming hmg point to get direction vector v': * | q T | * | v | = | q*v + T+w | --> v' = q*v + T+w - * | 0 1 | | w | | 0 w | + * | 0 1 | | w | | w | */ Matrix<T, 3, 1> v_dir = q_cw * lh_w.template head<3>() + p_cw * lh_w(3);