1694 lines
73 KiB
C#
1694 lines
73 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 CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Transform;
|
|
using CeresSharp;
|
|
using CeresSharp.Enums;
|
|
using RobotNet10.Shared.Numbers;
|
|
using QuaternionManifold = CeresSharp.QuaternionManifold;
|
|
|
|
namespace CartographerSharp.Mapping.Internal.Optimization;
|
|
|
|
/// <summary>
|
|
/// Submap specification for optimization.
|
|
/// </summary>
|
|
public struct SubmapSpec2D(Rigid2d globalPose)
|
|
{
|
|
public Rigid2d GlobalPose { get; set; } = globalPose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Node specification for optimization.
|
|
/// </summary>
|
|
public struct NodeSpec2D(
|
|
long time,
|
|
Rigid2d localPose2D,
|
|
Rigid2d globalPose2D,
|
|
Rigid3d globalPose,
|
|
Quaternion gravityAlignment)
|
|
{
|
|
public long Time { get; set; } = time;
|
|
public Rigid2d LocalPose2D { get; set; } = localPose2D;
|
|
public Rigid2d GlobalPose2D { get; set; } = globalPose2D;
|
|
public Rigid3d GlobalPose { get; set; } = globalPose;
|
|
public Quaternion GravityAlignment { get; set; } = gravityAlignment;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Optimization problem for 2D pose graph.
|
|
/// </summary>
|
|
public class OptimizationProblem2D(OptimizationProblemOptions options)
|
|
{
|
|
private int? _maxNumIterations; // Override value set via SetMaxNumIterations()
|
|
private readonly MapById<SubmapId, SubmapSpec2D> _submapData = new();
|
|
private readonly MapById<NodeId, NodeSpec2D> _nodeData = new();
|
|
private readonly Dictionary<string, Rigid3d> _landmarkData = [];
|
|
|
|
// Sensor data storage (simplified - using List for now, full implementation would use MapByTime)
|
|
private readonly Dictionary<int, List<Sensor.OdometryData>> _odometryData = [];
|
|
private readonly Dictionary<int, List<Sensor.FixedFramePoseData>> _fixedFramePoseData = [];
|
|
private readonly Dictionary<int, IPoseGraph.TrajectoryData> _trajectoryData = [];
|
|
|
|
/// <summary>
|
|
/// Sets maximum number of iterations (match C++ SetMaxNumIterations).
|
|
/// </summary>
|
|
public void SetMaxNumIterations(int maxNumIterations)
|
|
{
|
|
_maxNumIterations = maxNumIterations;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a submap to the optimization problem.
|
|
/// FIX: Use GetNextSubmapIndex instead of SizeOfTrajectoryOrZero to handle trimmed trajectories.
|
|
/// After trimming, Count can collide with existing indices (e.g., trim index 0 leaves {1,2},
|
|
/// Count=2 would create index 2 which already exists). Using Max()+1 avoids this.
|
|
/// </summary>
|
|
public void AddSubmap(int trajectoryId, Rigid2d globalPose)
|
|
{
|
|
var nextIndex = GetNextSubmapIndex(trajectoryId);
|
|
var submapId = new SubmapId(trajectoryId, nextIndex);
|
|
_submapData.Insert(submapId, new SubmapSpec2D(globalPose));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the next sequential submap index for a trajectory, accounting for trimmed entries.
|
|
/// </summary>
|
|
private int GetNextSubmapIndex(int trajectoryId)
|
|
{
|
|
var size = _submapData.SizeOfTrajectoryOrZero(trajectoryId);
|
|
if (size == 0) return 0;
|
|
// Find the max existing index and use max + 1
|
|
var last = _submapData.GetLastOfTrajectory(trajectoryId);
|
|
return last.HasValue ? last.Value.Id.SubmapIndex + 1 : 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a submap to the optimization problem with a specific SubmapId.
|
|
/// If the submap already exists, it will be updated with the new pose.
|
|
/// </summary>
|
|
public void AddSubmap(SubmapId submapId, Rigid2d globalPose)
|
|
{
|
|
if (_submapData.Contains(submapId))
|
|
{
|
|
// Update existing submap pose
|
|
var existingSpec = _submapData[submapId];
|
|
existingSpec.GlobalPose = globalPose;
|
|
_submapData[submapId] = existingSpec;
|
|
}
|
|
else
|
|
{
|
|
_submapData.Insert(submapId, new SubmapSpec2D(globalPose));
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a node to the optimization problem.
|
|
/// FIX: Use GetLastOfTrajectory to compute next index, same reasoning as AddSubmap.
|
|
/// </summary>
|
|
public void AddNode(
|
|
int trajectoryId,
|
|
long time,
|
|
Rigid2d localPose2D,
|
|
Rigid2d globalPose2D,
|
|
Rigid3d globalPose,
|
|
Quaternion gravityAlignment)
|
|
{
|
|
var size = _nodeData.SizeOfTrajectoryOrZero(trajectoryId);
|
|
int nextIndex;
|
|
if (size == 0)
|
|
{
|
|
nextIndex = 0;
|
|
}
|
|
else
|
|
{
|
|
var last = _nodeData.GetLastOfTrajectory(trajectoryId);
|
|
nextIndex = last.HasValue ? last.Value.Id.NodeIndex + 1 : 0;
|
|
}
|
|
var nodeId = new NodeId(trajectoryId, nextIndex);
|
|
_nodeData.Insert(nodeId, new NodeSpec2D(time, localPose2D, globalPose2D, globalPose, gravityAlignment));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a node to the optimization problem (backward compatibility).
|
|
/// </summary>
|
|
public void AddNode(int trajectoryId, Rigid2d globalPose2D, Rigid3d globalPose)
|
|
{
|
|
// Use default values for missing fields (for backward compatibility)
|
|
var nodeId = new NodeId(trajectoryId, _nodeData.SizeOfTrajectoryOrZero(trajectoryId));
|
|
var localPose2D = globalPose2D; // Assume same as global for backward compatibility
|
|
_nodeData.Insert(nodeId, new NodeSpec2D(0, localPose2D, globalPose2D, globalPose, Quaternion.Identity));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Inserts a node with a specific NodeId (match C++ InsertTrajectoryNode).
|
|
/// Used when loading from proto to restore exact node IDs.
|
|
/// </summary>
|
|
public void InsertTrajectoryNode(NodeId nodeId, NodeSpec2D nodeData)
|
|
{
|
|
_nodeData.Insert(nodeId, nodeData);
|
|
if (!_trajectoryData.ContainsKey(nodeId.TrajectoryId))
|
|
_trajectoryData[nodeId.TrajectoryId] = default;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets submap data.
|
|
/// </summary>
|
|
public MapById<SubmapId, SubmapSpec2D> SubmapData()
|
|
{
|
|
return _submapData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets node data.
|
|
/// </summary>
|
|
public MapById<NodeId, NodeSpec2D> NodeData()
|
|
{
|
|
return _nodeData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trims a submap from the optimization problem (match C++ optimization_problem_->TrimSubmap).
|
|
/// </summary>
|
|
public void TrimSubmap(SubmapId submapId)
|
|
{
|
|
_submapData.Trim(submapId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trims a node from the optimization problem (match C++ optimization_problem_->TrimTrajectoryNode).
|
|
/// C++ also trims odometry_data_ and fixed_frame_pose_data_ by the time "gap" of the removed node
|
|
/// (MapByTime.Trim): remove data strictly between prev and next node time, retaining first and last in gap.
|
|
/// </summary>
|
|
public void TrimTrajectoryNode(NodeId nodeId)
|
|
{
|
|
// Match C++: trim sensor data BEFORE trimming the node (Trim needs node_id in nodes to get gap)
|
|
var (gapStart, gapEnd) = GetNodeTimeGapUnsafe(nodeId);
|
|
if (gapStart < gapEnd)
|
|
{
|
|
TrimOdometryDataByGap(nodeId.TrajectoryId, gapStart, gapEnd);
|
|
TrimFixedFramePoseDataByGap(nodeId.TrajectoryId, gapStart, gapEnd);
|
|
}
|
|
|
|
_nodeData.Trim(nodeId);
|
|
if (_nodeData.SizeOfTrajectoryOrZero(nodeId.TrajectoryId) == 0)
|
|
_trajectoryData.Remove(nodeId.TrajectoryId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets (gap_start, gap_end) for the node: prev node time and next node time (match C++ MapByTime.Trim).
|
|
/// Returns (long.MinValue, long.MaxValue) if node not found or trajectory has no prev/next.
|
|
/// </summary>
|
|
private (long gapStart, long gapEnd) GetNodeTimeGapUnsafe(NodeId nodeId)
|
|
{
|
|
if (!_nodeData.Contains(nodeId))
|
|
return (long.MinValue, long.MaxValue);
|
|
var list = _nodeData.BeginOfTrajectory(nodeId.TrajectoryId)
|
|
.OrderBy(x => x.Id.GetIndex())
|
|
.ToList();
|
|
var idx = list.FindIndex(x => x.Id.GetIndex() == nodeId.NodeIndex);
|
|
if (idx < 0)
|
|
return (long.MinValue, long.MaxValue);
|
|
long gapStart = idx > 0 ? list[idx - 1].Data.Time : long.MinValue;
|
|
long gapEnd = idx < list.Count - 1 ? list[idx + 1].Data.Time : long.MaxValue;
|
|
return (gapStart, gapEnd);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trims odometry data in (gap_start, gap_end), retaining first and last in gap (match C++ MapByTime.Trim).
|
|
/// </summary>
|
|
private void TrimOdometryDataByGap(int trajectoryId, long gapStart, long gapEnd)
|
|
{
|
|
if (!_odometryData.TryGetValue(trajectoryId, out var list) || list.Count == 0)
|
|
return;
|
|
// Indices where time > gapStart && time < gapEnd
|
|
var indicesInGap = new List<int>();
|
|
for (int i = 0; i < list.Count; i++)
|
|
{
|
|
if (list[i].Time > gapStart && list[i].Time < gapEnd)
|
|
indicesInGap.Add(i);
|
|
}
|
|
if (indicesInGap.Count <= 2)
|
|
return;
|
|
// Remove middle elements (keep first and last in gap)
|
|
for (int k = indicesInGap.Count - 2; k >= 1; k--)
|
|
list.RemoveAt(indicesInGap[k]);
|
|
if (list.Count == 0)
|
|
_odometryData.Remove(trajectoryId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trims fixed frame pose data in (gap_start, gap_end), retaining first and last in gap (match C++ MapByTime.Trim).
|
|
/// </summary>
|
|
private void TrimFixedFramePoseDataByGap(int trajectoryId, long gapStart, long gapEnd)
|
|
{
|
|
if (!_fixedFramePoseData.TryGetValue(trajectoryId, out var list) || list.Count == 0)
|
|
return;
|
|
var indicesInGap = new List<int>();
|
|
for (int i = 0; i < list.Count; i++)
|
|
{
|
|
if (list[i].Time > gapStart && list[i].Time < gapEnd)
|
|
indicesInGap.Add(i);
|
|
}
|
|
if (indicesInGap.Count <= 2)
|
|
return;
|
|
for (int k = indicesInGap.Count - 2; k >= 1; k--)
|
|
list.RemoveAt(indicesInGap[k]);
|
|
if (list.Count == 0)
|
|
_fixedFramePoseData.Remove(trajectoryId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets landmark data.
|
|
/// </summary>
|
|
public Dictionary<string, Rigid3d> LandmarkData()
|
|
{
|
|
return _landmarkData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds odometry data.
|
|
/// </summary>
|
|
public void AddOdometryData(int trajectoryId, Sensor.OdometryData odometryData)
|
|
{
|
|
if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? value))
|
|
{
|
|
value = [];
|
|
_odometryData[trajectoryId] = value;
|
|
}
|
|
|
|
value.Add(odometryData);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds fixed frame pose data.
|
|
/// </summary>
|
|
public void AddFixedFramePoseData(int trajectoryId, Sensor.FixedFramePoseData fixedFramePoseData)
|
|
{
|
|
if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<Sensor.FixedFramePoseData>? value))
|
|
{
|
|
value = [];
|
|
_fixedFramePoseData[trajectoryId] = value;
|
|
}
|
|
|
|
value.Add(fixedFramePoseData);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets trajectory data.
|
|
/// </summary>
|
|
public void SetTrajectoryData(int trajectoryId, IPoseGraph.TrajectoryData trajectoryData)
|
|
{
|
|
_trajectoryData[trajectoryId] = trajectoryData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates odometry data at the given time.
|
|
/// </summary>
|
|
private Rigid3d? InterpolateOdometry(int trajectoryId, long time)
|
|
{
|
|
if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? odometryList) || odometryList.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Find the odometry data at or after the given time
|
|
var it = odometryList.FirstOrDefault(o => o.Time >= time);
|
|
if (it.Time == 0 && odometryList.Count > 0 && odometryList[0].Time > time)
|
|
{
|
|
// Time is before first odometry data
|
|
return null;
|
|
}
|
|
|
|
if (it.Time == 0)
|
|
{
|
|
// Time is after all odometry data, find the last one
|
|
if (odometryList.Count == 0)
|
|
return null;
|
|
it = odometryList[^1];
|
|
if (it.Time == time)
|
|
{
|
|
return it.Pose;
|
|
}
|
|
return null; // Cannot extrapolate
|
|
}
|
|
|
|
// Find previous odometry data
|
|
var prevIndex = odometryList.IndexOf(it);
|
|
if (prevIndex == 0)
|
|
{
|
|
if (it.Time == time)
|
|
{
|
|
return it.Pose;
|
|
}
|
|
return null; // Cannot interpolate before first data
|
|
}
|
|
|
|
var prevIt = odometryList[prevIndex - 1];
|
|
|
|
// Interpolate between prev and it
|
|
var timeDiff = it.Time - prevIt.Time;
|
|
if (timeDiff == 0)
|
|
{
|
|
return prevIt.Pose;
|
|
}
|
|
|
|
var interpolatedPose = TransformOperations.Interpolate(
|
|
prevIt.Pose,
|
|
prevIt.Time,
|
|
it.Pose,
|
|
it.Time,
|
|
time
|
|
);
|
|
|
|
return interpolatedPose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates relative odometry between two nodes.
|
|
/// </summary>
|
|
private Rigid3d? CalculateOdometryBetweenNodes(
|
|
int trajectoryId,
|
|
NodeSpec2D firstNodeData,
|
|
NodeSpec2D secondNodeData)
|
|
{
|
|
if (!_odometryData.ContainsKey(trajectoryId))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var firstOdometry = InterpolateOdometry(trajectoryId, firstNodeData.Time);
|
|
var secondOdometry = InterpolateOdometry(trajectoryId, secondNodeData.Time);
|
|
|
|
if (firstOdometry == null || secondOdometry == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Compute relative odometry with gravity alignment
|
|
var firstGravityRotation = new Rigid3d(Vector3.Zero, firstNodeData.GravityAlignment);
|
|
var secondGravityRotation = new Rigid3d(Vector3.Zero, secondNodeData.GravityAlignment);
|
|
|
|
var relativeOdometry = firstGravityRotation *
|
|
firstOdometry.Value.Inverse() *
|
|
secondOdometry.Value *
|
|
secondGravityRotation.Inverse();
|
|
|
|
return relativeOdometry;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates fixed frame pose data at the given time.
|
|
/// </summary>
|
|
private Rigid3d? InterpolateFixedFramePose(int trajectoryId, long time)
|
|
{
|
|
if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<Sensor.FixedFramePoseData>? fixedFramePoseList) || fixedFramePoseList.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Find the fixed frame pose data at or after the given time
|
|
var it = fixedFramePoseList.FirstOrDefault(f => f.Time >= time);
|
|
if (it.Time == 0 && fixedFramePoseList.Count > 0 && fixedFramePoseList[0].Time > time)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
if (it.Time == 0)
|
|
{
|
|
if (fixedFramePoseList.Count == 0)
|
|
return null;
|
|
it = fixedFramePoseList[^1];
|
|
if (it.Time == time)
|
|
{
|
|
return it.Pose;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
var prevIndex = fixedFramePoseList.IndexOf(it);
|
|
if (prevIndex == 0)
|
|
{
|
|
if (it.Time == time)
|
|
{
|
|
return it.Pose;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
var prevIt = fixedFramePoseList[prevIndex - 1];
|
|
|
|
if (!prevIt.Pose.HasValue)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
if (!it.Pose.HasValue)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var timeDiff = it.Time - prevIt.Time;
|
|
if (timeDiff == 0)
|
|
{
|
|
return prevIt.Pose;
|
|
}
|
|
|
|
var interpolatedPose = TransformOperations.Interpolate(
|
|
prevIt.Pose.Value,
|
|
prevIt.Time,
|
|
it.Pose.Value,
|
|
it.Time,
|
|
time
|
|
);
|
|
|
|
return interpolatedPose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Solves the optimization problem.
|
|
/// </summary>
|
|
///
|
|
// Log Ceres version from FullReport (first time only)
|
|
static bool versionLogged = false;
|
|
public void Solve(
|
|
List<IPoseGraph.Constraint> constraints,
|
|
Dictionary<int, IPoseGraph.TrajectoryState> trajectoriesState,
|
|
Dictionary<string, IPoseGraph.LandmarkNode> landmarkNodes,
|
|
int? maxNumIterationsOverride = null)
|
|
{
|
|
if (_nodeData.IsEmpty)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Verify Ceres library is loaded and log version info
|
|
// This will be done when we get the first SolverSummary
|
|
|
|
// Identify frozen trajectories
|
|
var frozenTrajectories = new HashSet<int>();
|
|
foreach (var kvp in trajectoriesState)
|
|
{
|
|
if (kvp.Value == IPoseGraph.TrajectoryState.Frozen)
|
|
{
|
|
frozenTrajectories.Add(kvp.Key);
|
|
}
|
|
}
|
|
|
|
// Create manifolds BEFORE problem so they are disposed AFTER problem
|
|
// (C# using var disposes in reverse declaration order)
|
|
// This ensures native manifold handles remain valid during Problem.Dispose()
|
|
using var sharedPose2DManifold = new CeresSharp.Pose2DManifold();
|
|
using var sharedQuaternionManifold = new QuaternionManifold();
|
|
|
|
// Create Ceres problem
|
|
using var problem = new Problem();
|
|
using var solverOptions = new SolverOptions();
|
|
solverOptions.LinearSolverType = LinearSolverType.SparseSchur; // For large problems
|
|
// Use override if provided, otherwise use _maxNumIterations (set via SetMaxNumIterations),
|
|
// otherwise use MaxNumIterations from options, with fallback to default if not set
|
|
solverOptions.MaxNumIterations = maxNumIterationsOverride ??
|
|
(_maxNumIterations ?? (options.MaxNumIterations > 0 ? options.MaxNumIterations : 50)); // Default for pose graph optimization
|
|
// Solution 1: Use single thread to eliminate race condition possibility
|
|
// Multi-threading may cause parameter blocks to be modified concurrently during evaluation
|
|
// This could lead to pose explosion when Ceres evaluates cost functions
|
|
solverOptions.NumThreads = 1; // Single-threaded to prevent race conditions (was 4)
|
|
// Relaxed tolerances to allow more iterations and better convergence
|
|
// This prevents premature termination when cost is still decreasing
|
|
// Increased tolerance values (less strict) to allow optimization to run more iterations
|
|
// This is important when initial poses are close but need fine-tuning
|
|
solverOptions.FunctionTolerance = 1e-6; // More strict to allow more iterations (was 1e-4)
|
|
solverOptions.GradientTolerance = 1e-10; // More strict to allow more iterations (was 1e-8)
|
|
solverOptions.ParameterTolerance = 1e-8; // More strict to allow more iterations (was 1e-6)
|
|
|
|
// Solution 3: Improve initial guess using odometry
|
|
// First, improve node initial poses using odometry if available
|
|
ImproveInitialPosesWithOdometry();
|
|
|
|
// Convert poses to parameter arrays
|
|
var submapParams = new Dictionary<SubmapId, double[]>();
|
|
var nodeParams = new Dictionary<NodeId, double[]>();
|
|
|
|
// Add submap parameter blocks
|
|
// Match C++: Fix first submap or all submaps of frozen trajectories
|
|
bool firstSubmap = true;
|
|
int submapCount = 0;
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
var frozen = frozenTrajectories.Contains(kvp.Id.TrajectoryId);
|
|
var pose = kvp.Data.GlobalPose;
|
|
var poseParams = OptimizationHelpers.Rigid2dToParameters(pose);
|
|
|
|
// Normalize rotation angle to [-π, π] to prevent bounds violations
|
|
poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
|
|
|
|
// Validate pose parameters (C# enhancement - C++ doesn't validate, but we keep for safety)
|
|
if (double.IsNaN(poseParams[0]) || double.IsNaN(poseParams[1]) || double.IsNaN(poseParams[2]) ||
|
|
double.IsInfinity(poseParams[0]) || double.IsInfinity(poseParams[1]) || double.IsInfinity(poseParams[2]))
|
|
{
|
|
continue; // Skip invalid submap, but don't update firstSubmap flag
|
|
}
|
|
|
|
submapParams[kvp.Id] = poseParams;
|
|
|
|
// Match C++: AddParameterBlock first
|
|
problem.AddParameterBlock(poseParams, 3);
|
|
|
|
// C# enhancement: Set Pose2D manifold for rotation handling (C++ doesn't use manifold for 2D)
|
|
// Shared manifold instance reused for all parameter blocks
|
|
problem.SetManifold(poseParams, sharedPose2DManifold);
|
|
|
|
// C# enhancement: Set bounds for translation to prevent explosion (C++ doesn't set bounds)
|
|
// Use much tighter bounds based on initial pose to prevent large deviations
|
|
const double MAX_POSE_BOUND = 10.0; // 10m - tight bound to prevent explosion
|
|
double lowerX = poseParams[0] - MAX_POSE_BOUND;
|
|
double upperX = poseParams[0] + MAX_POSE_BOUND;
|
|
double lowerY = poseParams[1] - MAX_POSE_BOUND;
|
|
double upperY = poseParams[1] + MAX_POSE_BOUND;
|
|
|
|
problem.SetParameterLowerBound(poseParams, 0, lowerX); // x
|
|
problem.SetParameterUpperBound(poseParams, 0, upperX);
|
|
problem.SetParameterLowerBound(poseParams, 1, lowerY); // y
|
|
problem.SetParameterUpperBound(poseParams, 1, upperY);
|
|
// Note: Rotation bounds are NOT set because AngleManifold handles rotation constraints
|
|
|
|
// Match C++: Fix first submap or all submaps of frozen trajectories
|
|
// CRITICAL FIX: Only set firstSubmap = false AFTER successfully adding parameter block
|
|
// This ensures the first VALID submap is fixed, not the first in iteration order
|
|
if (firstSubmap || frozen)
|
|
{
|
|
// Fix the pose of the first submap or all submaps of a frozen trajectory
|
|
problem.SetParameterBlockConstant(poseParams);
|
|
// Only update firstSubmap flag after successfully fixing a submap
|
|
if (firstSubmap)
|
|
{
|
|
firstSubmap = false;
|
|
}
|
|
}
|
|
submapCount++;
|
|
}
|
|
|
|
// Ensure we have at least one submap
|
|
if (submapParams.Count == 0)
|
|
{
|
|
return; // Nothing to optimize
|
|
}
|
|
|
|
// Add node parameter blocks
|
|
// Match C++: Fix nodes of frozen trajectories only (no first node fix)
|
|
int nodeCount = 0;
|
|
foreach (var kvp in _nodeData)
|
|
{
|
|
var frozen = frozenTrajectories.Contains(kvp.Id.TrajectoryId);
|
|
var pose = kvp.Data.GlobalPose2D;
|
|
var poseParams = OptimizationHelpers.Rigid2dToParameters(pose);
|
|
|
|
// Normalize rotation angle to [-π, π] to prevent bounds violations
|
|
// This is critical because rotation angles can accumulate and exceed ±3.0 rad bounds
|
|
poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
|
|
|
|
// Validate pose parameters (C# enhancement - C++ doesn't validate, but we keep for safety)
|
|
if (double.IsNaN(poseParams[0]) || double.IsNaN(poseParams[1]) || double.IsNaN(poseParams[2]) ||
|
|
double.IsInfinity(poseParams[0]) || double.IsInfinity(poseParams[1]) || double.IsInfinity(poseParams[2]))
|
|
{
|
|
continue; // Skip invalid node
|
|
}
|
|
|
|
nodeParams[kvp.Id] = poseParams;
|
|
|
|
// Match C++: AddParameterBlock first
|
|
problem.AddParameterBlock(poseParams, 3);
|
|
|
|
// C# enhancement: Set Pose2D manifold for rotation handling (C++ doesn't use manifold for 2D)
|
|
// Shared manifold instance reused for all parameter blocks
|
|
problem.SetManifold(poseParams, sharedPose2DManifold);
|
|
|
|
// C# enhancement: Set bounds for translation to prevent explosion (C++ doesn't set bounds)
|
|
// Use much tighter bounds based on initial pose to prevent large deviations
|
|
const double MAX_POSE_BOUND = 10.0; // 10m - tight bound to prevent explosion
|
|
double lowerX = poseParams[0] - MAX_POSE_BOUND;
|
|
double upperX = poseParams[0] + MAX_POSE_BOUND;
|
|
double lowerY = poseParams[1] - MAX_POSE_BOUND;
|
|
double upperY = poseParams[1] + MAX_POSE_BOUND;
|
|
|
|
problem.SetParameterLowerBound(poseParams, 0, lowerX); // x
|
|
problem.SetParameterUpperBound(poseParams, 0, upperX);
|
|
problem.SetParameterLowerBound(poseParams, 1, lowerY); // y
|
|
problem.SetParameterUpperBound(poseParams, 1, upperY);
|
|
// Note: Rotation bounds are NOT set because AngleManifold handles rotation constraints
|
|
|
|
// Match C++: Fix nodes of frozen trajectories only
|
|
if (frozen)
|
|
{
|
|
problem.SetParameterBlockConstant(poseParams);
|
|
}
|
|
nodeCount++;
|
|
}
|
|
// Solution 4: Detect conflicting constraints BEFORE adding to optimization
|
|
// Group constraints by (SubmapId, NodeId) to detect conflicts
|
|
var constraintGroups = new Dictionary<(SubmapId, NodeId), List<IPoseGraph.Constraint>>();
|
|
foreach (var constraint in constraints)
|
|
{
|
|
var key = (constraint.SubmapId, constraint.NodeId);
|
|
if (!constraintGroups.TryGetValue(key, out var group))
|
|
{
|
|
group = [];
|
|
constraintGroups[key] = group;
|
|
}
|
|
group.Add(constraint);
|
|
}
|
|
|
|
// Detect and handle conflicting constraints
|
|
int conflictingConstraintsCount = 0;
|
|
int groupsWithMultipleConstraints = 0;
|
|
var validConstraints = new List<IPoseGraph.Constraint>();
|
|
const double CONFLICT_THRESHOLD_TRANSLATION = 0.5; // 50cm difference indicates conflict
|
|
const double CONFLICT_THRESHOLD_ROTATION = 0.2; // ~11 degrees difference indicates conflict
|
|
|
|
foreach (var kvp in constraintGroups)
|
|
{
|
|
var (submapId, nodeId) = kvp.Key;
|
|
var group = kvp.Value;
|
|
|
|
if (group.Count <= 1)
|
|
{
|
|
// No conflict if only one constraint
|
|
validConstraints.AddRange(group);
|
|
continue;
|
|
}
|
|
|
|
groupsWithMultipleConstraints++;
|
|
|
|
// Get parameter blocks to compute expected relative poses
|
|
if (!submapParams.ContainsKey(submapId) || !nodeParams.ContainsKey(nodeId))
|
|
{
|
|
validConstraints.AddRange(group); // Add all if we can't check
|
|
continue;
|
|
}
|
|
|
|
var submapParam = submapParams[submapId];
|
|
var nodeParam = nodeParams[nodeId];
|
|
|
|
// Compute current relative pose from parameters
|
|
var currentSubmapPose = OptimizationHelpers.ParametersToRigid2d(submapParam);
|
|
var currentNodePose = OptimizationHelpers.ParametersToRigid2d(nodeParam);
|
|
var currentRelativePose = currentSubmapPose.Inverse() * currentNodePose;
|
|
|
|
// Check each constraint against other constraints in the group
|
|
var validGroupConstraints = new List<IPoseGraph.Constraint>();
|
|
for (int i = 0; i < group.Count; i++)
|
|
{
|
|
var constraint1 = group[i];
|
|
var constraintPose1 = TransformOperations.Project2D(constraint1.ConstraintPose.ZbarIj);
|
|
|
|
bool isConflicting = false;
|
|
string conflictReason = "";
|
|
|
|
// Check against other constraints in group
|
|
for (int j = 0; j < group.Count; j++)
|
|
{
|
|
if (i == j) continue;
|
|
|
|
var constraint2 = group[j];
|
|
var constraintPose2 = TransformOperations.Project2D(constraint2.ConstraintPose.ZbarIj);
|
|
|
|
var translationDiff = Vector2.Distance(
|
|
new Vector2(constraintPose1.Translation.X, constraintPose1.Translation.Y),
|
|
new Vector2(constraintPose2.Translation.X, constraintPose2.Translation.Y)
|
|
);
|
|
var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
|
|
constraintPose1.Rotation - constraintPose2.Rotation
|
|
));
|
|
|
|
if (translationDiff > CONFLICT_THRESHOLD_TRANSLATION || rotationDiff > CONFLICT_THRESHOLD_ROTATION)
|
|
{
|
|
isConflicting = true;
|
|
conflictReason = $"conflicts with constraint {j} (translationDiff={translationDiff:F3}m, rotationDiff={rotationDiff:F3}rad)";
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (isConflicting)
|
|
{
|
|
conflictingConstraintsCount++;
|
|
}
|
|
else
|
|
{
|
|
validGroupConstraints.Add(constraint1);
|
|
}
|
|
}
|
|
|
|
// If all constraints are conflicting, keep the one with highest weight (most confident)
|
|
if (validGroupConstraints.Count == 0 && group.Count > 0)
|
|
{
|
|
var bestConstraint = group.OrderByDescending(c =>
|
|
c.ConstraintPose.TranslationWeight + c.ConstraintPose.RotationWeight
|
|
).First();
|
|
validGroupConstraints.Add(bestConstraint);
|
|
}
|
|
|
|
validConstraints.AddRange(validGroupConstraints);
|
|
}
|
|
|
|
// Add cost functions for constraints (using filtered validConstraints)
|
|
int constraintCount = 0;
|
|
int skippedInvalidConstraints = 0;
|
|
foreach (var constraint in validConstraints)
|
|
{
|
|
try
|
|
{
|
|
// Validate constraint pose before creating cost function
|
|
var constraintPose = constraint.ConstraintPose.ZbarIj;
|
|
var constraintPose2D = TransformOperations.Project2D(constraintPose);
|
|
|
|
// Check for invalid constraint pose
|
|
if (double.IsNaN(constraintPose2D.Translation.X) || double.IsInfinity(constraintPose2D.Translation.X) ||
|
|
double.IsNaN(constraintPose2D.Translation.Y) || double.IsInfinity(constraintPose2D.Translation.Y) ||
|
|
double.IsNaN(constraintPose2D.Rotation) || double.IsInfinity(constraintPose2D.Rotation))
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
|
|
// Validate weights
|
|
if (double.IsNaN(constraint.ConstraintPose.TranslationWeight) ||
|
|
double.IsInfinity(constraint.ConstraintPose.TranslationWeight) ||
|
|
constraint.ConstraintPose.TranslationWeight <= 0 ||
|
|
double.IsNaN(constraint.ConstraintPose.RotationWeight) ||
|
|
double.IsInfinity(constraint.ConstraintPose.RotationWeight) ||
|
|
constraint.ConstraintPose.RotationWeight <= 0)
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
|
|
// Validate parameter blocks exist
|
|
if (!submapParams.ContainsKey(constraint.SubmapId))
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
if (!nodeParams.ContainsKey(constraint.NodeId))
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
|
|
// Validate parameter values
|
|
var submapParam = submapParams[constraint.SubmapId];
|
|
var nodeParam = nodeParams[constraint.NodeId];
|
|
bool hasInvalidParams = false;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (double.IsNaN(submapParam[i]) || double.IsInfinity(submapParam[i]) ||
|
|
double.IsNaN(nodeParam[i]) || double.IsInfinity(nodeParam[i]))
|
|
{
|
|
hasInvalidParams = true;
|
|
break;
|
|
}
|
|
}
|
|
if (hasInvalidParams)
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
|
|
// Method 3: Validate constraint pose magnitude (similar to Odometry constraints)
|
|
// Skip constraints with very large relative poses (likely outliers)
|
|
const double MAX_RELATIVE_TRANSLATION = 100.0; // 100m
|
|
const double MAX_RELATIVE_ROTATION = 10.0; // ~573 degrees
|
|
var translationMagnitude = Math.Sqrt(constraintPose2D.Translation.X * constraintPose2D.Translation.X +
|
|
constraintPose2D.Translation.Y * constraintPose2D.Translation.Y);
|
|
var rotationMagnitude = Math.Abs(constraintPose2D.Rotation);
|
|
|
|
// Method 3: Additional validation - check if constraint is consistent with current relative pose
|
|
var currentSubmapPose = OptimizationHelpers.ParametersToRigid2d(submapParam);
|
|
var currentNodePose = OptimizationHelpers.ParametersToRigid2d(nodeParam);
|
|
var currentRelativePose = currentSubmapPose.Inverse() * currentNodePose;
|
|
var constraintRelativePose = constraintPose2D;
|
|
|
|
var translationDiff = Vector2.Distance(
|
|
new Vector2(currentRelativePose.Translation.X, currentRelativePose.Translation.Y),
|
|
new Vector2(constraintRelativePose.Translation.X, constraintRelativePose.Translation.Y)
|
|
);
|
|
var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
|
|
currentRelativePose.Rotation - constraintRelativePose.Rotation
|
|
));
|
|
|
|
// Warn if constraint is very different from current relative pose (potential bad constraint)
|
|
const double CONSTRAINT_CONSISTENCY_THRESHOLD_TRANSLATION = 5.0; // 5m difference
|
|
const double CONSTRAINT_CONSISTENCY_THRESHOLD_ROTATION = 1.0; // ~57 degrees difference
|
|
if (translationDiff > CONSTRAINT_CONSISTENCY_THRESHOLD_TRANSLATION || rotationDiff > CONSTRAINT_CONSISTENCY_THRESHOLD_ROTATION)
|
|
{
|
|
}
|
|
|
|
if (translationMagnitude > MAX_RELATIVE_TRANSLATION || rotationMagnitude > MAX_RELATIVE_ROTATION)
|
|
{
|
|
skippedInvalidConstraints++;
|
|
continue;
|
|
}
|
|
|
|
var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(
|
|
constraint.ConstraintPose
|
|
);
|
|
|
|
// Loop closure constraints should have a loss function (matching C++ implementation)
|
|
LossFunction? lossFunction = null;
|
|
var huberScale = options.HuberScale;
|
|
// Only apply HuberLoss to INTER_SUBMAP constraints (matching C++ line 293-294)
|
|
if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap)
|
|
{
|
|
lossFunction = new HuberLoss(huberScale);
|
|
}
|
|
|
|
// Validate parameter blocks are within expected bounds before adding constraint
|
|
const double MAX_EXPECTED_POSE = 10.0;
|
|
const double MAX_EXPECTED_ROTATION = 3.0;
|
|
bool submapParamOutOfBounds = Math.Abs(submapParam[0]) > MAX_EXPECTED_POSE || Math.Abs(submapParam[1]) > MAX_EXPECTED_POSE || Math.Abs(submapParam[2]) > MAX_EXPECTED_ROTATION;
|
|
bool nodeParamOutOfBounds = Math.Abs(nodeParam[0]) > MAX_EXPECTED_POSE || Math.Abs(nodeParam[1]) > MAX_EXPECTED_POSE || Math.Abs(nodeParam[2]) > MAX_EXPECTED_ROTATION;
|
|
|
|
if (submapParamOutOfBounds || nodeParamOutOfBounds)
|
|
{
|
|
}
|
|
|
|
problem.AddResidualBlock(
|
|
costFunction,
|
|
lossFunction,
|
|
[submapParam, nodeParam]
|
|
);
|
|
constraintCount++;
|
|
}
|
|
catch (Exception)
|
|
{
|
|
skippedInvalidConstraints++;
|
|
}
|
|
}
|
|
|
|
// Store initial parameter values for post-solve bounds checking
|
|
var submapInitialParams = new Dictionary<SubmapId, double[]>();
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
submapInitialParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
|
|
}
|
|
|
|
var nodeInitialParams = new Dictionary<NodeId, double[]>();
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
nodeInitialParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
|
|
}
|
|
|
|
// Add landmark cost functions
|
|
var landmarkParams = new Dictionary<string, (double[] rotation, double[] translation)>();
|
|
AddLandmarkCostFunctions(landmarkNodes, nodeParams, landmarkParams, problem, sharedQuaternionManifold);
|
|
|
|
// Add odometry constraints between consecutive nodes
|
|
AddOdometryConstraints(nodeParams, problem, frozenTrajectories);
|
|
|
|
// Add fixed frame pose constraints
|
|
var fixedFrameParams = new Dictionary<int, double[]>();
|
|
AddFixedFramePoseConstraints(nodeParams, fixedFrameParams, problem, frozenTrajectories);
|
|
|
|
// Check if we have any residual blocks
|
|
if (problem.NumResidualBlocks == 0)
|
|
{
|
|
return; // Nothing to optimize
|
|
}
|
|
|
|
|
|
// Solve with retry logic (Strategy 1 + Strategy 3)
|
|
SolverSummary? summary = null;
|
|
const int maxRetries = 3; // 0, 1, 2, 3 = 4 attempts total
|
|
bool optimizationSucceeded = false;
|
|
|
|
for (int retry = 0; retry <= maxRetries; retry++)
|
|
{
|
|
if (retry > 0)
|
|
{
|
|
|
|
// Try different solver configuration on retry
|
|
if (retry == 1)
|
|
{
|
|
// Retry 1: Use DenseSchur instead of SparseSchur
|
|
solverOptions.LinearSolverType = LinearSolverType.DenseSchur;
|
|
solverOptions.MaxNumIterations = Math.Min(10, solverOptions.MaxNumIterations); // Fewer iterations for retry
|
|
}
|
|
else if (retry == 2)
|
|
{
|
|
// Retry 2: Use DenseQR and even fewer iterations
|
|
solverOptions.LinearSolverType = LinearSolverType.DenseQr;
|
|
solverOptions.MaxNumIterations = Math.Min(5, solverOptions.MaxNumIterations);
|
|
}
|
|
else if (retry == 3)
|
|
{
|
|
// Reset parameters to initial values (not zero - frozen params must keep their values)
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
if (submapInitialParams.TryGetValue(kvp.Key, out var initial))
|
|
{
|
|
kvp.Value[0] = initial[0];
|
|
kvp.Value[1] = initial[1];
|
|
kvp.Value[2] = initial[2];
|
|
}
|
|
}
|
|
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
if (nodeInitialParams.TryGetValue(kvp.Key, out var initial))
|
|
{
|
|
kvp.Value[0] = initial[0];
|
|
kvp.Value[1] = initial[1];
|
|
kvp.Value[2] = initial[2];
|
|
}
|
|
}
|
|
|
|
// Use DenseQR with minimal iterations
|
|
solverOptions.LinearSolverType = LinearSolverType.DenseQr;
|
|
solverOptions.MaxNumIterations = Math.Min(3, solverOptions.MaxNumIterations);
|
|
}
|
|
}
|
|
|
|
// Log parameter values BEFORE solving to detect external modifications
|
|
// Log ALL parameter blocks to find which ones are exploding
|
|
|
|
// Check for exploded poses BEFORE optimization
|
|
int explodedSubmapsBefore = 0;
|
|
int explodedNodesBefore = 0;
|
|
const double MAX_EXPECTED_POSE = 10.0;
|
|
const double MAX_EXPECTED_ROTATION = Math.PI; // Use π to match normalized angle range
|
|
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
|
|
if (isExploded)
|
|
explodedSubmapsBefore++;
|
|
}
|
|
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
|
|
if (isExploded)
|
|
explodedNodesBefore++;
|
|
}
|
|
|
|
// Dispose previous retry's SolverSummary before overwriting
|
|
summary?.Dispose();
|
|
summary = problem.Solve(solverOptions);
|
|
|
|
// Check parameter values after solving to detect explosion
|
|
|
|
int explodedSubmapsAfter = 0;
|
|
int explodedNodesAfter = 0;
|
|
int boundsViolationsSubmaps = 0;
|
|
int boundsViolationsNodes = 0;
|
|
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
// Compare against initial values to detect deviation from starting pose
|
|
if (submapInitialParams.TryGetValue(kvp.Key, out var initial))
|
|
{
|
|
bool hasBoundsViolation = Math.Abs(kvp.Value[0] - initial[0]) > 10.0 ||
|
|
Math.Abs(kvp.Value[1] - initial[1]) > 10.0 ||
|
|
Math.Abs(kvp.Value[2] - initial[2]) > 3.0;
|
|
if (hasBoundsViolation) boundsViolationsSubmaps++;
|
|
}
|
|
|
|
bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
|
|
if (isExploded) explodedSubmapsAfter++;
|
|
}
|
|
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
// Compare against initial values to detect deviation from starting pose
|
|
if (nodeInitialParams.TryGetValue(kvp.Key, out var initial))
|
|
{
|
|
bool hasBoundsViolation = Math.Abs(kvp.Value[0] - initial[0]) > 10.0 ||
|
|
Math.Abs(kvp.Value[1] - initial[1]) > 10.0 ||
|
|
Math.Abs(kvp.Value[2] - initial[2]) > 3.0;
|
|
if (hasBoundsViolation) boundsViolationsNodes++;
|
|
}
|
|
|
|
bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
|
|
Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
|
|
if (isExploded) explodedNodesAfter++;
|
|
}
|
|
|
|
if (summary != null && summary.TerminationType != TerminationType.Failure)
|
|
{
|
|
optimizationSucceeded = true;
|
|
|
|
// Check tolerance thresholds
|
|
double functionToleranceThreshold = summary.InitialCost * solverOptions.FunctionTolerance;
|
|
double costChangeRatio = (summary.InitialCost > 0) ? Math.Abs(summary.CostChange) / summary.InitialCost : 0.0;
|
|
bool functionToleranceReached = costChangeRatio <= solverOptions.FunctionTolerance;
|
|
|
|
break; // Success
|
|
}
|
|
}
|
|
|
|
if (summary == null)
|
|
{
|
|
return; // Cannot update poses without summary
|
|
}
|
|
|
|
try
|
|
{
|
|
if (!versionLogged)
|
|
{
|
|
var fullReport = summary.FullReport;
|
|
// Extract version string from FullReport (format: "Solver Summary (v 2.2.0-...)")
|
|
var versionMatch = System.Text.RegularExpressions.Regex.Match(fullReport, @"Solver Summary \(v ([^)]+)\)");
|
|
if (versionMatch.Success)
|
|
{
|
|
var ceresVersion = versionMatch.Groups[1].Value;
|
|
versionLogged = true;
|
|
}
|
|
}
|
|
|
|
// Only update poses if optimization succeeded
|
|
if (!optimizationSucceeded)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Store original parameter values to detect external modifications
|
|
var originalSubmapParams = new Dictionary<SubmapId, double[]>();
|
|
var originalNodeParams = new Dictionary<NodeId, double[]>();
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
originalSubmapParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
|
|
}
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
originalNodeParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
|
|
}
|
|
|
|
// Update poses from optimized parameters with validation and clamping
|
|
const double MAX_POSE_VALUE = 1e6; // Maximum allowed pose value (1 million meters or radians)
|
|
const double MAX_POSE_CHANGE = 1000.0; // Maximum allowed pose change per optimization (1000m or 1000rad)
|
|
|
|
foreach (var kvp in submapParams)
|
|
{
|
|
var oldPose = _submapData[kvp.Key].GlobalPose;
|
|
var poseParams = kvp.Value;
|
|
|
|
// Log optimized pose values and check bounds violations
|
|
const double MAX_POSE_BOUND = 10.0;
|
|
const double MAX_ROTATION_BOUND = 3.0;
|
|
double initialX = oldPose.Translation.X;
|
|
double initialY = oldPose.Translation.Y;
|
|
double initialTheta = oldPose.Rotation;
|
|
double lowerX = initialX - MAX_POSE_BOUND;
|
|
double upperX = initialX + MAX_POSE_BOUND;
|
|
double lowerY = initialY - MAX_POSE_BOUND;
|
|
double upperY = initialY + MAX_POSE_BOUND;
|
|
double lowerTheta = initialTheta - MAX_ROTATION_BOUND;
|
|
double upperTheta = initialTheta + MAX_ROTATION_BOUND;
|
|
|
|
bool xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
|
|
bool yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
|
|
bool thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
|
|
|
|
// Validate and clamp parameters
|
|
bool paramsValid = true;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (double.IsNaN(poseParams[i]) || double.IsInfinity(poseParams[i]))
|
|
{
|
|
paramsValid = false;
|
|
break;
|
|
}
|
|
|
|
// Clamp to reasonable bounds
|
|
if (Math.Abs(poseParams[i]) > MAX_POSE_VALUE)
|
|
{
|
|
poseParams[i] = Math.Sign(poseParams[i]) * MAX_POSE_VALUE;
|
|
}
|
|
}
|
|
|
|
if (!paramsValid)
|
|
continue;
|
|
|
|
// Normalize rotation angle to [-π, π] before converting to Rigid2d
|
|
poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
|
|
|
|
var newPose = OptimizationHelpers.ParametersToRigid2d(poseParams);
|
|
|
|
// Check for excessive pose change
|
|
var poseChange = Math.Sqrt(Math.Pow(newPose.Translation.X - oldPose.Translation.X, 2) +
|
|
Math.Pow(newPose.Translation.Y - oldPose.Translation.Y, 2));
|
|
// Normalize angle difference to handle wrap-around (e.g., π and -π should be close, not 2π apart)
|
|
var rotationChange = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(newPose.Rotation - oldPose.Rotation));
|
|
|
|
if (poseChange > MAX_POSE_CHANGE || rotationChange > MAX_POSE_CHANGE)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var spec = _submapData[kvp.Key];
|
|
spec.GlobalPose = newPose;
|
|
_submapData[kvp.Key] = spec;
|
|
}
|
|
|
|
foreach (var kvp in nodeParams)
|
|
{
|
|
var poseParams = kvp.Value;
|
|
var oldPose2D = _nodeData[kvp.Key].GlobalPose2D;
|
|
|
|
// Log optimized pose values and check bounds violations (only first 10 nodes to avoid log spam)
|
|
const double MAX_POSE_BOUND = 10.0;
|
|
const double MAX_ROTATION_BOUND = Math.PI; // Use π to match normalized angle range
|
|
double initialX = oldPose2D.Translation.X;
|
|
double initialY = oldPose2D.Translation.Y;
|
|
double initialTheta = oldPose2D.Rotation;
|
|
double lowerX = initialX - MAX_POSE_BOUND;
|
|
double upperX = initialX + MAX_POSE_BOUND;
|
|
double lowerY = initialY - MAX_POSE_BOUND;
|
|
double upperY = initialY + MAX_POSE_BOUND;
|
|
double lowerTheta = initialTheta - MAX_ROTATION_BOUND;
|
|
double upperTheta = initialTheta + MAX_ROTATION_BOUND;
|
|
|
|
bool xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
|
|
bool yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
|
|
bool thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
|
|
|
|
// Normalize rotation angle to [-π, π] before checking bounds and converting to Rigid2d
|
|
poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
|
|
|
|
// Re-check bounds after normalization
|
|
xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
|
|
yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
|
|
thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
|
|
|
|
if (xOutOfBounds || yOutOfBounds || thetaOutOfBounds)
|
|
{
|
|
}
|
|
|
|
// Validate and clamp parameters
|
|
bool paramsValid = true;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (double.IsNaN(poseParams[i]) || double.IsInfinity(poseParams[i]))
|
|
{
|
|
paramsValid = false;
|
|
break;
|
|
}
|
|
|
|
// Clamp to reasonable bounds
|
|
if (Math.Abs(poseParams[i]) > MAX_POSE_VALUE)
|
|
{
|
|
poseParams[i] = Math.Sign(poseParams[i]) * MAX_POSE_VALUE;
|
|
}
|
|
}
|
|
|
|
if (!paramsValid)
|
|
continue;
|
|
|
|
var newPose2D = OptimizationHelpers.ParametersToRigid2d(poseParams);
|
|
|
|
// Check for excessive pose change
|
|
var poseChange = Math.Sqrt(Math.Pow(newPose2D.Translation.X - oldPose2D.Translation.X, 2) +
|
|
Math.Pow(newPose2D.Translation.Y - oldPose2D.Translation.Y, 2));
|
|
// Normalize angle difference to handle wrap-around (e.g., π and -π should be close, not 2π apart)
|
|
var rotationChange = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(newPose2D.Rotation - oldPose2D.Rotation));
|
|
|
|
if (poseChange > MAX_POSE_CHANGE || rotationChange > MAX_POSE_CHANGE)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Update GlobalPose3D by embedding 2D pose
|
|
var newPose3D = TransformOperations.Embed3D(newPose2D);
|
|
var spec = _nodeData[kvp.Key];
|
|
spec.GlobalPose2D = newPose2D;
|
|
spec.GlobalPose = newPose3D;
|
|
_nodeData[kvp.Key] = spec;
|
|
}
|
|
|
|
// Update landmark poses from optimized parameters
|
|
foreach (var kvp in landmarkParams)
|
|
{
|
|
var landmarkRotation = OptimizationHelpers.ParametersToQuaternion(kvp.Value.rotation);
|
|
var landmarkTranslation = OptimizationHelpers.ParametersToVector3(kvp.Value.translation);
|
|
_landmarkData[kvp.Key] = new Rigid3d(landmarkTranslation, landmarkRotation);
|
|
}
|
|
|
|
// Update fixed frame poses from optimized parameters
|
|
foreach (var kvp in fixedFrameParams)
|
|
{
|
|
var pose2D = OptimizationHelpers.ParametersToRigid2d(kvp.Value);
|
|
var pose3D = TransformOperations.Embed3D(pose2D);
|
|
if (_trajectoryData.TryGetValue(kvp.Key, out IPoseGraph.TrajectoryData trajectoryData))
|
|
{
|
|
trajectoryData.FixedFrameOriginInMap = pose3D;
|
|
_trajectoryData[kvp.Key] = trajectoryData;
|
|
}
|
|
}
|
|
|
|
} // try
|
|
finally
|
|
{
|
|
// SolverSummary holds native resources - must always be disposed
|
|
summary?.Dispose();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds landmark cost functions to the optimization problem.
|
|
/// </summary>
|
|
private void AddLandmarkCostFunctions(
|
|
Dictionary<string, IPoseGraph.LandmarkNode> landmarkNodes,
|
|
Dictionary<NodeId, double[]> nodeParams,
|
|
Dictionary<string, (double[] rotation, double[] translation)> landmarkParams,
|
|
Problem problem,
|
|
QuaternionManifold sharedQuaternionManifold)
|
|
{
|
|
foreach (var landmarkNode in landmarkNodes)
|
|
{
|
|
var landmarkId = landmarkNode.Key;
|
|
var node = landmarkNode.Value;
|
|
|
|
foreach (var observation in node.LandmarkObservations ?? [])
|
|
{
|
|
// Find nodes before and after the observation time
|
|
var trajectoryNodes = _nodeData.Where(n => n.Id.TrajectoryId == observation.TrajectoryId)
|
|
.OrderBy(n => n.Data.Time)
|
|
.ToList();
|
|
|
|
if (trajectoryNodes.Count == 0)
|
|
continue;
|
|
|
|
// Check if observation time is before first node
|
|
if (observation.Time < trajectoryNodes[0].Data.Time)
|
|
continue;
|
|
|
|
// Find next node
|
|
var nextNode = trajectoryNodes.FirstOrDefault(n => n.Data.Time >= observation.Time);
|
|
if (nextNode.Id.NodeIndex == 0 && nextNode.Data.Time == 0)
|
|
{
|
|
// Time is after all nodes
|
|
continue;
|
|
}
|
|
|
|
var nextIndex = trajectoryNodes.IndexOf(nextNode);
|
|
if (nextIndex == 0)
|
|
{
|
|
nextIndex = 1; // Use first two nodes
|
|
if (nextIndex >= trajectoryNodes.Count)
|
|
continue;
|
|
nextNode = trajectoryNodes[nextIndex];
|
|
}
|
|
|
|
var prevNode = trajectoryNodes[nextIndex - 1];
|
|
|
|
// Get node parameter blocks
|
|
if (!nodeParams.ContainsKey(prevNode.Id) || !nodeParams.ContainsKey(nextNode.Id))
|
|
continue;
|
|
|
|
// Initialize landmark parameters if not already added
|
|
if (!landmarkParams.TryGetValue(landmarkId, out (double[] rotation, double[] translation) value))
|
|
{
|
|
Rigid3d startingPoint;
|
|
if (node.GlobalLandmarkPose.HasValue)
|
|
{
|
|
startingPoint = node.GlobalLandmarkPose.Value;
|
|
}
|
|
else
|
|
{
|
|
// Match C++: GetInitialLandmarkPose
|
|
// Interpolate node poses and multiply by landmark_to_tracking_transform
|
|
var prevNodePose = nodeParams[prevNode.Id]; // [x, y, theta]
|
|
var nextNodePose = nodeParams[nextNode.Id]; // [x, y, theta]
|
|
|
|
// Compute interpolation parameter
|
|
var interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
|
|
observation.Time,
|
|
prevNode.Data.Time,
|
|
nextNode.Data.Time
|
|
);
|
|
|
|
// Interpolate node poses (returns Quaternion rotation and Vector3 translation)
|
|
var (interpolatedRotation, interpolatedTranslation) = CostHelpers.InterpolateNodes2D(
|
|
prevNodePose,
|
|
prevNode.Data.GravityAlignment,
|
|
nextNodePose,
|
|
nextNode.Data.GravityAlignment,
|
|
interpolationParameter
|
|
);
|
|
|
|
// Create interpolated pose from rotation and translation
|
|
var interpolatedPose = new Rigid3d(interpolatedTranslation, interpolatedRotation);
|
|
|
|
// Match C++: starting_point = interpolated_pose * observation.landmark_to_tracking_transform
|
|
startingPoint = interpolatedPose * observation.LandmarkToTrackingTransform;
|
|
}
|
|
|
|
var (landmarkRotation, landmarkTranslation) = OptimizationHelpers.Rigid3dToParameters(startingPoint);
|
|
value = (landmarkRotation, landmarkTranslation);
|
|
landmarkParams[landmarkId] = value;
|
|
|
|
// Add parameter blocks
|
|
problem.AddParameterBlock(landmarkRotation, 4);
|
|
problem.AddParameterBlock(landmarkTranslation, 3);
|
|
|
|
// Set quaternion manifold (shared instance disposed after Problem)
|
|
problem.SetManifold(landmarkRotation, sharedQuaternionManifold);
|
|
|
|
// Set constant if frozen
|
|
if (node.Frozen)
|
|
{
|
|
problem.SetParameterBlockConstant(landmarkRotation);
|
|
problem.SetParameterBlockConstant(landmarkTranslation);
|
|
}
|
|
}
|
|
|
|
// Add cost function
|
|
var costFunction = LandmarkCostFunction2D.CreateAutoDiffCostFunction(
|
|
observation,
|
|
prevNode.Data,
|
|
nextNode.Data
|
|
);
|
|
|
|
var huberScale = options.HuberScale;
|
|
var lossFunction = new HuberLoss(huberScale);
|
|
|
|
problem.AddResidualBlock(
|
|
costFunction,
|
|
lossFunction,
|
|
[
|
|
nodeParams[prevNode.Id],
|
|
nodeParams[nextNode.Id],
|
|
value.rotation,
|
|
value.translation
|
|
]
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Solution 3: Improves initial poses using odometry data.
|
|
/// This helps optimization start from better initial guess, reducing pose explosion.
|
|
/// </summary>
|
|
private void ImproveInitialPosesWithOdometry()
|
|
{
|
|
int improvedNodes = 0;
|
|
int checkedNodes = 0;
|
|
int skippedNoOdometry = 0;
|
|
int skippedOutOfRange = 0;
|
|
const double MAX_ODOMETRY_CORRECTION = 1.0; // Maximum 1m correction from odometry
|
|
|
|
|
|
foreach (var trajectoryId in _nodeData.TrajectoryIds)
|
|
{
|
|
if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? value) || value.Count == 0)
|
|
{
|
|
skippedNoOdometry++;
|
|
continue;
|
|
}
|
|
|
|
var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
|
|
.OrderBy(n => n.Id.NodeIndex)
|
|
.ToList();
|
|
|
|
if (trajectoryNodes.Count < 2)
|
|
continue;
|
|
|
|
// Use odometry to improve initial poses for consecutive nodes
|
|
for (int i = 1; i < trajectoryNodes.Count; i++)
|
|
{
|
|
var prevNode = trajectoryNodes[i - 1];
|
|
var currNode = trajectoryNodes[i];
|
|
|
|
// Calculate relative odometry between nodes
|
|
var relativeOdometry = CalculateOdometryBetweenNodes(
|
|
trajectoryId,
|
|
prevNode.Data,
|
|
currNode.Data
|
|
);
|
|
|
|
if (relativeOdometry == null)
|
|
continue;
|
|
|
|
// Project to 2D
|
|
var relativeOdometry2D = TransformOperations.Project2D(relativeOdometry.Value);
|
|
|
|
// Compute expected current node pose based on previous node pose and odometry
|
|
var prevPose2D = prevNode.Data.GlobalPose2D;
|
|
var expectedPose2D = prevPose2D * relativeOdometry2D;
|
|
|
|
// Get current node pose
|
|
var currentPose2D = currNode.Data.GlobalPose2D;
|
|
|
|
// Calculate difference
|
|
var translationDiff = Vector2.Distance(
|
|
new Vector2(expectedPose2D.Translation.X, expectedPose2D.Translation.Y),
|
|
new Vector2(currentPose2D.Translation.X, currentPose2D.Translation.Y)
|
|
);
|
|
var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
|
|
expectedPose2D.Rotation - currentPose2D.Rotation
|
|
));
|
|
|
|
checkedNodes++;
|
|
|
|
// If difference is significant but reasonable, use odometry-based pose
|
|
if (translationDiff > 0.1 && translationDiff < MAX_ODOMETRY_CORRECTION && rotationDiff < 0.5)
|
|
{
|
|
// Blend: use 70% odometry-based pose, 30% current pose
|
|
// This provides better initial guess while maintaining some stability
|
|
var blendedTranslation = new Vector2(
|
|
(0.7 * expectedPose2D.Translation.X + 0.3 * currentPose2D.Translation.X),
|
|
(0.7 * expectedPose2D.Translation.Y + 0.3 * currentPose2D.Translation.Y)
|
|
);
|
|
var blendedRotation = OptimizationHelpers.NormalizeAngleDifference(
|
|
0.7 * expectedPose2D.Rotation + 0.3 * currentPose2D.Rotation
|
|
);
|
|
|
|
var improvedPose2D = new Rigid2d(blendedTranslation, blendedRotation);
|
|
var improvedPose3D = TransformOperations.Embed3D(improvedPose2D);
|
|
|
|
var spec = _nodeData[currNode.Id];
|
|
spec.GlobalPose2D = improvedPose2D;
|
|
spec.GlobalPose = improvedPose3D;
|
|
_nodeData[currNode.Id] = spec;
|
|
|
|
improvedNodes++;
|
|
|
|
if (improvedNodes <= 10) // Log first 10 improvements
|
|
{
|
|
}
|
|
}
|
|
else
|
|
{
|
|
skippedOutOfRange++;
|
|
if (checkedNodes <= 5 || translationDiff >= MAX_ODOMETRY_CORRECTION || rotationDiff >= 0.5)
|
|
{
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds odometry constraints between consecutive nodes.
|
|
/// </summary>
|
|
private void AddOdometryConstraints(
|
|
Dictionary<NodeId, double[]> nodeParams,
|
|
Problem problem,
|
|
HashSet<int> frozenTrajectories)
|
|
{
|
|
// Constants for validating relative poses before adding constraints
|
|
const double MAX_RELATIVE_TRANSLATION = 100.0; // 100m
|
|
const double MAX_RELATIVE_ROTATION = 10.0; // ~573 degrees
|
|
foreach (var trajectoryId in _nodeData.TrajectoryIds)
|
|
{
|
|
if (frozenTrajectories.Contains(trajectoryId))
|
|
continue;
|
|
|
|
var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
|
|
.OrderBy(n => n.Id.NodeIndex)
|
|
.Select(n => new { n.Id, n.Data })
|
|
.ToList();
|
|
|
|
for (int i = 1; i < trajectoryNodes.Count; i++)
|
|
{
|
|
var prevNode = trajectoryNodes[i - 1];
|
|
var currNode = trajectoryNodes[i];
|
|
|
|
// Only add constraint for consecutive nodes
|
|
if (currNode.Id.NodeIndex != prevNode.Id.NodeIndex + 1)
|
|
continue;
|
|
|
|
// Try to get relative odometry
|
|
var relativeOdometry = CalculateOdometryBetweenNodes(
|
|
trajectoryId,
|
|
prevNode.Data,
|
|
currNode.Data
|
|
);
|
|
|
|
if (relativeOdometry != null)
|
|
{
|
|
// Validate relative odometry pose before adding constraint
|
|
var relativeOdometry2D = TransformOperations.Project2D(relativeOdometry.Value);
|
|
var odomTranslationMagnitude = Math.Sqrt(relativeOdometry2D.Translation.X * relativeOdometry2D.Translation.X +
|
|
relativeOdometry2D.Translation.Y * relativeOdometry2D.Translation.Y);
|
|
var odomRotationMagnitude = Math.Abs(relativeOdometry2D.Rotation);
|
|
|
|
// Skip constraints with very large relative poses (likely outliers)
|
|
if (odomTranslationMagnitude > MAX_RELATIVE_TRANSLATION || odomRotationMagnitude > MAX_RELATIVE_ROTATION)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Add odometry constraint
|
|
// Match C++: nullptr loss function for odometry constraints
|
|
var constraintPose = new IPoseGraph.Constraint.Pose(
|
|
relativeOdometry.Value,
|
|
options.OdometryTranslationWeight,
|
|
options.OdometryRotationWeight
|
|
);
|
|
var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(constraintPose);
|
|
problem.AddResidualBlock(
|
|
costFunction,
|
|
null, // Match C++: nullptr loss function
|
|
[nodeParams[prevNode.Id], nodeParams[currNode.Id]]
|
|
);
|
|
}
|
|
|
|
// Always add local SLAM pose constraint
|
|
var relativeLocalSlamPose = TransformOperations.Embed3D(
|
|
prevNode.Data.LocalPose2D.Inverse() * currNode.Data.LocalPose2D
|
|
);
|
|
var relativeLocalSlamPose2D = TransformOperations.Project2D(relativeLocalSlamPose);
|
|
var localSlamTranslationMagnitude = Math.Sqrt(relativeLocalSlamPose2D.Translation.X * relativeLocalSlamPose2D.Translation.X +
|
|
relativeLocalSlamPose2D.Translation.Y * relativeLocalSlamPose2D.Translation.Y);
|
|
var localSlamRotationMagnitude = Math.Abs(relativeLocalSlamPose2D.Rotation);
|
|
|
|
// Skip constraints with very large relative poses (likely outliers)
|
|
if (localSlamTranslationMagnitude > MAX_RELATIVE_TRANSLATION || localSlamRotationMagnitude > MAX_RELATIVE_ROTATION)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var localSlamConstraintPose = new IPoseGraph.Constraint.Pose(
|
|
relativeLocalSlamPose,
|
|
options.LocalSlamPoseTranslationWeight,
|
|
options.LocalSlamPoseRotationWeight
|
|
);
|
|
var localSlamCostFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(localSlamConstraintPose);
|
|
// Match C++: nullptr loss function for local SLAM pose constraints
|
|
problem.AddResidualBlock(
|
|
localSlamCostFunction,
|
|
null, // Match C++: nullptr loss function
|
|
[nodeParams[prevNode.Id], nodeParams[currNode.Id]]
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds fixed frame pose constraints.
|
|
/// </summary>
|
|
private void AddFixedFramePoseConstraints(
|
|
Dictionary<NodeId, double[]> nodeParams,
|
|
Dictionary<int, double[]> fixedFrameParams,
|
|
Problem problem,
|
|
HashSet<int> frozenTrajectories)
|
|
{
|
|
foreach (var trajectoryId in _nodeData.TrajectoryIds)
|
|
{
|
|
if (frozenTrajectories.Contains(trajectoryId))
|
|
continue;
|
|
|
|
if (!_fixedFramePoseData.ContainsKey(trajectoryId))
|
|
continue;
|
|
|
|
if (!_trajectoryData.ContainsKey(trajectoryId))
|
|
continue;
|
|
|
|
var trajectoryData = _trajectoryData[trajectoryId];
|
|
var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
|
|
.OrderBy(n => n.Data.Time)
|
|
.ToList();
|
|
|
|
bool fixedFramePoseInitialized = false;
|
|
|
|
foreach (var node in trajectoryNodes)
|
|
{
|
|
var fixedFramePose = InterpolateFixedFramePose(trajectoryId, node.Data.Time);
|
|
if (fixedFramePose == null)
|
|
continue;
|
|
|
|
var constraintPose = new IPoseGraph.Constraint.Pose(
|
|
fixedFramePose.Value,
|
|
options.FixedFramePoseTranslationWeight,
|
|
options.FixedFramePoseRotationWeight
|
|
);
|
|
|
|
if (!fixedFramePoseInitialized)
|
|
{
|
|
Rigid2d fixedFramePoseInMap;
|
|
if (trajectoryData.FixedFrameOriginInMap.HasValue)
|
|
{
|
|
fixedFramePoseInMap = TransformOperations.Project2D(trajectoryData.FixedFrameOriginInMap.Value);
|
|
}
|
|
else
|
|
{
|
|
// Initialize from node pose
|
|
var relativePose2D = TransformOperations.Project2D(constraintPose.ZbarIj);
|
|
fixedFramePoseInMap = node.Data.GlobalPose2D * relativePose2D.Inverse();
|
|
}
|
|
|
|
var fixedFramePoseParams = new double[3]
|
|
{
|
|
fixedFramePoseInMap.Translation.X,
|
|
fixedFramePoseInMap.Translation.Y,
|
|
fixedFramePoseInMap.Rotation
|
|
};
|
|
|
|
fixedFrameParams[trajectoryId] = fixedFramePoseParams;
|
|
problem.AddParameterBlock(fixedFramePoseParams, 3);
|
|
fixedFramePoseInitialized = true;
|
|
}
|
|
|
|
// Add cost function
|
|
var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(constraintPose);
|
|
LossFunction? lossFunction = null;
|
|
if (options.FixedFramePoseUseTolerantLoss)
|
|
{
|
|
lossFunction = new TolerantLoss(
|
|
options.FixedFramePoseTolerantLossParamA,
|
|
options.FixedFramePoseTolerantLossParamB
|
|
);
|
|
}
|
|
|
|
problem.AddResidualBlock(
|
|
costFunction,
|
|
lossFunction,
|
|
[fixedFrameParams[trajectoryId], nodeParams[node.Id]]
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|