备份

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -80,25 +80,25 @@ namespace MultiWheelC.StateEstimation
                 throw new ArgumentNullException(
                     nameof(velocityEstimator));
 
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumLinearSpeedMetersPerSecond,
                 nameof(maximumLinearSpeedMetersPerSecond));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumAngularSpeedRadiansPerSecond,
                 nameof(maximumAngularSpeedRadiansPerSecond));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 positionJumpMarginMeters,
                 nameof(positionJumpMarginMeters));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 headingJumpMarginRadians,
                 nameof(headingJumpMarginRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityPositionResidualMeters,
                 nameof(velocityPositionResidualMeters));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityHeadingResidualRadians,
                 nameof(velocityHeadingResidualRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 stationaryConfirmationSeconds,
                 nameof(stationaryConfirmationSeconds));
 
@@ -238,9 +238,9 @@ namespace MultiWheelC.StateEstimation
             var location =
                 DetourInterface.getCartLocation();
 
-            EnsureFinite(location.x, "DetourX");
-            EnsureFinite(location.y, "DetourY");
-            EnsureFinite(location.th, "DetourTheta");
+            NumericGuard.EnsureFinite(location.x, "DetourX");
+            NumericGuard.EnsureFinite(location.y, "DetourY");
+            NumericGuard.EnsureFinite(location.th, "DetourTheta");
 
             return new Pose2D(
                 location.x / MillimetersPerMeter,
@@ -268,25 +268,6 @@ namespace MultiWheelC.StateEstimation
             return _latestState;
         }
 
-        /// <summary>
-        /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
-        /// </summary>
-        private VehicleState AcceptPoseAfterVelocityRebase(
-            Pose2D poseInWorld,
-            double timestampSeconds)
-        {
-            _latestState =
-                _velocityEstimator
-                    .RebasePreservingVelocity(
-                        poseInWorld,
-                        timestampSeconds);
-            _acceptedPoseInWorld = poseInWorld;
-            _acceptedTimestampSeconds =
-                timestampSeconds;
-            _stationaryHoldActive = false;
-            return _latestState;
-        }
-
         /// <summary>
         /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
         /// </summary>
@@ -340,7 +321,7 @@ namespace MultiWheelC.StateEstimation
             Pose2D endPoseInWorld,
             double deltaTimeSeconds)
         {
-            if (!IsFinite(deltaTimeSeconds) ||
+            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                 deltaTimeSeconds <= 0.0)
             {
                 return false;
@@ -448,62 +429,5 @@ namespace MultiWheelC.StateEstimation
                    HeadingEqualityToleranceRadians;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数和Detour位姿必须是有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于状态估计。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }