/// <summary>
/// Add a linear vector in Lie space to the the pose around its pose manifold.
/// It uses global coordinates, where translation and rotation are independent.
/// </summary>
/// <param name="delta">Odometry delta.</param>
public Pose3D AddGlobal(double[] delta)
{
Quaternion neworientation = Orientation.Add(new double[3] {
delta[3], delta[4], delta[5]
});
double[] location = new double[3] {
X + delta[0], Y + delta[1], Z + delta[2]
};
return(new Pose3D(location, neworientation.Normalize()));
}
/// <summary>
/// Add a linear vector in semi-Lie space to the the pose around its pose manifold.
/// </summary>
/// <param name="delta">Odometry delta.</param>
public Pose3D Add(double[] delta)
{
Quaternion neworientation = Orientation.Add(new double[3] {
delta[3], delta[4], delta[5]
});
Quaternion dlocation = Orientation * new Quaternion(0, delta[0], delta[1], delta[2]) * Orientation.Conjugate();
double[] location = new double[3] {
X + dlocation.X, Y + dlocation.Y, Z + dlocation.Z
};
Quaternion orientation = neworientation.Normalize();
return(new Pose3D(location, orientation));
}
