From e7047151f813183efc12921fb4c196c1d15b2dec Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Sun, 24 Apr 2022 20:06:41 +0200
Subject: [PATCH] Minor style improvements and doc
---
include/vision/factor/factor_pixel_hp.h | 10 ++++++++--
include/vision/math/pinhole_tools.h | 3 +--
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h
index d51cfd019..5eda3bbcc 100644
--- a/include/vision/factor/factor_pixel_hp.h
+++ b/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);
diff --git a/include/vision/math/pinhole_tools.h b/include/vision/math/pinhole_tools.h
index 2f3912e65..99d66a4e3 100644
--- a/include/vision/math/pinhole_tools.h
+++ b/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
--
GitLab