From bc0ed3f6b485547571a2316d3d4133aca32dabd3 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Thu, 5 Sep 2019 09:21:37 +0200
Subject: [PATCH] Removed more .cast<T>()
---
hello_wolf/factor_range_bearing.h | 2 +-
include/core/factor/factor_autodiff_distance_3D.h | 4 ++--
include/core/factor/factor_odom_3D.h | 3 ++-
include/core/factor/factor_pose_2D.h | 3 ++-
include/core/factor/factor_pose_3D.h | 2 +-
5 files changed, 8 insertions(+), 6 deletions(-)
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index cbe5de62f..cc9e70fda 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -110,7 +110,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
exp_rb(1) = atan2(lmk_s(1), lmk_s(0)); // bearing
// 4. Get the measured range-and-bearing to the point
- Matrix<T, 2, 1> meas_rb = getMeasurement().cast<T>(); // cast Eigen type vector to have scalar type 'T'
+ auto& meas_rb = getMeasurement();
// 5. Get the error by comparing the expected against the measurement
Matrix<T, 2, 1> err_rb = meas_rb - exp_rb;
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index c213624e5..dc62037fd 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -55,12 +55,12 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
- Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
+ // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
// Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
// res = sqrt_info_upper * (dist_meas - dist_exp);
- res = getMeasurementSquareRootInformationUpper() * (dist_meas - dist_exp);
+ res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
return true;
}
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index b3b265f5a..c191eaf4f 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -208,7 +208,8 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
- Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+ // Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
+ auto& dp_m = getMeasurement().head<3>();
Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
Eigen::Matrix<T,3,1> dp = expected.head(3);
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index 2918c61af..ecdb08eed 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -34,7 +34,8 @@ template<typename T>
inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
- Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
+ // Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
+ auto& meas = getMeasurement();
// error
Eigen::Matrix<T,3,1> er;
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index 722d7b0c1..d5920de23 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -43,7 +43,7 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
// error
Eigen::Matrix<T, 6, 1> er;
- er.head(3) = p_measured.cast<T>() - p;
+ er.head(3) = p_measured - p;
er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
// residual
--
GitLab