Initial commit
This commit is contained in:
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Penalizes differences between IMU data and optimized accelerations.
|
||||
/// Based on acceleration_cost_function_3d.h
|
||||
/// </summary>
|
||||
public class AccelerationCostFunction3D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly Vector3 _deltaVelocityImuFrame;
|
||||
private readonly double _firstDeltaTimeSeconds;
|
||||
private readonly double _secondDeltaTimeSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for acceleration constraint.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Scaling factor for the cost.</param>
|
||||
/// <param name="deltaVelocityImuFrame">Delta velocity from IMU integration in IMU frame.</param>
|
||||
/// <param name="firstDeltaTimeSeconds">Time duration of first interval in seconds.</param>
|
||||
/// <param name="secondDeltaTimeSeconds">Time duration of second interval in seconds.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Vector3 deltaVelocityImuFrame,
|
||||
double firstDeltaTimeSeconds,
|
||||
double secondDeltaTimeSeconds)
|
||||
{
|
||||
var costFunction = new AccelerationCostFunction3D(
|
||||
scalingFactor,
|
||||
deltaVelocityImuFrame,
|
||||
firstDeltaTimeSeconds,
|
||||
secondDeltaTimeSeconds);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 3, // [dx, dy, dz] - velocity difference error
|
||||
parameterBlockSizes: [4, 3, 3, 3, 1, 4] // [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]]
|
||||
);
|
||||
}
|
||||
|
||||
private AccelerationCostFunction3D(
|
||||
double scalingFactor,
|
||||
Vector3 deltaVelocityImuFrame,
|
||||
double firstDeltaTimeSeconds,
|
||||
double secondDeltaTimeSeconds)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_deltaVelocityImuFrame = deltaVelocityImuFrame;
|
||||
_firstDeltaTimeSeconds = firstDeltaTimeSeconds;
|
||||
_secondDeltaTimeSeconds = secondDeltaTimeSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz] (velocity difference error).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
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 < 3 ||
|
||||
parameters[3].Length < 3 || parameters[4].Length < 1 || parameters[5].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var middleRotation = parameters[0]; // [w, x, y, z]
|
||||
var startPosition = parameters[1]; // [x, y, z]
|
||||
var middlePosition = parameters[2]; // [x, y, z]
|
||||
var endPosition = parameters[3]; // [x, y, z]
|
||||
var gravityConstant = parameters[4][0]; // [g]
|
||||
var imuCalibration = parameters[5]; // [w, x, y, z]
|
||||
|
||||
// Convert to quaternions
|
||||
var middleRot = new Quaternion(
|
||||
middleRotation[1], middleRotation[2], middleRotation[3], middleRotation[0]);
|
||||
var imuCal = new Quaternion(
|
||||
imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
|
||||
|
||||
// Convert positions to Vector3
|
||||
var startPos = new Vector3(startPosition[0], startPosition[1], startPosition[2]);
|
||||
var middlePos = new Vector3(middlePosition[0], middlePosition[1], middlePosition[2]);
|
||||
var endPos = new Vector3(endPosition[0], endPosition[1], endPosition[2]);
|
||||
|
||||
// Compute IMU delta velocity in map frame
|
||||
// Formula from C++:
|
||||
// imu_delta_velocity = middle_rotation * imu_calibration * delta_velocity_imu_frame - gravity_term
|
||||
// where gravity_term = gravity_constant * 0.5 * (first_delta_time + second_delta_time) * UnitZ
|
||||
|
||||
// Transform delta_velocity_imu_frame from IMU frame to map frame
|
||||
// In Eigen: quaternion * vector rotates the vector
|
||||
// In System.Numerics: Vector3.Transform(vector, quaternion) rotates the vector
|
||||
// C++: middle_rotation * imu_calibration * delta_velocity
|
||||
// = middle_rotation * (imu_calibration * delta_velocity)
|
||||
// Apply IMU calibration first, then middle rotation
|
||||
var imuDeltaVelocityCalibrated = Vector3.Transform(_deltaVelocityImuFrame, imuCal);
|
||||
var imuDeltaVelocityInMapFrame = Vector3.Transform(imuDeltaVelocityCalibrated, middleRot);
|
||||
|
||||
// Subtract gravity contribution
|
||||
// Gravity acts in positive Z direction in map frame (upward)
|
||||
var gravityTerm = gravityConstant * 0.5 * (_firstDeltaTimeSeconds + _secondDeltaTimeSeconds) * Vector3.UnitZ;
|
||||
var imuDeltaVelocity = imuDeltaVelocityInMapFrame - gravityTerm;
|
||||
|
||||
// Compute velocities from positions
|
||||
// start_velocity = (middle_position - start_position) / first_delta_time
|
||||
var startVelocity = (middlePos - startPos) / _firstDeltaTimeSeconds;
|
||||
|
||||
// end_velocity = (end_position - middle_position) / second_delta_time
|
||||
var endVelocity = (endPos - middlePos) / _secondDeltaTimeSeconds;
|
||||
|
||||
// delta_velocity = end_velocity - start_velocity
|
||||
var deltaVelocity = endVelocity - startVelocity;
|
||||
|
||||
// Error = IMU delta velocity - computed delta velocity
|
||||
var error = imuDeltaVelocity - deltaVelocity;
|
||||
|
||||
// Scale error
|
||||
residuals[0] = _scalingFactor * error.X;
|
||||
residuals[1] = _scalingFactor * error.Y;
|
||||
residuals[2] = _scalingFactor * error.Z;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Penalizes differences between IMU data and optimized orientations.
|
||||
/// Based on rotation_cost_function_3d.h
|
||||
/// </summary>
|
||||
public class RotationCostFunction3D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly Quaternion _deltaRotationImuFrame;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for rotation constraint.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Scaling factor for the cost.</param>
|
||||
/// <param name="deltaRotationImuFrame">Delta rotation from IMU integration in IMU frame.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Quaternion deltaRotationImuFrame)
|
||||
{
|
||||
var costFunction = new RotationCostFunction3D(scalingFactor, deltaRotationImuFrame);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 3, // [dx, dy, dz] - rotation error as angle-axis vector
|
||||
parameterBlockSizes: [4, 4, 4] // [start_rotation[4], end_rotation[4], imu_calibration[4]]
|
||||
);
|
||||
}
|
||||
|
||||
private RotationCostFunction3D(double scalingFactor, Quaternion deltaRotationImuFrame)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_deltaRotationImuFrame = deltaRotationImuFrame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [start_rotation[4], end_rotation[4], imu_calibration[4]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz] (angle-axis error).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 3)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 4 || parameters[2].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var startRotation = parameters[0]; // [w, x, y, z] from Ceres
|
||||
var endRotation = parameters[1]; // [w, x, y, z] from Ceres
|
||||
var imuCalibration = parameters[2]; // [w, x, y, z] from Ceres
|
||||
|
||||
// Convert to quaternions
|
||||
// C++ line 42-48: Eigen::Quaternion<T>(w, x, y, z)
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
// So we need to convert [w, x, y, z] to (x, y, z, w)
|
||||
var start = new Quaternion(
|
||||
startRotation[1], startRotation[2], startRotation[3], startRotation[0]);
|
||||
var end = new Quaternion(
|
||||
endRotation[1], endRotation[2], endRotation[3], endRotation[0]);
|
||||
var imuCal = new Quaternion(
|
||||
imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
|
||||
|
||||
// Compute error: end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
|
||||
// C++ line 49-51: error = end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
|
||||
// C++ line 52-54: residual = scaling_factor * error.vector() (x, y, z components of quaternion, not angle-axis)
|
||||
var endConj = Quaternion.Conjugate(end);
|
||||
var imuCalConj = Quaternion.Conjugate(imuCal);
|
||||
var error = Quaternion.Multiply(
|
||||
Quaternion.Multiply(
|
||||
Quaternion.Multiply(
|
||||
Quaternion.Multiply(endConj, start),
|
||||
imuCal),
|
||||
_deltaRotationImuFrame),
|
||||
imuCalConj);
|
||||
|
||||
// C++ uses error.x(), error.y(), error.z() which are the vector (imaginary) parts of the quaternion
|
||||
// NOT angle-axis representation. For small rotations, these are approximately the same, but we should match C++ exactly.
|
||||
// Scale error using vector part of quaternion (x, y, z components)
|
||||
residuals[0] = _scalingFactor * error.X;
|
||||
residuals[1] = _scalingFactor * error.Y;
|
||||
residuals[2] = _scalingFactor * error.Z;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,190 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Sparse Pose Adjustment (SPA) cost function for 3D pose graph optimization.
|
||||
/// Computes the error between observed relative pose and computed relative pose.
|
||||
/// </summary>
|
||||
public class SpaCostFunction3D
|
||||
{
|
||||
private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for SPA 3D.
|
||||
/// </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 SpaCostFunction3D(observedRelativePose);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz] (quaternion rotation error as 3D vector)
|
||||
parameterBlockSizes: [4, 3, 4, 3] // [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]]
|
||||
);
|
||||
}
|
||||
|
||||
private SpaCostFunction3D(IPoseGraph.Constraint.Pose observedRelativePose)
|
||||
{
|
||||
_observedRelativePose = observedRelativePose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz, dqx, dqy, dqz].</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 4)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 3 ||
|
||||
parameters[2].Length < 4 || parameters[3].Length < 3)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 6)
|
||||
return false;
|
||||
|
||||
var submapRotation = parameters[0];
|
||||
var submapTranslation = parameters[1];
|
||||
var nodeRotation = parameters[2];
|
||||
var nodeTranslation = parameters[3];
|
||||
|
||||
// Compute unscaled error
|
||||
var unscaledError = ComputeUnscaledError(
|
||||
_observedRelativePose.ZbarIj,
|
||||
submapRotation,
|
||||
submapTranslation,
|
||||
nodeRotation,
|
||||
nodeTranslation
|
||||
);
|
||||
|
||||
// Scale error with weights
|
||||
var scaledError = ScaleError(
|
||||
unscaledError,
|
||||
_observedRelativePose.TranslationWeight,
|
||||
_observedRelativePose.RotationWeight
|
||||
);
|
||||
|
||||
residuals[0] = scaledError[0];
|
||||
residuals[1] = scaledError[1];
|
||||
residuals[2] = scaledError[2];
|
||||
residuals[3] = scaledError[3];
|
||||
residuals[4] = scaledError[4];
|
||||
residuals[5] = scaledError[5];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes unscaled error between observed and computed relative pose.
|
||||
/// Based on cost_helpers_impl.h ComputeUnscaledError for 3D.
|
||||
/// </summary>
|
||||
private static double[] ComputeUnscaledError(
|
||||
Rigid3d observedRelativePose,
|
||||
double[] submapRotation,
|
||||
double[] submapTranslation,
|
||||
double[] nodeRotation,
|
||||
double[] nodeTranslation)
|
||||
{
|
||||
// submapRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
|
||||
// submapTranslation = [x, y, z]
|
||||
// nodeRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
|
||||
// nodeTranslation = [x, y, z]
|
||||
|
||||
// IMPORTANT: System.Numerics.Quaternion constructor is (x, y, z, w), NOT (w, x, y, z)!
|
||||
// Eigen::Quaternion uses (w, x, y, z), so we must reorder when creating System.Numerics.Quaternion.
|
||||
|
||||
// Compute R_i_inverse (inverse of submap rotation)
|
||||
// C++: Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1], -start_rotation[2], -start_rotation[3])
|
||||
var submapQuatInv = new Quaternion(
|
||||
-submapRotation[1], // -x
|
||||
-submapRotation[2], // -y
|
||||
-submapRotation[3], // -z
|
||||
submapRotation[0] // w
|
||||
);
|
||||
|
||||
// Compute delta = node_translation - submap_translation
|
||||
var delta = new Vector3(
|
||||
(nodeTranslation[0] - submapTranslation[0]),
|
||||
(nodeTranslation[1] - submapTranslation[1]),
|
||||
(nodeTranslation[2] - submapTranslation[2])
|
||||
);
|
||||
|
||||
// h_translation = R_i_inverse * delta
|
||||
var hTranslation = Vector3.Transform(delta, submapQuatInv);
|
||||
|
||||
// Compute h_rotation_inverse = node_rotation_inverse * submap_rotation
|
||||
// C++: Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2], -end_rotation[3]) *
|
||||
// Eigen::Quaternion<T>(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3])
|
||||
var nodeQuatInv = new Quaternion(
|
||||
-nodeRotation[1], // -x
|
||||
-nodeRotation[2], // -y
|
||||
-nodeRotation[3], // -z
|
||||
nodeRotation[0] // w
|
||||
);
|
||||
var submapQuat = new Quaternion(
|
||||
submapRotation[1], // x
|
||||
submapRotation[2], // y
|
||||
submapRotation[3], // z
|
||||
submapRotation[0] // w
|
||||
);
|
||||
var hRotationInverse = nodeQuatInv * submapQuat;
|
||||
|
||||
// Compute angle-axis difference: RotationQuaternionToAngleAxisVector(h_rotation_inverse * observed_rotation)
|
||||
var observedQuat = observedRelativePose.Rotation;
|
||||
var angleAxisDifference = TransformOperations.RotationQuaternionToAngleAxisVector(
|
||||
hRotationInverse * observedQuat
|
||||
);
|
||||
|
||||
// Error = observed - computed
|
||||
return
|
||||
[
|
||||
observedRelativePose.Translation.X - hTranslation.X,
|
||||
observedRelativePose.Translation.Y - hTranslation.Y,
|
||||
observedRelativePose.Translation.Z - hTranslation.Z,
|
||||
angleAxisDifference.X,
|
||||
angleAxisDifference.Y,
|
||||
angleAxisDifference.Z
|
||||
];
|
||||
}
|
||||
|
||||
/// <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],
|
||||
translationWeight * unscaledError[2],
|
||||
rotationWeight * unscaledError[3],
|
||||
rotationWeight * unscaledError[4],
|
||||
rotationWeight * unscaledError[5]
|
||||
];
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user