eig3x3known0.h

Go to the documentation of this file.

// Copyright 2019, Philipp Zabel
// Copyright 2026, Beyley Cardellio
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Eigenvalue solver for 3x3 symmetric matrices with known zero eigenvalue, specialized for P3P solving.
 * @author Philipp Zabel <philipp.zabel@gmail.com>
 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
 * @ingroup xrt_iface
 */
 
#pragma once
 
#include "quadratic.h"
 
#include <math.h>
 
 
/*
 * This implements algorithm 2 "eig3x3known0" described in the paper "Lambda
 * Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver." by
 * Mikael Persson and Klas Nordberg. See the paper for details.
 *
 * 1: function GETEIGVECTOR(m, r)
 * this is open coded below, since it is called twice, and intermediate
 * values are reused
 *
 * 7: function EIG3X3KNOWN0(M)
 * this function does not write E[2,5,8]
 */
static inline void
eig3x3known0(const double *M, double *E, double *sigma)
{
        /* 8: b₃ = M(:,2) x M(:,3); */
        /* 9: b₃ = b₃ / |b₃| */
        /* we skip steps 8 and 9 */
 
        /*
         * 10: m = M(:)
         *
         * we are passed just the upper triangular part of symmetric matrix M:
         *
         *       ⎛ m₁ m₂ m₃ ⎞
         *   M = ⎜    m₅ m₆ ⎟
         *       ⎝       m₈ ⎠
         */
        const double m1 = M[0];
        const double m2 = M[1];
        const double m3 = M[2];
        const double m5 = M[3];
        const double m6 = M[4];
        const double m9 = M[5];
 
        /* reusable intermediate values */
        const double m2_sq = m2 * m2;
        const double m1_plus_m5 = m1 + m5;
 
        /* 11: p₁ = m₁ - m₅ - m₉ */
        /*         --            */
        const double p1 = -m1_plus_m5 - m9;
 
        /* 12: p₀ = -m₂² - m₃² - m₆² + m₁m₅ + m₉ + m₅m₉ */
        /*                             ---------        */
        const double p0 = -m2_sq - m3 * m3 - m6 * m6 + m1 * (m5 + m9) + m5 * m9;
 
        /* 13: [σ₁, σ₂] as the roots of σ² + p₁σ + p₀ = 0 */
        double sigma1 = 0.0;
        double sigma2 = 0.0;
        reduced_quadratic_real_roots(p1, p0, &sigma1, &sigma2);
 
        if (fabs(sigma1) > fabs(sigma2)) {
                sigma[0] = sigma1;
                sigma[1] = sigma2;
        } else {
                sigma[0] = sigma2;
                sigma[1] = sigma1;
        }
 
        const double r1 = sigma[0];
        const double r2 = sigma[1];
 
        /*
         * 14: b₁ = GETEIGVECTOR(m, σ₁)
         * 15: b₂ = GETEIGVECTOR(m, σ₂)
         * 16: if |σ₁| > |σ₂| then
         * 17:   Return ([b1, b2, b3], σ₁, σ₂)
         * 18: else
         * 19:   Return ([b2, b1, b3], σ₂, σ₁)
         */
 
        /* reusable intermediate values */
        const double m1m5_minus_m2sq = m1 * m5 - m2_sq;
        const double m2m6_minus_m3m5 = m2 * m6 - m3 * m5;
        const double m2m3_minus_m1m6 = m2 * m3 - m1 * m6;
 
        /* 1: function GETEIGVECTOR(m, r) */
        /* 2: c = (r² + m₁m₅ - r(m₁ + m₅) - m₂²) */
        const double tmp1 = 1.0 / (r1 * (r1 - m1_plus_m5) + m1m5_minus_m2sq);
        const double tmp2 = 1.0 / (r2 * (r2 - m1_plus_m5) + m1m5_minus_m2sq);
        /* 3: a₁ = (rm₃ + m₂m₆ - m₃m₅) / c */
        /* 4: a₂ = (rm₆ + m₂m₃ - m₁m₆) / c */
        double a11 = (r1 * m3 + m2m6_minus_m3m5) * tmp1;
        double a12 = (r1 * m6 + m2m3_minus_m1m6) * tmp1;
        double a21 = (r2 * m3 + m2m6_minus_m3m5) * tmp2;
        double a22 = (r2 * m6 + m2m3_minus_m1m6) * tmp2;
        /* 5: v = (a₁ a₂ 1) */
        /* 6: Return v / |v| */
        const double rnorm1 = 1.0 / sqrt(a11 * a11 + a12 * a12 + 1.0);
        const double rnorm2 = 1.0 / sqrt(a21 * a21 + a22 * a22 + 1.0);
 
        E[0] = a11 * rnorm1;
        E[3] = a12 * rnorm1;
        E[6] = rnorm1;
 
        E[1] = a21 * rnorm2;
        E[4] = a22 * rnorm2;
        E[7] = rnorm2;
}