Commit ff5102c9 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Recode factor with quaternion algebra

parent 73673fda
......@@ -130,37 +130,21 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
{
using namespace Eigen;
// All involved transforms typedef
typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// world to current robot transform
Map<const Matrix<T, 3, 1> > p_w_r(_frame_p);
Translation<T, 3> t_w_r(p_w_r);
Map<const Quaternion<T> > q_w_r(_frame_o);
TransformType T_w_r = t_w_r * q_w_r;
// current robot to current camera transform
Map<const Matrix<T, 3, 1> > p_r_c(_sensor_p);
Translation<T, 3> t_r_c(p_r_c);
Map<const Quaternion<T> > q_r_c(_sensor_o);
TransformType T_r_c = t_r_c * q_r_c;
// hmg point in current camera frame C
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
Eigen::Matrix<T, 4, 1> landmark_hmg_c = (T_w_r * T_r_c).inverse(Eigen::Isometry)
* landmark_hmg;
//std::cout << "p_w_r = \n\t" << _frame_p[0] << "\n\t" << _frame_p[1] << "\n\t" << _frame_p[2] << "\n";
// std::cout << "q_w_r = \n\t" << _frame_o[0] << "\n\t" << _frame_o[1] << "\n\t" << _frame_o[2] << "\n\t" << _frame_o[3] << "\n";
// std::cout << "p_r_c = \n\t" << _sensor_p[0] << "\n\t" << _sensor_p[1] << "\n\t" << _sensor_p[2] << "\n";
// std::cout << "q_r_c = \n\t" << _sensor_o[0] << "\n\t" << _sensor_o[1] << "\n\t" << _sensor_o[2] << "\n\t" << _sensor_o[3] << "\n";
// std::cout << "landmark_hmg_c = \n\t" << landmark_hmg_c(0) << "\n\t" << landmark_hmg_c(1) << "\n\t" << landmark_hmg_c(2) << "\n\t" << landmark_hmg_c(3) << "\n";
// lmk direction vector
Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
// lmk inverse distance
T rho = landmark_hmg_c(3);
// frames
Matrix<T, 3, 1> p_wr(_frame_p);
Quaternion<T> q_wr(_frame_o);
Matrix<T, 3, 1> p_rc(_sensor_p);
Quaternion<T> q_rc(_sensor_o);
Matrix<T, 3, 1> p_wc = p_wr + q_wr * p_rc;
Quaternion<T> q_wc = q_wr*q_rc;
Quaternion<T> q_cw = q_wc.conjugate();
Matrix<T, 3, 1> p_cw = - (q_cw * p_wc);
// landmark hmg
Matrix<T, 4, 1> lh_w(_lmk_hmg);
// landmark dir vector in C frame
Matrix<T, 3, 1> v_dir = q_cw*lh_w.template head<3>() + p_cw * lh_w(3);
// camera parameters
Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
......@@ -168,7 +152,7 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
// project point and exit
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
expectation = pinhole::projectPoint(intrinsic, distortion, v_dir/rho);
expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment