//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return fabs(deltaTij_ - other.deltaTij_) < tol
&& deltaXij_.equals(other.deltaXij_, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
//------------------------------------------------------------------------------
bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& deltaXij_.equals(other.deltaXij_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
/* ************************************************************************* */
bool LinearizedJacobianFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
if (e) {
Matrix thisMatrix = this->Ab_.range(0, Ab_.nBlocks());
Matrix rhsMatrix = e->Ab_.range(0, Ab_.nBlocks());
return Base::equals(expected, tol)
&& lin_points_.equals(e->lin_points_, tol)
&& equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
} else {
return false;
}
}
/* ************************************************************************* */
bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
if (e) {
Matrix thisMatrix = this->info_.selfadjointView();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = e->info_.selfadjointView();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return Base::equals(expected, tol)
&& lin_points_.equals(e->lin_points_, tol)
&& equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
} else {
return false;
}
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}