refine_lambda.h

Go to the documentation of this file.

// Copyright 2019, Philipp Zabel
// Copyright 2026, Beyley Cardellio
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Gauss-Newton refinement of Λ, specialized for P3P solving.
 * @author Philipp Zabel <philipp.zabel@gmail.com>
 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
 * @ingroup xrt_iface
 */
 
#pragma once
 
#include <math.h>
 
 
/*
 * The number of iterations is chosen to just match the precision of the
 * original Lambda Twist implementation.
 * Increasing this further, we'd reach the point of diminishing returns.
 */
#define REFINE_LAMBDA_ITERATIONS 5
 
/*
 * Section 3.8 "Implementation Details" states:
 *
 *   "Specifically, we refine the solution using a few verified steps of
 *    Gauss-Newton optimization on the sum of squares of Eqn (4)."
 *
 * Equation 4 in the paper:
 *
 *   Λ^T M₁₂ Λ = a₁₂,   Λ^T M₁₃ Λ = a₁₃,   Λ^T M₂₃ Λ = a₂₃   (4)
 *
 * where
 *
 *         ⎛ 1  -b₁₂ 0 ⎞        ⎛ 1   0 -b₁₃⎞        ⎛ 0   0   0 ⎞
 *   M₁₂ = ⎜-b₁₂ 1   0 ⎟, M₁₃ = ⎜ 0   0   0 ⎟, M₂₃ = ⎜ 0   1 -b₂₃⎟.
 *         ⎝ 0   0   0 ⎠        ⎝-b₁₃ 0   1 ⎠        ⎝ 0 -b₂₃  1 ⎠
 *
 * thus refine Λ towards:
 *
 *   λ₁² - 2b₁₂λ₁λ₂ + λ₂² - a₁₂ = 0
 *   λ₁² - 2b₁₃λ₁λ₃ + λ₃² - a₁₃ = 0
 *   λ₂² - 2b₂₃λ₂λ₃ + λ₃² - a₂₃ = 0
 *
 */
static inline void
gauss_newton_refineL(double *L, double a12, double a13, double a23, double b12, double b13, double b23)
{
        double l1 = L[0];
        double l2 = L[1];
        double l3 = L[2];
 
        for (int i = 0; i < REFINE_LAMBDA_ITERATIONS; i++) {
                /* λᵢ² */
                const double l1_sq = l1 * l1;
                const double l2_sq = l2 * l2;
                const double l3_sq = l3 * l3;
 
                /* residuals r(Λ) = λᵢ² - 2bᵢⱼλᵢλⱼ + λⱼ² - aᵢⱼ */
                const double r1 = l1_sq - 2.0 * b12 * l1 * l2 + l2_sq - a12;
                const double r2 = l1_sq - 2.0 * b13 * l1 * l3 + l3_sq - a13;
                const double r3 = l2_sq - 2.0 * b23 * l2 * l3 + l3_sq - a23;
 
                /* bail early if the solution is good enough */
                if (fabs(r1) + fabs(r2) + fabs(r3) < 1e-10) {
                        break;
                }
 
                /*
                 *                   ⎛ λ₁-b₁₂λ₂  λ₂-b₁₂λ₁  0       ⎞
                 * Jᵢⱼ = ∂rᵢ/∂λⱼ = 2 ⎜ λ₁-b₁₃λ₃  0         λ₃-b₁₃λ₁⎟
                 *                   ⎝ 0         λ₂-b₂₃λ₃  λ₃-b₂₃λ₂⎠
                 */
                const double j0 = l1 - b12 * l2; /* ½∂r₁/∂λ₁ */
                const double j1 = l2 - b12 * l1; /* ½∂r₁/∂λ₂ */
                const double j3 = l1 - b13 * l3; /* ½∂r₂/∂λ₁ */
                const double j5 = l3 - b13 * l1; /* ½∂r₂/∂λ₃ */
                const double j7 = l2 - b23 * l3; /* ½∂r₃/∂λ₂ */
                const double j8 = l3 - b23 * l2; /* ½∂r₃/∂λ₃ */
 
                /* 4 / det(J) */
                const double det = 0.5 / (-j0 * j5 * j7 - j1 * j3 * j8);
 
                /* Λ = Λ - J⁻¹ * r(Λ) */
                l1 -= det * (-j5 * j7 * r1 - j1 * j8 * r2 + j1 * j5 * r3);
                l2 -= det * (-j3 * j8 * r1 + j0 * j8 * r2 - j0 * j5 * r3);
                l3 -= det * (j3 * j7 * r1 - j0 * j7 * r2 - j1 * j3 * r3);
        }
 
        L[0] = l1;
        L[1] = l2;
        L[2] = l3;
}