diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index 1b5ff40497096ac74f35024ce0b37b1c88076266..6975a798db580e1c127b0215609f49904b29ae40 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 ffe896f75932fb269068ad1210ad37350e62ce30..cbe5de62ff34a98c83c8a5192292f7fe6051f018 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 e26c412812fd7c7e869a79c8b95085eac43ed633..c213624e554a15ec319da15c6374769cd3b9f167 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 d26cfcd7c996f10ae7b81bdb4a7eb406a876fbf2..eaf64a11ed59127337ae69a37189687ac8e433cd 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 e5cfd57a69329ca9c82c9019f29d4ee591b27f77..3eb1c566796728c454cc2d918656607b3ba1516c 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 587472541124f2d0d158b3d999c9b65bb1bc5dcb..b3b265f5af12f100fc74c6ba5e163d655451e0dc 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 e4caa3b2dbd2ed26ef8b23a83443b0662b1ec823..2918c61af692c0750fed7c8975f8bc8e640a5d61 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 d36bcdb32a832695b29bbbe5a0610ded37746218..722d7b0c13cd9d1e1d2a6abe1cd3864149e76c4b 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 477db6368bb4217b41e154095cbca3bd2c00cf6f..2f17c14a54a5b8d8e46fb24165ceb140a6b1d55d 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;
}