lm_rotations_ceres.inl

Go to the documentation of this file.

// 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 Moshi Turner <moshiturner@protonmail.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;
        }
}