Removed all .cast<T>() instances involved in multiplication of with doubles

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;

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;
 }

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;
         }

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;
 }

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):

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;
 }

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):

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;
 }

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;
 }