diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index 261d3a0f3..c04278e35 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.h @@ -282,8 +282,7 @@ inline T DotProduct(const T x[3], const T y[3]); // y = R(angle_axis) * x; // -// Inplace rotation is not supported. pt and result must point to different -// memory locations, otherwise the result will be undefined. +// Inplace rotation is supported. template inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], @@ -791,9 +790,10 @@ template inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { - DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + using std::cos; using std::fpclassify; using std::hypot; + using std::sin; const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]); diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index 0dfcf7136..85c8b78e2 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2023 Google Inc. All rights reserved. +// Copyright 2025 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -1732,18 +1732,22 @@ TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; - AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); - for (int k = 0; k < 3; ++k) { - // clang-format off - EXPECT_NEAR(rotation_matrix_rotated_p[k], - angle_axis_rotated_p[k], - kTolerance) << "p: " << p[0] - << " " << p[1] - << " " << p[2] - << " angle_axis: " << angle_axis[0] - << " " << angle_axis[1] - << " " << angle_axis[2]; - // clang-format on + // Rotate point and write the result to a different and the same + // destination + for (double* const dst : {angle_axis_rotated_p, angle_axis}) { + AngleAxisRotatePoint(angle_axis, p, dst); + for (int k = 0; k < 3; ++k) { + // clang-format off + EXPECT_NEAR(rotation_matrix_rotated_p[k], + dst[k], + kTolerance) << "p: " << p[0] + << " " << p[1] + << " " << p[2] + << " angle_axis: " << angle_axis[0] + << " " << angle_axis[1] + << " " << angle_axis[2]; + // clang-format on + } } } } @@ -1790,18 +1794,22 @@ TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) { rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; - AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); - for (int k = 0; k < 3; ++k) { - // clang-format off - EXPECT_NEAR(rotation_matrix_rotated_p[k], - angle_axis_rotated_p[k], - kTolerance) << "p: " << p[0] - << " " << p[1] - << " " << p[2] - << " angle_axis: " << angle_axis[0] - << " " << angle_axis[1] - << " " << angle_axis[2]; - // clang-format on + // Rotate point and write the result to a different and the same + // destination + for (double* const dst : {angle_axis_rotated_p, angle_axis}) { + AngleAxisRotatePoint(angle_axis, p, dst); + for (int k = 0; k < 3; ++k) { + // clang-format off + EXPECT_NEAR(rotation_matrix_rotated_p[k], + dst[k], + kTolerance) << "p: " << p[0] + << " " << p[1] + << " " << p[2] + << " angle_axis: " << angle_axis[0] + << " " << angle_axis[1] + << " " << angle_axis[2]; + // clang-format on + } } } }