112 lines
4.3 KiB
C#
112 lines
4.3 KiB
C#
/*
|
|
* Copyright 2018 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using System.Text.Json.Serialization;
|
|
|
|
namespace CartographerSharp.Models.Mapping;
|
|
|
|
/// <summary>
|
|
/// Protocol buffer representation of pose extrapolator options.
|
|
/// </summary>
|
|
public struct PoseExtrapolatorOptions
|
|
{
|
|
[JsonPropertyName("use_imu_based")]
|
|
public bool UseImuBased { get; set; }
|
|
|
|
[JsonPropertyName("constant_velocity")]
|
|
public ConstantVelocityPoseExtrapolatorOptions ConstantVelocity { get; set; }
|
|
|
|
[JsonPropertyName("imu_based")]
|
|
[JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
|
|
public ImuBasedPoseExtrapolatorOptions? ImuBased { get; set; }
|
|
|
|
/// <summary>
|
|
/// Velocity threshold for detecting robot movement (m/s).
|
|
/// Robot is considered "moving" if horizontal velocity exceeds this threshold.
|
|
/// Default: 0.003 m/s (3 mm/s)
|
|
/// </summary>
|
|
[JsonPropertyName("velocity_threshold")]
|
|
public double? VelocityThreshold { get; set; }
|
|
|
|
/// <summary>
|
|
/// Acceleration threshold for detecting IMU acceleration (m/s²).
|
|
/// IMU is considered "accelerating" if horizontal acceleration exceeds this threshold.
|
|
/// Default: 0.4 m/s²
|
|
/// </summary>
|
|
[JsonPropertyName("acceleration_threshold")]
|
|
public double? AccelerationThreshold { get; set; }
|
|
|
|
/// <summary>
|
|
/// Gravity deviation threshold for detecting vertical acceleration changes (m/s²).
|
|
/// Used to detect changes in vertical acceleration (gravity deviation).
|
|
/// Default: 0.1 m/s²
|
|
/// </summary>
|
|
[JsonPropertyName("gravity_deviation_threshold")]
|
|
public double? GravityDeviationThreshold { get; set; }
|
|
|
|
/// <summary>
|
|
/// If true, use odometry pose directly at the time of range data instead of calculating velocity.
|
|
/// This is useful when odometry data is accurate and reliable.
|
|
/// When enabled, ExtrapolatePose will use the latest odometry data at or before the requested time,
|
|
/// instead of extrapolating from the last pose using calculated velocity.
|
|
/// Default: false (use velocity-based extrapolation)
|
|
/// </summary>
|
|
[JsonPropertyName("use_odometry_directly")]
|
|
public bool UseOdometryDirectly { get; set; }
|
|
|
|
public PoseExtrapolatorOptions(
|
|
bool useImuBased = false,
|
|
ConstantVelocityPoseExtrapolatorOptions? constantVelocity = null,
|
|
ImuBasedPoseExtrapolatorOptions? imuBased = null,
|
|
double? velocityThreshold = null,
|
|
double? accelerationThreshold = null,
|
|
double? gravityDeviationThreshold = null,
|
|
bool useOdometryDirectly = false)
|
|
{
|
|
UseImuBased = useImuBased;
|
|
ConstantVelocity = constantVelocity ?? new ConstantVelocityPoseExtrapolatorOptions();
|
|
ImuBased = imuBased;
|
|
VelocityThreshold = velocityThreshold;
|
|
AccelerationThreshold = accelerationThreshold;
|
|
GravityDeviationThreshold = gravityDeviationThreshold;
|
|
UseOdometryDirectly = useOdometryDirectly;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constant velocity pose extrapolator options.
|
|
/// </summary>
|
|
public struct ConstantVelocityPoseExtrapolatorOptions
|
|
{
|
|
/// <summary>
|
|
/// Time constant in seconds for the orientation moving average based on
|
|
/// observed gravity via the IMU.
|
|
/// </summary>
|
|
[JsonPropertyName("imu_gravity_time_constant")]
|
|
public double ImuGravityTimeConstant { get; set; }
|
|
|
|
[JsonPropertyName("pose_queue_duration")]
|
|
public double PoseQueueDuration { get; set; }
|
|
|
|
public ConstantVelocityPoseExtrapolatorOptions(
|
|
double imuGravityTimeConstant = 10.0,
|
|
double poseQueueDuration = 0.001)
|
|
{
|
|
ImuGravityTimeConstant = imuGravityTimeConstant;
|
|
PoseQueueDuration = poseQueueDuration;
|
|
}
|
|
}
|