lambdatwist_p3p.h File Reference

Lambda Twist P3P solver. More...

#include "cubic.h"
#include "quadratic.h"
#include "eig3x3known0.h"
#include "refine_lambda.h"
#include "mat.h"
#include "vec.h"
#include <math.h>
#include <string.h>

Include dependency graph for lambdatwist_p3p.h:

Go to the source code of this file.

Macros
#define	MIX(n, m)
#define	VEC3_DOT(x, y)   (x[0] * y[0] + x[1] * y[1] + x[2] * y[2])
#define	VEC3_SCALE(a, x)   {a * x[0], a * x[1], a * x[2]}
#define	VEC3_SUB(x, y)   {x[0] - y[0], x[1] - y[1], x[2] - y[2]}
#define	COMPUTE_LAMBDA(tau)

Functions
static int	lambdatwist_p3p (const double *iny1, const double *iny2, const double *iny3, const double *x1, const double *x2, const double *x3, double(*R)[9], double(*t)[3])

Detailed Description

Lambda Twist P3P solver.

Author

Philipp Zabel phili.nosp@m.pp.z.nosp@m.abel@.nosp@m.gmai.nosp@m.l.com

Beyley Cardellio ep1cm.nosp@m.1n10.nosp@m.n123@.nosp@m.gmai.nosp@m.l.com

Macro Definition Documentation

COMPUTE_LAMBDA

#define COMPUTE_LAMBDA

(

tau

)

Value:

        if ((tau) > 0.0)                                                                                               \
                do {                                                                                                   \
                        /*                                                                                             \
                         * The solution to equation (16) for λ₂ₖ is:                                              \
                         * λ₂ₖ = √(a₂₃ / (τₖ(-2b₂₃ + τₖ) + 1))                                    \
                         *                   ^^ missing in the paper                                                   \
                         */                                                                                            \
                        const double tmp = (tau) * (-2.0 * b23 + (tau)) + 1.0;                                         \
                        const double l2 = sqrt(a23 / tmp);                                                             \
                        /* λ₃ₖ = τₖλ₂ₖ */                                                                              \
                        const double l3 = (tau) * l2;                                                                  \
                        /* λ₁ₖ = ω₀λ₂ₖ + ω₁λ₃ₖ (13) */                                                                 \
                        const double l1 = w0 * l2 + w1 * l3;                                                           \
                        if (l1 > 0.0) {                                                                                \
                                L[k][0] = l1;                                                                          \
                                L[k][1] = l2;                                                                          \
                                L[k][2] = l3;                                                                          \
                                k++;                                                                                   \
                        }                                                                                              \
        } while (0)

MIX

#define MIX	(		n,
			m
	)

Value:

        {n[0], m[0], n[1] * m[2] - n[2] * m[1], n[1], m[1], n[2] * m[0] - n[0] * m[2],                                 \
         n[2], m[2], n[0] * m[1] - n[1] * m[0]}