diff options
Diffstat (limited to 'extern/ceres/include/ceres/rotation.h')
| -rw-r--r-- | extern/ceres/include/ceres/rotation.h | 30 |
1 files changed, 15 insertions, 15 deletions
diff --git a/extern/ceres/include/ceres/rotation.h b/extern/ceres/include/ceres/rotation.h index 0c82a417a2c..51079901aaf 100644 --- a/extern/ceres/include/ceres/rotation.h +++ b/extern/ceres/include/ceres/rotation.h @@ -521,18 +521,18 @@ inline void UnitQuaternionRotatePoint(const T q[4], DCHECK_NE(pt, result) << "Inplace rotation is not supported."; // clang-format off - const T t2 = q[0] * q[1]; - const T t3 = q[0] * q[2]; - const T t4 = q[0] * q[3]; - const T t5 = -q[1] * q[1]; - const T t6 = q[1] * q[2]; - const T t7 = q[1] * q[3]; - const T t8 = -q[2] * q[2]; - const T t9 = q[2] * q[3]; - const T t1 = -q[3] * q[3]; - result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT - result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT - result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT + T uv0 = q[2] * pt[2] - q[3] * pt[1]; + T uv1 = q[3] * pt[0] - q[1] * pt[2]; + T uv2 = q[1] * pt[1] - q[2] * pt[0]; + uv0 += uv0; + uv1 += uv1; + uv2 += uv2; + result[0] = pt[0] + q[0] * uv0; + result[1] = pt[1] + q[0] * uv1; + result[2] = pt[2] + q[0] * uv2; + result[0] += q[2] * uv2 - q[3] * uv1; + result[1] += q[3] * uv0 - q[1] * uv2; + result[2] += q[1] * uv1 - q[2] * uv0; // clang-format on } @@ -624,16 +624,16 @@ inline void AngleAxisRotatePoint(const T angle_axis[3], result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; } else { // Near zero, the first order Taylor approximation of the rotation - // matrix R corresponding to a vector w and angle w is + // matrix R corresponding to a vector w and angle theta is // // R = I + hat(w) * sin(theta) // // But sintheta ~ theta and theta * w = angle_axis, which gives us // - // R = I + hat(w) + // R = I + hat(angle_axis) // // and actually performing multiplication with the point pt, gives us - // R * pt = pt + w x pt. + // R * pt = pt + angle_axis x pt. // // Switching to the Taylor expansion near zero provides meaningful // derivatives when evaluated using Jets. |