Merge branch '257-eigen-matrix-cast-ceres-jet-not-needed-if-upgrading-to-eigen-3-3' into devel

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

@@ -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;
@@ -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() * (getMeasurement() - dist_exp);
 
             return true;
         }

include/core/factor/factor_diff_drive.h

@@ -104,26 +104,19 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
 
     Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
 
-    // Compute corrected delta
 
-    /// Is this my delta_preint ? Yes!
-    const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
+    // Correct delta due to changes in the calibration parameters
+    auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
 
-    Matrix<T, 3, 1> c_preint = calib_preint_.cast<T>();
-
-    Eigen::Matrix<T, 3, 1> delta_corrected = delta_preint + J_delta_calib_.cast<T>() * (c - c_preint);
-
-    // 2d pose residual
 
+    // Predict delta from states
     Eigen::Matrix<T, 3, 1> delta_predicted;
-
     // position
     delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
-
     // orientation
     delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
 
-    // residual
+    // 2d pose residual
     residuals = delta_corrected - delta_predicted;
 
     // angle remapping
@@ -133,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

@@ -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);
@@ -221,7 +222,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

@@ -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;
@@ -48,7 +49,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

@@ -43,12 +43,12 @@ 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
     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;
 }

test/gtest_factor_diff_drive.cpp

@@ -620,7 +620,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
 
     WOLF_TRACE("\n  ========== SOLVE =========");
-    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE("\n", report);
 
     WOLF_TRACE("x1           : ", problem->getState(N*dt).transpose());