From 00f9087c59ebf3c28f5f48b4ae262e0222a5cc5c Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Sun, 24 Apr 2022 18:38:29 +0200
Subject: [PATCH] Remove unnecessary uses of cast<T>()
---
include/vision/factor/factor_pixel_hp.h | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h
index db475d2c6..d51cfd019 100644
--- a/include/vision/factor/factor_pixel_hp.h
+++ b/include/vision/factor/factor_pixel_hp.h
@@ -144,15 +144,15 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
Matrix<T, 4, 1> lh_w(_lmk_hmg);
// landmark dir vector in C frame
+ /* note: transforming hmg point to get direction vector v':
+ * | 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);
- // camera parameters
- Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
- Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
-
// project point and exit
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
- expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
+ expectation = pinhole::projectPoint(intrinsic_, distortion_, v_dir);
}
@@ -170,7 +170,7 @@ inline bool FactorPixelHp::operator ()(const T* const _frame_p,
// residual
Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected - getMeasurement());
+ residuals = getMeasurementSquareRootInformationUpper() * (expected - getMeasurement());
return true;
}
--
GitLab