mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-19 13:18:32 +00:00
h/mercury: Split lm_rotations into two files to make copyright ownership clear
This commit is contained in:
Moshi Turner
parent
7485afbeaa
commit
4ccc133ac5
|
|
@ -1,39 +1,12 @@
|
||||||
// Copyright 2022, Google, Inc.
|
|
||||||
// Copyright 2022, Collabora, Ltd.
|
// Copyright 2022, Collabora, Ltd.
|
||||||
// SPDX-License-Identifier: BSD-3-Clause
|
// SPDX-License-Identifier: BSL-1.0
|
||||||
/*!
|
/*!
|
||||||
* @file
|
* @file
|
||||||
* @brief Autodiff-safe rotations for Levenberg-Marquardt kinematic optimizer.
|
* @brief Autodiff-safe rotations for Levenberg-Marquardt kinematic optimizer.
|
||||||
* Copied out of Ceres's `rotation.h` with some modifications.
|
|
||||||
* @author Kier Mierle <kier@google.com>
|
|
||||||
* @author Sameer Agarwal <sameeragarwal@google.com>
|
|
||||||
* @author Moses Turner <moses@collabora.com>
|
* @author Moses Turner <moses@collabora.com>
|
||||||
* @ingroup tracking
|
* @ingroup tracking
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Redistribution and use in source and binary forms, with or without
|
|
||||||
// modification, are permitted provided that the following conditions are met:
|
|
||||||
//
|
|
||||||
// * Redistributions of source code must retain the above copyright notice,
|
|
||||||
// this list of conditions and the following disclaimer.
|
|
||||||
// * Redistributions in binary form must reproduce the above copyright notice,
|
|
||||||
// this list of conditions and the following disclaimer in the documentation
|
|
||||||
// and/or other materials provided with the distribution.
|
|
||||||
// * Neither the name of Google Inc. nor the names of its contributors may be
|
|
||||||
// used to endorse or promote products derived from this software without
|
|
||||||
// specific prior written permission.
|
|
||||||
//
|
|
||||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
||||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
||||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
||||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
||||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
||||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
||||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
||||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
||||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
||||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
// POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
|
@ -68,102 +41,7 @@ namespace xrt::tracking::hand::mercury::lm {
|
||||||
#define assert_quat_length_1(q)
|
#define assert_quat_length_1(q)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#include "lm_rotations_ceres.inl"
|
||||||
template <typename T>
|
|
||||||
inline void
|
|
||||||
QuaternionProduct(const Quat<T> &z, const Quat<T> &w, Quat<T> &zw)
|
|
||||||
{
|
|
||||||
// Inplace product is not supported
|
|
||||||
assert(&z != &zw);
|
|
||||||
assert(&w != &zw);
|
|
||||||
|
|
||||||
assert_quat_length_1(z);
|
|
||||||
assert_quat_length_1(w);
|
|
||||||
|
|
||||||
|
|
||||||
// clang-format off
|
|
||||||
zw.w = z.w * w.w - z.x * w.x - z.y * w.y - z.z * w.z;
|
|
||||||
zw.x = z.w * w.x + z.x * w.w + z.y * w.z - z.z * w.y;
|
|
||||||
zw.y = z.w * w.y - z.x * w.z + z.y * w.w + z.z * w.x;
|
|
||||||
zw.z = z.w * w.z + z.x * w.y - z.y * w.x + z.z * w.w;
|
|
||||||
// clang-format on
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template <typename T>
|
|
||||||
inline void
|
|
||||||
UnitQuaternionRotatePoint(const Quat<T> &q, const Vec3<T> &pt, Vec3<T> &result)
|
|
||||||
{
|
|
||||||
// clang-format off
|
|
||||||
T uv0 = q.y * pt.z - q.z * pt.y;
|
|
||||||
T uv1 = q.z * pt.x - q.x * pt.z;
|
|
||||||
T uv2 = q.x * pt.y - q.y * pt.x;
|
|
||||||
uv0 += uv0;
|
|
||||||
uv1 += uv1;
|
|
||||||
uv2 += uv2;
|
|
||||||
result.x = pt.x + q.w * uv0;
|
|
||||||
result.y = pt.y + q.w * uv1;
|
|
||||||
result.z = pt.z + q.w * uv2;
|
|
||||||
result.x += q.y * uv2 - q.z * uv1;
|
|
||||||
result.y += q.z * uv0 - q.x * uv2;
|
|
||||||
result.z += q.x * uv1 - q.y * uv0;
|
|
||||||
// clang-format on
|
|
||||||
}
|
|
||||||
|
|
||||||
template <typename T>
|
|
||||||
inline void
|
|
||||||
UnitQuaternionRotateAndScalePoint(const Quat<T> &q, const Vec3<T> &pt, const T scale, Vec3<T> &result)
|
|
||||||
{
|
|
||||||
T uv0 = q.y * pt.z - q.z * pt.y;
|
|
||||||
T uv1 = q.z * pt.x - q.x * pt.z;
|
|
||||||
T uv2 = q.x * pt.y - q.y * pt.x;
|
|
||||||
uv0 += uv0;
|
|
||||||
uv1 += uv1;
|
|
||||||
uv2 += uv2;
|
|
||||||
result.x = pt.x + q.w * uv0;
|
|
||||||
result.y = pt.y + q.w * uv1;
|
|
||||||
result.z = pt.z + q.w * uv2;
|
|
||||||
result.x += q.y * uv2 - q.z * uv1;
|
|
||||||
result.y += q.z * uv0 - q.x * uv2;
|
|
||||||
result.z += q.x * uv1 - q.y * uv0;
|
|
||||||
|
|
||||||
result.x *= scale;
|
|
||||||
result.y *= scale;
|
|
||||||
result.z *= scale;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template <typename T>
|
|
||||||
inline void
|
|
||||||
AngleAxisToQuaternion(const Vec3<T> angle_axis, Quat<T> &result)
|
|
||||||
{
|
|
||||||
const T &a0 = angle_axis.x;
|
|
||||||
const T &a1 = angle_axis.y;
|
|
||||||
const T &a2 = angle_axis.z;
|
|
||||||
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
|
|
||||||
|
|
||||||
// For points not at the origin, the full conversion is numerically stable.
|
|
||||||
if (likely(theta_squared > T(0.0))) {
|
|
||||||
const T theta = sqrt(theta_squared);
|
|
||||||
const T half_theta = theta * T(0.5);
|
|
||||||
const T k = sin(half_theta) / theta;
|
|
||||||
result.w = cos(half_theta);
|
|
||||||
result.x = a0 * k;
|
|
||||||
result.y = a1 * k;
|
|
||||||
result.z = a2 * k;
|
|
||||||
} else {
|
|
||||||
// At the origin, sqrt() will produce NaN in the derivative since
|
|
||||||
// the argument is zero. By approximating with a Taylor series,
|
|
||||||
// and truncating at one term, the value and first derivatives will be
|
|
||||||
// computed correctly when Jets are used.
|
|
||||||
const T k(0.5);
|
|
||||||
result.w = T(1.0);
|
|
||||||
result.x = a0 * k;
|
|
||||||
result.y = a1 * k;
|
|
||||||
result.z = a2 * k;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
|
|
|
||||||
133
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations_ceres.inl
Normal file
133
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations_ceres.inl
Normal file
|
|
@ -0,0 +1,133 @@
|
||||||
|
// Copyright 2022, Google, Inc.
|
||||||
|
// Copyright 2022, Collabora, Ltd.
|
||||||
|
// SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
/*!
|
||||||
|
* @file
|
||||||
|
* @brief Autodiff-safe rotations for Levenberg-Marquardt kinematic optimizer.
|
||||||
|
* Copied out of Ceres's `rotation.h` with some modifications.
|
||||||
|
* @author Kier Mierle <kier@google.com>
|
||||||
|
* @author Sameer Agarwal <sameeragarwal@google.com>
|
||||||
|
* @author Moses Turner <moses@collabora.com>
|
||||||
|
* @ingroup tracking
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Redistribution and use in source and binary forms, with or without
|
||||||
|
// modification, are permitted provided that the following conditions are met:
|
||||||
|
//
|
||||||
|
// * Redistributions of source code must retain the above copyright notice,
|
||||||
|
// this list of conditions and the following disclaimer.
|
||||||
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
// this list of conditions and the following disclaimer in the documentation
|
||||||
|
// and/or other materials provided with the distribution.
|
||||||
|
// * Neither the name of Google Inc. nor the names of its contributors may be
|
||||||
|
// used to endorse or promote products derived from this software without
|
||||||
|
// specific prior written permission.
|
||||||
|
//
|
||||||
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||||
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||||
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||||
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||||
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||||
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||||
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
// POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void
|
||||||
|
QuaternionProduct(const Quat<T> &z, const Quat<T> &w, Quat<T> &zw)
|
||||||
|
{
|
||||||
|
// Inplace product is not supported
|
||||||
|
assert(&z != &zw);
|
||||||
|
assert(&w != &zw);
|
||||||
|
|
||||||
|
assert_quat_length_1(z);
|
||||||
|
assert_quat_length_1(w);
|
||||||
|
|
||||||
|
|
||||||
|
// clang-format off
|
||||||
|
zw.w = z.w * w.w - z.x * w.x - z.y * w.y - z.z * w.z;
|
||||||
|
zw.x = z.w * w.x + z.x * w.w + z.y * w.z - z.z * w.y;
|
||||||
|
zw.y = z.w * w.y - z.x * w.z + z.y * w.w + z.z * w.x;
|
||||||
|
zw.z = z.w * w.z + z.x * w.y - z.y * w.x + z.z * w.w;
|
||||||
|
// clang-format on
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void
|
||||||
|
UnitQuaternionRotatePoint(const Quat<T> &q, const Vec3<T> &pt, Vec3<T> &result)
|
||||||
|
{
|
||||||
|
// clang-format off
|
||||||
|
T uv0 = q.y * pt.z - q.z * pt.y;
|
||||||
|
T uv1 = q.z * pt.x - q.x * pt.z;
|
||||||
|
T uv2 = q.x * pt.y - q.y * pt.x;
|
||||||
|
uv0 += uv0;
|
||||||
|
uv1 += uv1;
|
||||||
|
uv2 += uv2;
|
||||||
|
result.x = pt.x + q.w * uv0;
|
||||||
|
result.y = pt.y + q.w * uv1;
|
||||||
|
result.z = pt.z + q.w * uv2;
|
||||||
|
result.x += q.y * uv2 - q.z * uv1;
|
||||||
|
result.y += q.z * uv0 - q.x * uv2;
|
||||||
|
result.z += q.x * uv1 - q.y * uv0;
|
||||||
|
// clang-format on
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void
|
||||||
|
UnitQuaternionRotateAndScalePoint(const Quat<T> &q, const Vec3<T> &pt, const T scale, Vec3<T> &result)
|
||||||
|
{
|
||||||
|
T uv0 = q.y * pt.z - q.z * pt.y;
|
||||||
|
T uv1 = q.z * pt.x - q.x * pt.z;
|
||||||
|
T uv2 = q.x * pt.y - q.y * pt.x;
|
||||||
|
uv0 += uv0;
|
||||||
|
uv1 += uv1;
|
||||||
|
uv2 += uv2;
|
||||||
|
result.x = pt.x + q.w * uv0;
|
||||||
|
result.y = pt.y + q.w * uv1;
|
||||||
|
result.z = pt.z + q.w * uv2;
|
||||||
|
result.x += q.y * uv2 - q.z * uv1;
|
||||||
|
result.y += q.z * uv0 - q.x * uv2;
|
||||||
|
result.z += q.x * uv1 - q.y * uv0;
|
||||||
|
|
||||||
|
result.x *= scale;
|
||||||
|
result.y *= scale;
|
||||||
|
result.z *= scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void
|
||||||
|
AngleAxisToQuaternion(const Vec3<T> angle_axis, Quat<T> &result)
|
||||||
|
{
|
||||||
|
const T &a0 = angle_axis.x;
|
||||||
|
const T &a1 = angle_axis.y;
|
||||||
|
const T &a2 = angle_axis.z;
|
||||||
|
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
|
||||||
|
|
||||||
|
// For points not at the origin, the full conversion is numerically stable.
|
||||||
|
if (likely(theta_squared > T(0.0))) {
|
||||||
|
const T theta = sqrt(theta_squared);
|
||||||
|
const T half_theta = theta * T(0.5);
|
||||||
|
const T k = sin(half_theta) / theta;
|
||||||
|
result.w = cos(half_theta);
|
||||||
|
result.x = a0 * k;
|
||||||
|
result.y = a1 * k;
|
||||||
|
result.z = a2 * k;
|
||||||
|
} else {
|
||||||
|
// At the origin, sqrt() will produce NaN in the derivative since
|
||||||
|
// the argument is zero. By approximating with a Taylor series,
|
||||||
|
// and truncating at one term, the value and first derivatives will be
|
||||||
|
// computed correctly when Jets are used.
|
||||||
|
const T k(0.5);
|
||||||
|
result.w = T(1.0);
|
||||||
|
result.x = a0 * k;
|
||||||
|
result.y = a1 * k;
|
||||||
|
result.z = a2 * k;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
Loading…
Reference in a new issue