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

Minor style improvements and doc

parent 00f9087c
......@@ -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);
......
......@@ -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
......
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