250 lines
12 KiB
C#
250 lines
12 KiB
C#
/*
|
|
* Copyright 2016 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;
|
|
using AdaptiveVoxelFilterOptions = CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions;
|
|
|
|
namespace CartographerSharp.Models.Mapping;
|
|
|
|
/// <summary>
|
|
/// Protocol buffer representation of local trajectory builder options for 2D.
|
|
/// </summary>
|
|
public struct LocalTrajectoryBuilderOptions2D
|
|
{
|
|
/// <summary>
|
|
/// Rangefinder points outside these ranges will be dropped.
|
|
/// </summary>
|
|
[JsonPropertyName("min_range")]
|
|
public double MinRange { get; set; }
|
|
|
|
[JsonPropertyName("max_range")]
|
|
public double MaxRange { get; set; }
|
|
|
|
[JsonPropertyName("min_z")]
|
|
public double MinZ { get; set; }
|
|
|
|
[JsonPropertyName("max_z")]
|
|
public double MaxZ { get; set; }
|
|
|
|
/// <summary>
|
|
/// Points beyond 'max_range' will be inserted with this length as empty space.
|
|
/// </summary>
|
|
[JsonPropertyName("missing_data_ray_length")]
|
|
public double MissingDataRayLength { get; set; }
|
|
|
|
/// <summary>
|
|
/// Number of range data to accumulate into one unwarped, combined range data
|
|
/// to use for scan matching.
|
|
/// </summary>
|
|
[JsonPropertyName("num_accumulated_range_data")]
|
|
public int NumAccumulatedRangeData { get; set; }
|
|
|
|
/// <summary>
|
|
/// Voxel filter that gets applied to the range data immediately after cropping.
|
|
/// </summary>
|
|
[JsonPropertyName("voxel_filter_size")]
|
|
public double VoxelFilterSize { get; set; }
|
|
|
|
/// <summary>
|
|
/// Whether to solve the online scan matching first using the correlative scan
|
|
/// matcher to generate a good starting point for Ceres.
|
|
/// </summary>
|
|
[JsonPropertyName("use_online_correlative_scan_matching")]
|
|
public bool UseOnlineCorrelativeScanMatching { get; set; }
|
|
|
|
[JsonPropertyName("real_time_correlative_scan_matcher_options")]
|
|
public RealTimeCorrelativeScanMatcherOptions RealTimeCorrelativeScanMatcherOptions { get; set; }
|
|
|
|
[JsonPropertyName("ceres_scan_matcher_options")]
|
|
public CeresScanMatcherOptions2D CeresScanMatcherOptions { get; set; }
|
|
|
|
[JsonPropertyName("motion_filter_options")]
|
|
public MotionFilterOptions MotionFilterOptions { get; set; }
|
|
|
|
[JsonPropertyName("submaps_options")]
|
|
public SubmapsOptions2D SubmapsOptions { get; set; }
|
|
|
|
/// <summary>
|
|
/// True if IMU data should be expected and used.
|
|
/// </summary>
|
|
[JsonPropertyName("use_imu_data")]
|
|
public bool UseImuData { get; set; }
|
|
|
|
/// <summary>
|
|
/// Adaptive voxel filter options for 2D.
|
|
/// Used instead of fixed voxel_filter_size when configured.
|
|
/// </summary>
|
|
[JsonPropertyName("adaptive_voxel_filter_options")]
|
|
public Models.Sensor.AdaptiveVoxelFilterOptions AdaptiveVoxelFilterOptions { get; set; }
|
|
|
|
/// <summary>
|
|
/// Voxel filter used to compute a sparser point cloud for finding loop closures.
|
|
/// </summary>
|
|
[JsonPropertyName("loop_closure_adaptive_voxel_filter_options")]
|
|
public Models.Sensor.AdaptiveVoxelFilterOptions LoopClosureAdaptiveVoxelFilterOptions { get; set; }
|
|
|
|
/// <summary>
|
|
/// Time constant in seconds for the orientation moving average based on
|
|
/// observed gravity via the IMU. It should be chosen so that the error
|
|
/// 1. from acceleration measurements not due to gravity (which gets worse when
|
|
/// the constant is reduced) and
|
|
/// 2. from integration of angular velocities (which gets worse when the
|
|
/// constant is increased) is balanced.
|
|
/// TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
|
|
/// was replaced by pose_extrapolator_options.
|
|
/// </summary>
|
|
[JsonPropertyName("imu_gravity_time_constant")]
|
|
public double ImuGravityTimeConstant { get; set; }
|
|
|
|
[JsonPropertyName("pose_extrapolator_options")]
|
|
public PoseExtrapolatorOptions PoseExtrapolatorOptions { get; set; }
|
|
|
|
/// <summary>
|
|
/// Scan matcher options used when applying a pending initial pose (SetInitialPose called mid-run).
|
|
/// The next accumulated range data uses this pose as prediction and matches with wider search and stricter Ceres.
|
|
/// Configured with wider search windows for relocalization scenarios.
|
|
/// </summary>
|
|
[JsonPropertyName("initial_pose_real_time_correlative_scan_matcher_options")]
|
|
public RealTimeCorrelativeScanMatcherOptions InitialPoseRealTimeCorrelativeScanMatcherOptions { get; set; }
|
|
|
|
[JsonPropertyName("initial_pose_ceres_scan_matcher_options")]
|
|
public CeresScanMatcherOptions2D InitialPoseCeresScanMatcherOptions { get; set; }
|
|
|
|
/// <summary>
|
|
/// If true, provides a confidence score for the pose estimate based on
|
|
/// real-time correlative scan matching.
|
|
/// Match C++: provide_confidence_score option in LocalTrajectoryBuilderOptions2D
|
|
/// </summary>
|
|
[JsonPropertyName("provide_confidence_score")]
|
|
public bool ProvideConfidenceScore { get; set; }
|
|
|
|
/// <summary>
|
|
/// Soft limit for Ceres scan match cost. When ceresScore exceeds this threshold
|
|
/// but is below HardLimit, the pose is trusted (AddPose is called) but the scan
|
|
/// is NOT inserted into the submap to avoid corrupting the map.
|
|
/// Set to 0 to disable. Default: 0 (disabled).
|
|
/// DEPRECATED: Use ProbabilityGridCeresScoreSoftLimit / TsdfCeresScoreSoftLimit instead.
|
|
/// Kept as fallback when per-grid-type values are not set (both == 0).
|
|
/// </summary>
|
|
[JsonPropertyName("ceres_score_soft_limit")]
|
|
public double CeresScoreSoftLimit { get; set; }
|
|
|
|
/// <summary>
|
|
/// Hard limit for Ceres scan match cost. When ceresScore exceeds this threshold,
|
|
/// the scan-matched pose is considered unreliable: neither AddPose nor InsertIntoSubmap
|
|
/// is called. The odometry prediction is used instead.
|
|
/// Set to 0 to disable. Default: 0 (disabled).
|
|
/// Must be >= CeresScoreSoftLimit when both are > 0.
|
|
/// DEPRECATED: Use ProbabilityGridCeresScoreHardLimit / TsdfCeresScoreHardLimit instead.
|
|
/// Kept as fallback when per-grid-type values are not set (both == 0).
|
|
/// </summary>
|
|
[JsonPropertyName("ceres_score_hard_limit")]
|
|
public double CeresScoreHardLimit { get; set; }
|
|
|
|
/// <summary>
|
|
/// After this many consecutive hard-limit failures, forces creation of a new submap
|
|
/// at the current odometry-predicted position. This breaks the deadlock when the robot
|
|
/// enters a genuinely new area that cannot match any existing submap.
|
|
/// Set to 0 to disable. Default: 5.
|
|
/// </summary>
|
|
[JsonPropertyName("max_consecutive_high_cost_before_new_submap")]
|
|
public int MaxConsecutiveHighCostBeforeNewSubmap { get; set; }
|
|
|
|
public LocalTrajectoryBuilderOptions2D(
|
|
double minRange = 0.0,
|
|
double maxRange = 30.0,
|
|
double minZ = -0.8f,
|
|
double maxZ = 2.0,
|
|
double missingDataRayLength = 5.0,
|
|
int numAccumulatedRangeData = 1,
|
|
double voxelFilterSize = 0.025f,
|
|
bool useOnlineCorrelativeScanMatching = true,
|
|
RealTimeCorrelativeScanMatcherOptions realTimeCorrelativeScanMatcherOptions = default,
|
|
CeresScanMatcherOptions2D ceresScanMatcherOptions = default,
|
|
MotionFilterOptions motionFilterOptions = default,
|
|
SubmapsOptions2D submapsOptions = default,
|
|
bool useImuData = false,
|
|
AdaptiveVoxelFilterOptions adaptiveVoxelFilterOptions = default,
|
|
AdaptiveVoxelFilterOptions loopClosureAdaptiveVoxelFilterOptions = default,
|
|
double imuGravityTimeConstant = 10.0,
|
|
PoseExtrapolatorOptions poseExtrapolatorOptions = default,
|
|
RealTimeCorrelativeScanMatcherOptions initialPoseRealTimeCorrelativeScanMatcherOptions = default,
|
|
CeresScanMatcherOptions2D initialPoseCeresScanMatcherOptions = default,
|
|
bool provideConfidenceScore = false,
|
|
double ceresScoreSoftLimit = 0,
|
|
double ceresScoreHardLimit = 0,
|
|
int maxConsecutiveHighCostBeforeNewSubmap = 5)
|
|
{
|
|
MinRange = minRange;
|
|
MaxRange = maxRange;
|
|
MinZ = minZ;
|
|
MaxZ = maxZ;
|
|
MissingDataRayLength = missingDataRayLength;
|
|
NumAccumulatedRangeData = numAccumulatedRangeData;
|
|
VoxelFilterSize = voxelFilterSize;
|
|
UseOnlineCorrelativeScanMatching = useOnlineCorrelativeScanMatching;
|
|
|
|
// Set default values if not provided (default struct check)
|
|
RealTimeCorrelativeScanMatcherOptions = realTimeCorrelativeScanMatcherOptions.Equals(default(RealTimeCorrelativeScanMatcherOptions))
|
|
? new RealTimeCorrelativeScanMatcherOptions(linearSearchWindow: 0.1, angularSearchWindow: 0.2)
|
|
: realTimeCorrelativeScanMatcherOptions;
|
|
|
|
CeresScanMatcherOptions = ceresScanMatcherOptions.Equals(default(CeresScanMatcherOptions2D))
|
|
? new CeresScanMatcherOptions2D(occupiedSpaceWeight: 1.0, translationWeight: 10.0, rotationWeight: 1.0)
|
|
: ceresScanMatcherOptions;
|
|
|
|
MotionFilterOptions = motionFilterOptions.Equals(default(MotionFilterOptions))
|
|
? new MotionFilterOptions(maxTimeSeconds: 5.0, maxDistanceMeters: 0.2, maxAngleRadians: 0.1)
|
|
: motionFilterOptions;
|
|
|
|
SubmapsOptions = submapsOptions.Equals(default(SubmapsOptions2D))
|
|
? new SubmapsOptions2D(
|
|
numRangeData: 90,
|
|
new GridOptions2D(GridOptions2D.GridType.ProbabilityGrid, 0.05f),
|
|
new RangeDataInserterOptions(RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D))
|
|
: submapsOptions;
|
|
|
|
UseImuData = useImuData;
|
|
|
|
AdaptiveVoxelFilterOptions = adaptiveVoxelFilterOptions.Equals(default(AdaptiveVoxelFilterOptions))
|
|
? new AdaptiveVoxelFilterOptions(maxLength: 0.3, minNumPoints: 250, maxRange: 50.0)
|
|
: adaptiveVoxelFilterOptions;
|
|
|
|
LoopClosureAdaptiveVoxelFilterOptions = loopClosureAdaptiveVoxelFilterOptions.Equals(default(AdaptiveVoxelFilterOptions))
|
|
? new AdaptiveVoxelFilterOptions(maxLength: 0.9, minNumPoints: 100, maxRange: 50.0)
|
|
: loopClosureAdaptiveVoxelFilterOptions;
|
|
|
|
ImuGravityTimeConstant = imuGravityTimeConstant;
|
|
|
|
PoseExtrapolatorOptions = poseExtrapolatorOptions.Equals(default(PoseExtrapolatorOptions))
|
|
? new PoseExtrapolatorOptions()
|
|
: poseExtrapolatorOptions;
|
|
|
|
InitialPoseRealTimeCorrelativeScanMatcherOptions = initialPoseRealTimeCorrelativeScanMatcherOptions.Equals(default(RealTimeCorrelativeScanMatcherOptions))
|
|
? new RealTimeCorrelativeScanMatcherOptions(linearSearchWindow: 0.5, angularSearchWindow: 0.5)
|
|
: initialPoseRealTimeCorrelativeScanMatcherOptions;
|
|
|
|
InitialPoseCeresScanMatcherOptions = initialPoseCeresScanMatcherOptions.Equals(default(CeresScanMatcherOptions2D))
|
|
? new CeresScanMatcherOptions2D(occupiedSpaceWeight: 20.0, translationWeight: 50.0, rotationWeight: 10.0)
|
|
: initialPoseCeresScanMatcherOptions;
|
|
|
|
ProvideConfidenceScore = provideConfidenceScore;
|
|
CeresScoreSoftLimit = ceresScoreSoftLimit;
|
|
CeresScoreHardLimit = ceresScoreHardLimit;
|
|
MaxConsecutiveHighCostBeforeNewSubmap = maxConsecutiveHighCostBeforeNewSubmap;
|
|
}
|
|
}
|