Skip to content

Commit f39b657

Browse files
author
peng.li24
committed
fix: store quaternion-derived matrix (not raw input) to match scipy pipeline exactly
scipy internally: from_matrix → quat → normalize → quat_to_matrix → euler. We must store the quaternion-derived matrix (not the raw input) so as_euler/as_matrix produce bit-identical results to scipy. Also compute and store quat-derived matrix in from_euler for consistency.
1 parent c41abd5 commit f39b657

1 file changed

Lines changed: 61 additions & 3 deletions

File tree

scipy/transform.h

Lines changed: 61 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -96,10 +96,26 @@ struct Rotation {
9696
rot.quat[2] = q[2] / norm;
9797
rot.quat[3] = q[3] / norm;
9898

99-
// Store original matrix to avoid quaternion roundtrip ULP-error
100-
// in as_euler() and as_matrix().
99+
// Store quaternion-derived matrix to exactly match scipy's pipeline.
100+
// scipy: from_matrix → quat → normalize → quat_to_matrix → euler
101+
// Quat normalization can change matrix values by 1 ULP; scipy uses
102+
// the quaternion-derived matrix for as_euler(), not the raw input.
103+
// We MUST store the same quaternion-derived matrix for 0-bit match.
101104
rot.has_matrix = true;
102-
for (int i = 0; i < 9; ++i) rot.matrix[i] = matrix[i];
105+
{
106+
T x = rot.quat[0], y = rot.quat[1], z = rot.quat[2], w = rot.quat[3];
107+
T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
108+
T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
109+
rot.matrix[0] = x2 - y2 - z2 + w2;
110+
rot.matrix[1] = T(2) * (xy - zw);
111+
rot.matrix[2] = T(2) * (xz + yw);
112+
rot.matrix[3] = T(2) * (xy + zw);
113+
rot.matrix[4] = -x2 + y2 - z2 + w2;
114+
rot.matrix[5] = T(2) * (yz - xw);
115+
rot.matrix[6] = T(2) * (xz - yw);
116+
rot.matrix[7] = T(2) * (yz + xw);
117+
rot.matrix[8] = -x2 - y2 + z2 + w2;
118+
}
103119

104120
return rot;
105121
}
@@ -339,6 +355,28 @@ struct Rotation {
339355
throw std::invalid_argument(
340356
"Rotation::from_euler: unsupported single-axis seq '" + s + "'");
341357
}
358+
359+
// Compute and store the quaternion-derived matrix so as_euler()
360+
// and as_matrix() use the same values as scipy's quat→matrix path.
361+
// Single-axis quaternion: no composition, no normalization needed
362+
// (norm = sh²+ch² = sin²(θ/2)+cos²(θ/2) = 1 exactly in ℝ,
363+
// but floating-point rounding may give norm ≈ 1 ± eps).
364+
// Using quat→matrix formulas matches scipy's exact path.
365+
rot.has_matrix = true;
366+
{
367+
T x = rot.quat[0], y = rot.quat[1], z = rot.quat[2], w = rot.quat[3];
368+
T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
369+
T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
370+
rot.matrix[0] = x2 - y2 - z2 + w2;
371+
rot.matrix[1] = T(2) * (xy - zw);
372+
rot.matrix[2] = T(2) * (xz + yw);
373+
rot.matrix[3] = T(2) * (xy + zw);
374+
rot.matrix[4] = -x2 + y2 - z2 + w2;
375+
rot.matrix[5] = T(2) * (yz - xw);
376+
rot.matrix[6] = T(2) * (xz - yw);
377+
rot.matrix[7] = T(2) * (yz + xw);
378+
rot.matrix[8] = -x2 - y2 + z2 + w2;
379+
}
342380
return rot;
343381
}
344382

@@ -390,6 +428,26 @@ struct Rotation {
390428
result.quat[2] = rw*qi[2] + rz*qi[3] + rx*qi[1] - ry*qi[0];
391429
result.quat[3] = rw*qi[3] - rx*qi[0] - ry*qi[1] - rz*qi[2];
392430
}
431+
432+
// Compute and store the quaternion-derived matrix so as_euler()
433+
// and as_matrix() use the same values as scipy's quat→matrix path.
434+
// This ensures 0-ULP match: scipy always reads the matrix from quaternion
435+
// after composing, never stores a raw matrix.
436+
result.has_matrix = true;
437+
{
438+
T x = result.quat[0], y = result.quat[1], z = result.quat[2], w = result.quat[3];
439+
T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
440+
T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
441+
result.matrix[0] = x2 - y2 - z2 + w2;
442+
result.matrix[1] = T(2) * (xy - zw);
443+
result.matrix[2] = T(2) * (xz + yw);
444+
result.matrix[3] = T(2) * (xy + zw);
445+
result.matrix[4] = -x2 + y2 - z2 + w2;
446+
result.matrix[5] = T(2) * (yz - xw);
447+
result.matrix[6] = T(2) * (xz - yw);
448+
result.matrix[7] = T(2) * (yz + xw);
449+
result.matrix[8] = -x2 - y2 + z2 + w2;
450+
}
393451
return result;
394452
}
395453
};

0 commit comments

Comments
 (0)