Initial commit
This commit is contained in:
@@ -0,0 +1,199 @@
|
||||
/*
|
||||
* 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 CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Helper functions for cost function computation.
|
||||
/// </summary>
|
||||
internal static class CostHelpers
|
||||
{
|
||||
/// <summary>
|
||||
/// Computes spherical linear interpolation of unit quaternions.
|
||||
/// </summary>
|
||||
public static Quaternion SlerpQuaternions(Quaternion start, Quaternion end, double factor)
|
||||
{
|
||||
// Normalize quaternions
|
||||
start = Quaternion.Normalize(start);
|
||||
end = Quaternion.Normalize(end);
|
||||
|
||||
// Compute dot product
|
||||
var cosTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
|
||||
// Clamp to [-1, 1] to handle floating-point errors that could cause Math.Acos to return NaN
|
||||
var absCosTheta = Math.Min(1.0, Math.Abs(cosTheta));
|
||||
|
||||
// If quaternions are nearly collinear, use linear interpolation
|
||||
const double kEpsilon = 1e-6;
|
||||
double prevScale = 1.0 - factor;
|
||||
double nextScale = factor;
|
||||
|
||||
if (absCosTheta < 1.0 - kEpsilon)
|
||||
{
|
||||
var theta = Math.Acos(absCosTheta);
|
||||
var sinTheta = Math.Sin(theta);
|
||||
if (sinTheta > kEpsilon)
|
||||
{
|
||||
prevScale = Math.Sin((1.0 - factor) * theta) / sinTheta;
|
||||
nextScale = Math.Sin(factor * theta) / sinTheta;
|
||||
}
|
||||
}
|
||||
|
||||
if (cosTheta < 0.0)
|
||||
{
|
||||
nextScale = -nextScale;
|
||||
}
|
||||
|
||||
// Quaternion constructor is (x, y, z, w), matching C++ output format [w, x, y, z]
|
||||
// but converting to C# Quaternion format (x, y, z, w)
|
||||
var result = new Quaternion(
|
||||
prevScale * start.X + nextScale * end.X,
|
||||
prevScale * start.Y + nextScale * end.Y,
|
||||
prevScale * start.Z + nextScale * end.Z,
|
||||
prevScale * start.W + nextScale * end.W
|
||||
);
|
||||
// Normalize to ensure unit quaternion (Eigen SLERP automatically normalizes)
|
||||
return Quaternion.Normalize(result);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Interpolates 3D nodes.
|
||||
/// </summary>
|
||||
public static (Quaternion rotation, Vector3 translation) InterpolateNodes3D(
|
||||
double[] prevNodeRotation, // [w, x, y, z]
|
||||
double[] prevNodeTranslation, // [x, y, z]
|
||||
double[] nextNodeRotation, // [w, x, y, z]
|
||||
double[] nextNodeTranslation, // [x, y, z]
|
||||
double interpolationParameter)
|
||||
{
|
||||
// Match C++: prev_node_rotation is [w, x, y, z]
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
var prevQuaternion = new Quaternion(
|
||||
prevNodeRotation[1], // x
|
||||
prevNodeRotation[2], // y
|
||||
prevNodeRotation[3], // z
|
||||
prevNodeRotation[0] // w
|
||||
);
|
||||
var nextQuaternion = new Quaternion(
|
||||
nextNodeRotation[1], // x
|
||||
nextNodeRotation[2], // y
|
||||
nextNodeRotation[3], // z
|
||||
nextNodeRotation[0] // w
|
||||
);
|
||||
|
||||
// Interpolate rotation using SLERP
|
||||
var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
|
||||
|
||||
// Interpolate translation linearly
|
||||
var interpolatedTranslation = new Vector3(
|
||||
(prevNodeTranslation[0] + interpolationParameter * (nextNodeTranslation[0] - prevNodeTranslation[0])),
|
||||
(prevNodeTranslation[1] + interpolationParameter * (nextNodeTranslation[1] - prevNodeTranslation[1])),
|
||||
(prevNodeTranslation[2] + interpolationParameter * (nextNodeTranslation[2] - prevNodeTranslation[2]))
|
||||
);
|
||||
|
||||
return (interpolatedRotation, interpolatedTranslation);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Interpolates 2D nodes embedded in 3D space.
|
||||
/// </summary>
|
||||
public static (Quaternion rotation, Vector3 translation) InterpolateNodes2D(
|
||||
double[] prevNodePose, // [x, y, theta]
|
||||
Quaternion prevNodeGravityAlignment,
|
||||
double[] nextNodePose, // [x, y, theta]
|
||||
Quaternion nextNodeGravityAlignment,
|
||||
double interpolationParameter)
|
||||
{
|
||||
// Embed 2D pose into 3D with gravity alignment
|
||||
// Equivalent to: Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)
|
||||
var prevRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, prevNodePose[2]);
|
||||
var prevQuaternion = Quaternion.Normalize(prevRotation2D * prevNodeGravityAlignment);
|
||||
|
||||
var nextRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, nextNodePose[2]);
|
||||
var nextQuaternion = Quaternion.Normalize(nextRotation2D * nextNodeGravityAlignment);
|
||||
|
||||
// Interpolate rotation using SLERP
|
||||
var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
|
||||
|
||||
// Interpolate translation linearly (2D, z=0)
|
||||
var interpolatedTranslation = new Vector3(
|
||||
(prevNodePose[0] + interpolationParameter * (nextNodePose[0] - prevNodePose[0])),
|
||||
(prevNodePose[1] + interpolationParameter * (nextNodePose[1] - prevNodePose[1])),
|
||||
0.0
|
||||
);
|
||||
|
||||
return (interpolatedRotation, interpolatedTranslation);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes unscaled error for 3D poses.
|
||||
/// Error = observed_relative_pose - computed_relative_pose
|
||||
/// </summary>
|
||||
public static double[] ComputeUnscaledError3D(
|
||||
Rigid3d observedRelativePose,
|
||||
Quaternion startRotation,
|
||||
Vector3 startTranslation,
|
||||
Quaternion endRotation,
|
||||
Vector3 endTranslation)
|
||||
{
|
||||
// Compute relative transform: start^-1 * end
|
||||
var startInverse = Quaternion.Inverse(startRotation);
|
||||
var deltaTranslation = endTranslation - startTranslation;
|
||||
var rotatedDelta = Vector3.Transform(deltaTranslation, startInverse);
|
||||
|
||||
// Compute h_rotation_inverse = (end^-1) * start (matching C++ implementation)
|
||||
// This is equivalent to: endRotation.Inverse() * startRotation
|
||||
var endInverse = Quaternion.Inverse(endRotation);
|
||||
var hRotationInverse = endInverse * startRotation;
|
||||
|
||||
// Error rotation: h_rotation_inverse * observed_relative_rotation
|
||||
var errorRotation = hRotationInverse * observedRelativePose.Rotation;
|
||||
|
||||
// Convert rotation error to angle-axis
|
||||
var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(errorRotation);
|
||||
|
||||
return
|
||||
[
|
||||
observedRelativePose.Translation.X - rotatedDelta.X,
|
||||
observedRelativePose.Translation.Y - rotatedDelta.Y,
|
||||
observedRelativePose.Translation.Z - rotatedDelta.Z,
|
||||
angleAxis.X,
|
||||
angleAxis.Y,
|
||||
angleAxis.Z
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scales error with translation and rotation weights.
|
||||
/// </summary>
|
||||
public static double[] ScaleError3D(
|
||||
double[] unscaledError,
|
||||
double translationWeight,
|
||||
double rotationWeight)
|
||||
{
|
||||
return
|
||||
[
|
||||
translationWeight * unscaledError[0],
|
||||
translationWeight * unscaledError[1],
|
||||
translationWeight * unscaledError[2],
|
||||
rotationWeight * unscaledError[3],
|
||||
rotationWeight * unscaledError[4],
|
||||
rotationWeight * unscaledError[5]
|
||||
];
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* 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 CeresSharp;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Cost function measuring the weighted error between the observed pose given by
|
||||
/// the landmark measurement and the linearly interpolated pose of embedded in 3D
|
||||
/// space node poses.
|
||||
/// </summary>
|
||||
public class LandmarkCostFunction2D
|
||||
{
|
||||
private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
|
||||
private readonly NodeSpec2D _prevNode;
|
||||
private readonly NodeSpec2D _nextNode;
|
||||
private readonly double _interpolationParameter;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for landmark constraints.
|
||||
/// </summary>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
IPoseGraph.LandmarkNode.LandmarkObservation observation,
|
||||
NodeSpec2D prevNode,
|
||||
NodeSpec2D nextNode)
|
||||
{
|
||||
var costFunction = new LandmarkCostFunction2D(observation, prevNode, nextNode);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
|
||||
parameterBlockSizes: [3, 3, 4, 3] // [prev_node[3], next_node[3], landmark_rotation[4], landmark_translation[3]]
|
||||
);
|
||||
}
|
||||
|
||||
private LandmarkCostFunction2D(
|
||||
IPoseGraph.LandmarkNode.LandmarkObservation observation,
|
||||
NodeSpec2D prevNode,
|
||||
NodeSpec2D nextNode)
|
||||
{
|
||||
_observation = observation;
|
||||
_prevNode = prevNode;
|
||||
_nextNode = nextNode;
|
||||
|
||||
// Compute interpolation parameter
|
||||
_interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
|
||||
_observation.Time,
|
||||
_prevNode.Time,
|
||||
_nextNode.Time
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 4)
|
||||
return false;
|
||||
if (parameters[0].Length < 3 || parameters[1].Length < 3 ||
|
||||
parameters[2].Length < 4 || parameters[3].Length < 3)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 6)
|
||||
return false;
|
||||
|
||||
var prevNodePose = parameters[0]; // [x, y, theta]
|
||||
var nextNodePose = parameters[1]; // [x, y, theta]
|
||||
var landmarkRotation = parameters[2]; // [w, x, y, z]
|
||||
var landmarkTranslation = parameters[3]; // [x, y, z]
|
||||
|
||||
// Interpolate node poses
|
||||
var (interpolatedRotation, interpolatedTranslation) = CostHelpers.InterpolateNodes2D(
|
||||
prevNodePose,
|
||||
_prevNode.GravityAlignment,
|
||||
nextNodePose,
|
||||
_nextNode.GravityAlignment,
|
||||
_interpolationParameter
|
||||
);
|
||||
|
||||
// Landmark pose parameters
|
||||
var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
|
||||
var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
|
||||
|
||||
// The landmark cost function computes error between:
|
||||
// - observed: landmark_to_tracking_transform (from observation)
|
||||
// - computed: (interpolated_tracking_pose^-1 * landmark_pose)
|
||||
// Error = observed - computed
|
||||
// This is equivalent to: landmark_to_tracking_transform - (interpolated_pose^-1 * landmark_pose)
|
||||
var unscaledError = CostHelpers.ComputeUnscaledError3D(
|
||||
_observation.LandmarkToTrackingTransform,
|
||||
interpolatedRotation,
|
||||
interpolatedTranslation,
|
||||
landmarkRotationQuat,
|
||||
landmarkTranslationVec
|
||||
);
|
||||
|
||||
// Scale error
|
||||
var scaledError = CostHelpers.ScaleError3D(
|
||||
unscaledError,
|
||||
_observation.TranslationWeight,
|
||||
_observation.RotationWeight
|
||||
);
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
residuals[i] = scaledError[i];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* 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 CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
using CeresSharp;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Cost function measuring the weighted error between the observed pose given by
|
||||
/// the landmark measurement and the linearly interpolated pose.
|
||||
/// </summary>
|
||||
public class LandmarkCostFunction3D
|
||||
{
|
||||
private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
|
||||
private readonly NodeSpec3D _prevNode;
|
||||
private readonly NodeSpec3D _nextNode;
|
||||
private readonly double _interpolationParameter;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for landmark constraints in 3D.
|
||||
/// </summary>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
IPoseGraph.LandmarkNode.LandmarkObservation observation,
|
||||
NodeSpec3D prevNode,
|
||||
NodeSpec3D nextNode)
|
||||
{
|
||||
var costFunction = new LandmarkCostFunction3D(observation, prevNode, nextNode);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
|
||||
parameterBlockSizes: [4, 3, 4, 3, 4, 3] // [prev_rotation[4], prev_translation[3], next_rotation[4], next_translation[3], landmark_rotation[4], landmark_translation[3]]
|
||||
);
|
||||
}
|
||||
|
||||
private LandmarkCostFunction3D(
|
||||
IPoseGraph.LandmarkNode.LandmarkObservation observation,
|
||||
NodeSpec3D prevNode,
|
||||
NodeSpec3D nextNode)
|
||||
{
|
||||
_observation = observation;
|
||||
_prevNode = prevNode;
|
||||
_nextNode = nextNode;
|
||||
|
||||
// Compute interpolation parameter
|
||||
_interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
|
||||
_observation.Time,
|
||||
_prevNode.Time,
|
||||
_nextNode.Time
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 6)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 3 ||
|
||||
parameters[2].Length < 4 || parameters[3].Length < 3 ||
|
||||
parameters[4].Length < 4 || parameters[5].Length < 3)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 6)
|
||||
return false;
|
||||
|
||||
var prevNodeRotation = parameters[0]; // [w, x, y, z]
|
||||
var prevNodeTranslation = parameters[1]; // [x, y, z]
|
||||
var nextNodeRotation = parameters[2]; // [w, x, y, z]
|
||||
var nextNodeTranslation = parameters[3]; // [x, y, z]
|
||||
var landmarkRotation = parameters[4]; // [w, x, y, z]
|
||||
var landmarkTranslation = parameters[5]; // [x, y, z]
|
||||
|
||||
// Interpolate node poses
|
||||
var (interpolatedRotationQuat, interpolatedTranslationVec) = CostHelpers.InterpolateNodes3D(
|
||||
prevNodeRotation,
|
||||
prevNodeTranslation,
|
||||
nextNodeRotation,
|
||||
nextNodeTranslation,
|
||||
_interpolationParameter
|
||||
);
|
||||
|
||||
var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
|
||||
var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
|
||||
|
||||
// Compute error
|
||||
var unscaledError = CostHelpers.ComputeUnscaledError3D(
|
||||
_observation.LandmarkToTrackingTransform,
|
||||
interpolatedRotationQuat,
|
||||
interpolatedTranslationVec,
|
||||
landmarkRotationQuat,
|
||||
landmarkTranslationVec
|
||||
);
|
||||
|
||||
// Scale error
|
||||
var scaledError = CostHelpers.ScaleError3D(
|
||||
unscaledError,
|
||||
_observation.TranslationWeight,
|
||||
_observation.RotationWeight
|
||||
);
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
residuals[i] = scaledError[i];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Helper utilities for optimization problems.
|
||||
/// Provides common operations for pose parameter conversion and angle normalization.
|
||||
/// </summary>
|
||||
public static class OptimizationHelpers
|
||||
{
|
||||
/// <summary>
|
||||
/// Normalizes angle difference to [-pi, pi].
|
||||
/// Uses modulo-based approach for efficiency with large angles.
|
||||
/// </summary>
|
||||
/// <param name="angle">The angle to normalize.</param>
|
||||
/// <returns>Normalized angle in [-pi, pi].</returns>
|
||||
public static double NormalizeAngleDifference(double angle)
|
||||
{
|
||||
// Use modulo for efficiency - handles large angles in O(1)
|
||||
const double twoPi = 2.0 * Math.PI;
|
||||
angle = angle % twoPi;
|
||||
if (angle > Math.PI)
|
||||
angle -= twoPi;
|
||||
else if (angle < -Math.PI)
|
||||
angle += twoPi;
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts Rigid2d pose to parameter array [x, y, theta].
|
||||
/// </summary>
|
||||
/// <param name="pose">The 2D pose.</param>
|
||||
/// <returns>Parameter array [x, y, theta].</returns>
|
||||
public static double[] Rigid2dToParameters(Rigid2d pose) => [ pose.Translation.X, pose.Translation.Y, pose.Rotation ];
|
||||
|
||||
/// <summary>
|
||||
/// Converts parameter array [x, y, theta] to Rigid2d pose.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter array [x, y, theta].</param>
|
||||
/// <returns>The 2D pose.</returns>
|
||||
public static Rigid2d ParametersToRigid2d(double[] parameters)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 3)
|
||||
throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));
|
||||
|
||||
return new Rigid2d(
|
||||
new Vector2(parameters[0], parameters[1]),
|
||||
parameters[2]
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts Rigid3d pose to parameter arrays (rotation and translation).
|
||||
/// </summary>
|
||||
/// <param name="pose">The 3D pose.</param>
|
||||
/// <returns>Tuple of (rotation[4], translation[3]).</returns>
|
||||
public static (double[] rotation, double[] translation) Rigid3dToParameters(Rigid3d pose)
|
||||
{
|
||||
var rotation = new double[4]
|
||||
{
|
||||
pose.Rotation.W,
|
||||
pose.Rotation.X,
|
||||
pose.Rotation.Y,
|
||||
pose.Rotation.Z
|
||||
};
|
||||
var translation = new double[3]
|
||||
{
|
||||
pose.Translation.X,
|
||||
pose.Translation.Y,
|
||||
pose.Translation.Z
|
||||
};
|
||||
return (rotation, translation);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts parameter arrays to Rigid3d pose.
|
||||
/// </summary>
|
||||
/// <param name="rotation">Rotation parameters [w, x, y, z].</param>
|
||||
/// <param name="translation">Translation parameters [x, y, z].</param>
|
||||
/// <returns>The 3D pose.</returns>
|
||||
public static Rigid3d ParametersToRigid3d(double[] rotation, double[] translation)
|
||||
{
|
||||
if (rotation == null || rotation.Length < 4)
|
||||
throw new ArgumentException("Rotation array must have at least 4 elements", nameof(rotation));
|
||||
if (translation == null || translation.Length < 3)
|
||||
throw new ArgumentException("Translation array must have at least 3 elements", nameof(translation));
|
||||
|
||||
// Convert from [w, x, y, z] to (x, y, z, w) for System.Numerics.Quaternion
|
||||
return new Rigid3d(
|
||||
new Vector3(translation[0], translation[1], translation[2]),
|
||||
new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts Quaternion to parameter array [w, x, y, z].
|
||||
/// </summary>
|
||||
/// <param name="quaternion">The quaternion.</param>
|
||||
/// <returns>Parameter array [w, x, y, z].</returns>
|
||||
public static double[] QuaternionToParameters(Quaternion quaternion) => [ quaternion.W, quaternion.X, quaternion.Y, quaternion.Z ];
|
||||
|
||||
/// <summary>
|
||||
/// Converts parameter array [w, x, y, z] to System.Numerics.Quaternion (x, y, z, w).
|
||||
/// Match C++: Eigen::Quaternion<T> uses (w, x, y, z) format.
|
||||
/// System.Numerics.Quaternion uses (x, y, z, w) format.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter array [w, x, y, z].</param>
|
||||
/// <returns>The quaternion.</returns>
|
||||
public static Quaternion ParametersToQuaternion(double[] parameters)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 4)
|
||||
throw new ArgumentException("Parameters array must have at least 4 elements", nameof(parameters));
|
||||
|
||||
// Convert from [w, x, y, z] to (x, y, z, w)
|
||||
return new Quaternion(
|
||||
parameters[1], // x
|
||||
parameters[2], // y
|
||||
parameters[3], // z
|
||||
parameters[0] // w
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts Vector3 to parameter array [x, y, z].
|
||||
/// </summary>
|
||||
/// <param name="vector">The vector.</param>
|
||||
/// <returns>Parameter array [x, y, z].</returns>
|
||||
public static double[] Vector3ToParameters(Vector3 vector) => [ vector.X, vector.Y, vector.Z ];
|
||||
|
||||
/// <summary>
|
||||
/// Converts parameter array [x, y, z] to Vector3.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter array [x, y, z].</param>
|
||||
/// <returns>The vector.</returns>
|
||||
public static Vector3 ParametersToVector3(double[] parameters)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 3)
|
||||
throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));
|
||||
|
||||
return new Vector3(
|
||||
parameters[0],
|
||||
parameters[1],
|
||||
parameters[2]
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes interpolation parameter for time-based interpolation.
|
||||
/// </summary>
|
||||
/// <param name="observationTime">The observation time.</param>
|
||||
/// <param name="prevTime">The previous node time.</param>
|
||||
/// <param name="nextTime">The next node time.</param>
|
||||
/// <returns>Interpolation parameter in [0, 1].</returns>
|
||||
public static double ComputeInterpolationParameter(long observationTime, long prevTime, long nextTime)
|
||||
{
|
||||
var timeDiff = nextTime - prevTime;
|
||||
if (timeDiff == 0)
|
||||
return 0.0;
|
||||
// Cast to double to avoid integer division
|
||||
return (double)(observationTime - prevTime) / timeDiff;
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,187 @@
|
||||
/*
|
||||
* 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 CartographerSharp.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Sparse Pose Adjustment (SPA) cost function for 2D pose graph optimization.
|
||||
/// Computes the error between observed relative pose and computed relative pose.
|
||||
/// </summary>
|
||||
public class SpaCostFunction2D
|
||||
{
|
||||
private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
|
||||
private readonly Rigid2d _observedRelativePose2D;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for SPA.
|
||||
/// </summary>
|
||||
/// <param name="observedRelativePose">The observed relative pose constraint.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
IPoseGraph.Constraint.Pose observedRelativePose)
|
||||
{
|
||||
var costFunction = new SpaCostFunction2D(observedRelativePose);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 3, // [dx, dy, dtheta]
|
||||
parameterBlockSizes: [3, 3] // [start_pose[3], end_pose[3]]
|
||||
);
|
||||
}
|
||||
|
||||
private SpaCostFunction2D(IPoseGraph.Constraint.Pose observedRelativePose)
|
||||
{
|
||||
_observedRelativePose = observedRelativePose;
|
||||
// Project 3D pose to 2D
|
||||
_observedRelativePose2D = TransformOperations.Project2D(observedRelativePose.ZbarIj);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// Match C++ spa_cost_function_2d.h operator() implementation.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [start_pose[3], end_pose[3]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dtheta].</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 2)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (parameters[0].Length < 3 || parameters[1].Length < 3)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var startPose = parameters[0];
|
||||
var endPose = parameters[1];
|
||||
|
||||
// Validate parameters for NaN/Infinity
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (double.IsNaN(startPose[i]) || double.IsInfinity(startPose[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (double.IsNaN(endPose[i]) || double.IsInfinity(endPose[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// NOTE: Weight validation removed to match C++ behavior.
|
||||
// C++ does not validate weights - Ceres handles invalid weights internally.
|
||||
// Validation was causing constraints to be incorrectly rejected.
|
||||
|
||||
// NOTE: Pose explosion handling REMOVED to match C++ behavior.
|
||||
// The original C++ spa_cost_function_2d.h does NOT have any pose distance checks.
|
||||
// Returning zero residuals was causing optimization to skip constraints incorrectly,
|
||||
// leading to optimization failures and incorrect pose graph results.
|
||||
// If poses diverge, Ceres will handle it through its own convergence criteria.
|
||||
|
||||
// Compute unscaled error (match C++ cost_helpers_impl.h ComputeUnscaledError)
|
||||
var unscaledError = ComputeUnscaledError(
|
||||
_observedRelativePose2D,
|
||||
startPose,
|
||||
endPose
|
||||
);
|
||||
|
||||
// Scale error with weights (match C++ ScaleError)
|
||||
var translationWeight = _observedRelativePose.TranslationWeight;
|
||||
var rotationWeight = _observedRelativePose.RotationWeight;
|
||||
|
||||
var scaledError = ScaleError(
|
||||
unscaledError,
|
||||
translationWeight,
|
||||
rotationWeight
|
||||
);
|
||||
|
||||
residuals[0] = scaledError[0];
|
||||
residuals[1] = scaledError[1];
|
||||
residuals[2] = scaledError[2];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes unscaled error between observed and computed relative pose.
|
||||
/// Match C++: Uses direct formula for numerical stability with Ceres autodiff.
|
||||
/// </summary>
|
||||
private static double[] ComputeUnscaledError(
|
||||
Rigid2d observedRelativePose,
|
||||
double[] startPose,
|
||||
double[] endPose)
|
||||
{
|
||||
// Match C++ implementation in cost_helpers_impl.h
|
||||
// startPose = [x1, y1, theta1]
|
||||
// endPose = [x2, y2, theta2]
|
||||
// observedRelativePose = relative pose from start to end (in start frame)
|
||||
|
||||
var cosThetaI = Math.Cos(startPose[2]);
|
||||
var sinThetaI = Math.Sin(startPose[2]);
|
||||
var deltaX = endPose[0] - startPose[0];
|
||||
var deltaY = endPose[1] - startPose[1];
|
||||
|
||||
// Compute h = relative pose from start to end (in start frame)
|
||||
// h[0] = cos_theta_i * delta_x + sin_theta_i * delta_y
|
||||
// h[1] = -sin_theta_i * delta_x + cos_theta_i * delta_y
|
||||
// h[2] = end[2] - start[2]
|
||||
var h0 = cosThetaI * deltaX + sinThetaI * deltaY;
|
||||
var h1 = -sinThetaI * deltaX + cosThetaI * deltaY;
|
||||
var h2 = endPose[2] - startPose[2];
|
||||
|
||||
// Error = observed - computed
|
||||
var translationErrorX = observedRelativePose.Translation.X - h0;
|
||||
var translationErrorY = observedRelativePose.Translation.Y - h1;
|
||||
|
||||
// Rotation error (normalize angle difference)
|
||||
var rotationError = OptimizationHelpers.NormalizeAngleDifference(
|
||||
observedRelativePose.Rotation - h2
|
||||
);
|
||||
|
||||
return
|
||||
[
|
||||
translationErrorX,
|
||||
translationErrorY,
|
||||
rotationError
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scales error with translation and rotation weights.
|
||||
/// </summary>
|
||||
private static double[] ScaleError(
|
||||
double[] unscaledError,
|
||||
double translationWeight,
|
||||
double rotationWeight)
|
||||
{
|
||||
return
|
||||
[
|
||||
translationWeight * unscaledError[0],
|
||||
translationWeight * unscaledError[1],
|
||||
rotationWeight * unscaledError[2]
|
||||
];
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user