From b0a7b609d0843e80923cdd5c9ee47405728339ca Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 2 Sep 2019 11:25:13 +0200
Subject: [PATCH] Removed all .cast<T>() instances involved in multiplication
of with doubles
---
hello_wolf/factor_bearing.h | 4 ++--
hello_wolf/factor_range_bearing.h | 2 +-
include/core/factor/factor_autodiff_distance_3D.h | 8 +++++---
include/core/factor/factor_diff_drive.h | 2 +-
include/core/factor/factor_odom_2D.h | 4 ++--
include/core/factor/factor_odom_3D.h | 2 +-
include/core/factor/factor_pose_2D.h | 2 +-
include/core/factor/factor_pose_3D.h | 2 +-
include/core/factor/factor_quaternion_absolute.h | 2 +-
9 files changed, 15 insertions(+), 13 deletions(-)
diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 1b5ff4049..6975a798d 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -69,8 +69,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
T bearing = atan2(point_r(1), point_r(0));
// 4. Get the measured range-and-bearing to the point, and its covariance
- Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
- Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance().cast<T>();
+ Matrix<T, 2, 1> range_bearing = getMeasurement();
+ Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index ffe896f75..cbe5de62f 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -126,7 +126,7 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
// R^T * R = Omega
// in other words, R is the Cholesky decomposition of Omega.
// NOTE: you get R directly from the Feature with getMeasurementSquareRootInformationUpper()
- res = getMeasurementSquareRootInformationUpper().cast<T>() * err_rb;
+ res = getMeasurementSquareRootInformationUpper() * err_rb;
return true;
}
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index e26c41281..c213624e5 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -55,10 +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> sqrt_info_upper = getMeasurementSquareRootInformationUpper().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 = sqrt_info_upper * (dist_meas - dist_exp);
+
+ res = getMeasurementSquareRootInformationUpper() * (dist_meas - dist_exp);
return true;
}
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index d26cfcd7c..eaf64a11e 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -126,7 +126,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
residuals(2) = residuals(2) + T(2. * M_PI);
// weighted residual
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
+ residuals = getMeasurementSquareRootInformationUpper() * residuals;
return true;
}
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index e5cfd57a6..3eb1c5667 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -64,14 +64,14 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
expected_measurement(2) = o2 - o1;
// Error
- er = expected_measurement - getMeasurement().cast<T>();
+ er = expected_measurement - getMeasurement();
while (er(2) > T( M_PI ))
er(2) -= T( 2 * M_PI );
while (er(2) < T( -M_PI ))
er(2) += T( 2 * M_PI );
// Residuals
- res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index 587472541..b3b265f5a 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -221,7 +221,7 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.tail(3) = q2v(dq.conjugate() * dq_m);
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
+ residuals = getMeasurementSquareRootInformationUpper() * residuals;
return true;
}
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index e4caa3b2d..2918c61af 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -48,7 +48,7 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index d36bcdb32..722d7b0c1 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -48,7 +48,7 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 477db6368..2f17c14a5 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -63,7 +63,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
return true;
}
--
GitLab