math.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <algorithm>
#include <array>
 
#include <ceres/ceres.h>
 
namespace armarx::navigation::global_planning::optimization
{
 
    /**
     * @brief Signed, wrapped difference `b - a`, in (-pi, pi].
     *
     * `atan2` of the cross and dot products of the two unit vectors. The previous formulation
     * was `acos` of the dot product alone, which has two problems this one does not have:
     *
     *  * `acos` is defined on [-1, 1] and the dot product of two unit vectors leaves it by up to
     *    one ulp whenever the angles agree -- which is every straight stretch of a path -- so it
     *    returned NaN for 3.7 % of equal-angle pairs and Ceres rejected the step;
     *  * its derivative is infinite exactly where the angles agree, so the analytic jacobian had
     *    to guard the singularity by returning *zero* there. That is the wrong value: a set of
     *    perfectly aligned waypoints is where the gradient was needed and where it was
     *    suppressed.
     *
     * `atan2` is well conditioned for every pair and its derivative is exactly -1 with respect to
     * `a` and +1 with respect to `b`, everywhere.
     *
     * The residual is now signed where it used to be a magnitude. The objective is unchanged:
     * Ceres squares residuals, and `|b - a|^2 == (b - a)^2` for the wrapped difference.
     */
    auto
    periodicDiff(const auto a, const auto b)
    {
        const auto cross = ceres::cos(a) * ceres::sin(b) - ceres::sin(a) * ceres::cos(b);
        const auto dot = ceres::cos(a) * ceres::cos(b) + ceres::sin(a) * ceres::sin(b);
 
        return ceres::atan2(cross, dot);
    }
 
    /// Partial derivatives of `periodicDiff(a, b)` with respect to `a` and `b`.
    ///
    /// Constant, and no longer a guarded singularity: the wrapped difference is locally just
    /// `b - a`. The wrap itself is not differentiable at +-pi, where the shortest way round
    /// swaps -- a measure-zero point the minimizer has no reason to sit on, and one the old
    /// formulation could not represent either.
    std::array<double, 2> inline periodicDiffJacobian(const auto /*a*/, const auto /*b*/)
    {
        return {-1.0, 1.0};
    }
 
} // namespace armarx::navigation::global_planning::optimization