Skip to content
Snippets Groups Projects

Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"

6 files
+ 55
43
Compare changes
  • Side-by-side
  • Inline
Files
6
@@ -68,9 +68,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
// 3. Get the expected bearing of the point
T bearing = atan2(point_r(1), point_r(0));
// 4. Get the measured range-and-bearing to the point, and its covariance
// 4. Get the measured range-and-bearing to the point
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;
@@ -80,8 +79,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
T sigma = sqrt(range_bearing_cov(1,1));
*_res = er / sigma;
*_res = er * T(getMeasurementSquareRootInformationUpper()(1,1));
return true;
}
Loading