saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Allow AngleAxisRotatePoint to be applied in-place

Browse Source 
Thanks to @CatInTheRain for the suggestion.

Fixes #1163

Change-Id: I4e148c62891979e1ec01b91f95d3e2d04501c8ef
This commit is contained in:
Sergiu Deitsch
2025-06-10 23:14:28 +02:00
parent 9c22db814c
commit 4148d6a353
2 changed files with 36 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/rotation.h
+3 -3
View File
@@ -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 <typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3],
const T pt[3],
@@ -791,9 +790,10 @@ template <typename T>
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]);
internal/ceres/rotation_test.cc
+33 -25
View File
@@ -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
}
}
}
}