Recode factor with quaternion algebra

include/vision/factor/factor_pixel_hp.h

@@ -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);
 
 }