Minor style improvements and doc

include/vision/factor/factor_pixel_hp.h

@@ -130,13 +130,19 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
 {
     using namespace Eigen;
 
-    // frames
+    // frames w: world; r: robot; c: camera
     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);
+
+    // camera pose in world frame: transforms from camera to world
     Matrix<T, 3, 1> p_wc = p_wr + q_wr * p_rc;
     Quaternion<T>   q_wc = q_wr*q_rc;
+
+    // invert transform: from world to camera
+    // | R T |.inv = | R.tr -R.tr*T | == | q.conj -q.conj*T |
+    // | 0 1 |       |  0      1    |    |   0        1     |
     Quaternion<T>   q_cw = q_wc.conjugate();
     Matrix<T, 3, 1> p_cw = - (q_cw * p_wc);
 
@@ -148,7 +154,7 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
      * | q T | * | v | = | q*v + T+w | --> v' = q*v + T+w
      * | 0 1 |   | w |   |  0     w  |
      */
-    Matrix<T, 3, 1> v_dir = q_cw*lh_w.template head<3>() + p_cw * lh_w(3);
+    Matrix<T, 3, 1> v_dir = q_cw * lh_w.template head<3>() + p_cw * lh_w(3);
 
     // project point and exit
     Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);

include/vision/math/pinhole_tools.h

@@ -272,8 +272,7 @@ Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1>
     EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
     MatrixSizeCheck<2,1>::check(up);
 
-    SizeEigen n = d.size();
-    if (n == 0)
+    if (d.size() == 0)
         return up;
     else {
         typename Derived2::Scalar r2 = up(0) * up(0) + up(1) * up(1); // this is the norm squared: r2 = ||u||^2