bool Pose::operator==(const Pose& right) const { // invalid poses return false for comparison, even against identical invalid poses, like NaN if (!valid || !right.valid) { return false; } // FIXME add margin of error? Or add an additional withinEpsilon function? return translation == right.getTranslation() && rotation == right.getRotation() && velocity == right.getVelocity() && angularVelocity == right.getAngularVelocity(); }