7

Context:

A camera at O has an image of a rectangular object ABCD.

  • From the 2D coordinates of the image + the projection matrix of the camera I can calculate the direction vectors $\hat {A}, \hat{B}, \hat{D}$
  • From $\hat {A}, \hat{B}, \hat{D}$ and the cosine rule I can calculate the angles $∠AOB, ∠AOD, ∠BOD$
  • I know the expected dimensions of the rectangle $ABCD$ so I know the vector magnitudes $|\vec{AB}|, |\vec{AD}|$

rectangle projection

ultimately, I want to know the 3D coordinates $\vec{A},\vec{B},\vec{D}$

Since I have $\hat {A}, \hat{B}, \hat{D}$ - I need to determine $|\vec {A}|, |\vec{B}|, |\vec{D}|$

From the sine rule I can write the following equations about triangles $OAB, OAD, OBD$, respectively

$\frac{|\vec{AB}|}{Sin(∠AOB)} = \frac{|\vec{B}|}{Sin(∠OAB)} = \frac{|\vec{A}|}{Sin(π-∠AOB-∠OAB)} $

$\frac{|\vec{AD}|}{Sin(∠AOD)} = \frac{|\vec{A}|}{Sin(∠ODA)} = \frac{|\vec{D}|}{Sin(π-∠AOD-∠ODA)} $

$\frac{+\sqrt{|\vec{AB}|²+|\vec{AD}|²}}{Sin(∠BOD)} = \frac{|\vec{D}|}{Sin(∠OBD)} = \frac{|\vec{B}|}{Sin(π-∠BOD-∠OBD)} $

Since this is essentially 6 equations with 6 unknowns $|\vec{A}|, |\vec{B}|, |\vec{D}|, ∠OAB, ∠ODA, ∠OBD$, it should be solvable(?)

This is how I phrase the problem to mathematica:

Solve[
A2B / Sin[AOB] == B / Sin[OAB] == A / Sin[\[Pi]-AOB-OAB] &&
A2D / Sin[AOD] == A / Sin[ODA] == D / Sin[\[Pi]-AOD-ODA] &&
Sqrt[A2B * A2B + A2D * A2D] / Sin[BOD] == D / Sin[OBD] == B / Sin[\[Pi]-BOD-OBD],
{A, B, D, OAB, ODA, OBD}]

However, mathematica hangs indefinitely.

What is the correct way to phrase this problem? Have I missed something or made a mistake?

Edit: Some measured example values:

$\hat {A} = [-0.0359537, 0.0186775, 0.999179]$

$\hat {B} = [0.101493, 0.0185931, 0.994662]$

$\hat{D} = [-0.0359263, -0.0432848, 0.998417]$

$∠AOB=0.13763, ∠AOD=0.0619769, ∠BOD=0.150898$

$|\vec{AB}|=5.76, |\vec{AD}|=2.438$

In this case, the lengths $|\vec {A}|, |\vec{B}|, |\vec{D}|$ are around 43m

ezekiel
  • 123
  • 6
  • In fact, you have eleven variables, {A, A2B, A2D, AOB, AOD, B, BOD, D, OAB, OBD, ODA}. Even if you had only six, `Solve might not be able to handle this problem. Incidentally, a Weierstrass Substitution then might be helpful. – bbgodfrey Apr 20 '22 at 22:57
  • There are 11 symbols - but 5 of them AOB, AOD, BOD, A2B, A2D are constants, not variables - so a solution in terms of them (and any other constants like pi, is what I'm looking for. – ezekiel Apr 21 '22 at 09:35
  • 1
    I have converted your equations to polynomials, which Mathematica should be able to solve. Now running GroebnerBasis. – bbgodfrey Apr 21 '22 at 17:22
  • thanks! when you say "converted to polynomials" - do you mean like an approximation using the first N terms of a taylor expansion or similar? – ezekiel Apr 21 '22 at 17:34
  • No. I first eliminated {a, b, d} and then converted all instances of the three unknown angles to Sin of those angles. Thus, the remaining three equations are polynomials in the three Sins. In all, it is a sixth order system, which I now am attempting to solve. – bbgodfrey Apr 21 '22 at 17:38
  • Related questions have appeard in this forum. One approach is here. Also see this MSE thread or this other. – Daniel Lichtblau Apr 22 '22 at 17:14
  • See point 4, https://mathematica.stackexchange.com/a/18395/4999. In particular D is a protected system symbol, which you should probably avoid, along with other symbols beginning with a capital. – Michael E2 Apr 25 '22 at 16:04

2 Answers2

5

With capitalized symbols for the dependent variables converted to lower-case symbols to help distinguish them from parameters, the equations are

eq = {A2B/Sin[AOB] == b/Sin[oab], 
      A2B/Sin[AOB] == a/Sin[\[Pi] - AOB - oab], 
      A2D/Sin[AOD] == a/Sin[oda], 
      A2D/Sin[AOD] == d/Sin[\[Pi] - AOD - oda], 
      Sqrt[A2B*A2B + A2D*A2D]/Sin[BOD] == d/Sin[obd], 
      Sqrt[A2B*A2B + A2D*A2D]/Sin[BOD] == b/Sin[\[Pi] - BOD - obd]};

(Doing so includes replacing D by d, which is necessary, because D is a reserved symbol.) Next, eliminate {a, b, d}.

s1 = Solve[eq[[1 ;; 5 ;; 2]], {a, b, d}] // Flatten
(* {a -> A2D Csc[AOD] Sin[oda], b -> A2B Csc[AOB] Sin[oab], 
    d -> Sqrt[A2B^2 + A2D^2] Csc[BOD] Sin[obd]} *)
eq /. s1 /. True -> Nothing;
(DivideSides[#, Times @@ Cases[#, _Csc, 4]] & /@ %)[[All, 1, 1, 1]] // TrigExpand
(* {A2B Cos[oab] Sin[AOB] Sin[AOD] + A2B Cos[AOB] Sin[AOD] Sin[oab] == 
        A2D Sin[AOB] Sin[oda], 
    A2D Cos[oda] Sin[AOD] Sin[BOD] + A2D Cos[AOD] Sin[BOD] Sin[oda] == 
        Sqrt[A2B^2 + A2D^2] Sin[AOD] Sin[obd], 
    Sqrt[A2B^2 + A2D^2] Cos[obd] Sin[AOB] Sin[BOD] + 
        Sqrt[A2B^2 + A2D^2] Cos[BOD] Sin[AOB] Sin[obd] == A2B Sin[BOD] Sin[oab]} *)

Trigonometric functions of the dependent variables next are converted to polynomials.

Flatten@Solve[%, Cos /@ {oab, oda, obd}] /. Rule -> Equal;
ApplySides[#^2 &, #] & /@ %;
eq2 = % /. Cos[z_]^2 -> 1 - Sin[z]^2 
    /. {Sin[oab] -> soab, Sin[oda] -> soda, Sin[obd] -> sobd}
(* {1 - soab^2 == (-A2B soab Cot[AOB] + A2D soda Csc[AOD])^2/A2B^2, 
    1 - soda^2 == (-A2D soda Cot[AOD] + Sqrt[A2B^2 + A2D^2] sobd Csc[BOD])^2/A2D^2,
    1 - sobd^2 == (-Sqrt[A2B^2 + A2D^2] sobd Cot[BOD] + 
        A2B soab Csc[AOB])^2/(A2B^2 + A2D^2)} *)

I had expected at this point that GroebnerBasis[%, {soab, soda, sobd}, Sort -> True] would cast the equations into solvable form without difficulty, but the computation ran for 20 hours without result. At that point, I proceeded as follows to reduce the three equations to one.

s2 = Solve[eq2[[2 ;;]], {soab, soda}] // Simplify;
s2[[4]]
(* {soab -> (Sin[AOB] (A2B Sqrt[A2B^2 + A2D^2] sobd Cot[BOD] + 
        Sqrt[-A2B^2 (A2B^2 + A2D^2) (-1 + sobd^2) Csc[AOB]^2] Sin[AOB]))/A2B^2, 
    soda -> ((Sqrt[A2B^2 + A2D^2] sobd Cot[AOD] Csc[BOD] + 
        A2D Sqrt[1 + Cot[AOD]^2 - ((A2B^2 + A2D^2) sobd^2 Csc[BOD]^2)/
        A2D^2]) Sin[AOD]^2)/A2D} *)

with the other three solutions differing only by signs of the Sqrts. The strategy now is to use these two rules to obtain a single equation for sobd and then squaring the Sqrts to eliminate them to create a polynomial equation.

s2[[4]] /. {Sqrt[-A2B^2 (A2B^2 + A2D^2) (-1 + sobd^2) Csc[AOB]^2] -> sqrt1, 
    Sqrt[1 + Cot[AOD]^2 - ((A2B^2 + A2D^2) sobd^2 Csc[BOD]^2)/A2D^2] -> sqrt2};
(Expand[eq2[[1]] /. %] /. 
    {sqrt1^2 -> -A2B^2 (A2B^2 + A2D^2) (-1 + sobd^2) Csc[AOB]^2, 
     sqrt2^2 -> 1 + Cot[AOD]^2 - ((A2B^2 + A2D^2) sobd^2 Csc[BOD]^2)/A2D^2})
         // Simplify;
Solve[%, sqrt2][[1, 1]] /. Rule -> Equal;
(Expand@ApplySides[(# Denominator[%[[2]]])^2 &, %] /. 
    {sqrt1^2 -> -A2B^2 (A2B^2 + A2D^2) (-1 + sobd^2) Csc[AOB]^2, 
     sqrt2^2 -> 1 + Cot[AOD]^2 - ((A2B^2 + A2D^2) sobd^2 Csc[BOD]^2)/A2D^2})
         // Simplify;
Solve[%, sqrt1][[1, 1]] /. Rule -> Equal;
Expand@ApplySides[(# Denominator[%[[2]]])^2 &, %] /. 
    sqrt1^2 -> -A2B^2 (A2B^2 + A2D^2) (-1 + sobd^2) Csc[AOB]^2;
eq3 = Collect[-(Subtract @@ %) Sin[BOD]^2/64, sobd, Simplify] == 0

The results, with a LeafCount of 1851, is far too long to reproduce here. It is biquartic and easily solved in terms of Root functions by

s3 = Solve[eq3, sobd, Quartics -> False]

Each of the eight solutions is enormous. I presume that two satisfy -1 < sobd <1, and that the rest do not and can be discarded. Thereafter, s2 can be used to obtain {soab, soda}, and s1 to obtain {a, b, d} by back substitution, again with enormous expressions.

Addendum: Numerical Solutions

With the numerical parameters just added to the question, NSolve produces results without difficulty.

NSolve[eq2 /. {A2B -> 5.76, A2D -> 2.438, AOB -> 0.13763, 
    AOD -> 0.0619769, BOD -> 0.150898}, Reals]
(* {{soab -> 0.975185, sobd -> 0.946034, soda -> 0.997894}, 
    {soab -> -0.975185, sobd -> -0.946034, soda -> -0.997894}, 
    {soab -> 0.878631, sobd -> 0.946037, soda -> 0.998155}, 
    {soab -> -0.878631, sobd -> -0.946037, soda -> -0.998155}} *)
bbgodfrey
  • 61,439
  • 17
  • 89
  • 156
  • wow, thanks! I'm trying to understand this a bit more - I've reproduced all of this in a notebook and have the 8 Root functions you describe from S3 which are all of the form

    Sqrt(Root( f(AOB, AOD, BOD, A2B, A2D, Slot(1)))

    each equation has 4 instances of Slot(1) - I understand that means "the first argument of a pure function" - but in this context I'm not sure what that means?

    Does this mean that for a given AOB, AOD, BOD, A2B, A2D you would have to solve equations from s3. ie. there isn't a single generic expression I could write in c++ that would work for arbitrary inputs?

    – ezekiel Apr 23 '22 at 18:02
  • @ezekiel I should have asked earlier, what are you trying to accomplish. For instance, do you want numerical solutions? If so, the parameter values should be inserted much earlier in the computation. Give me a sample set of parameter values, and I shall see what I can do. – bbgodfrey Apr 23 '22 at 22:32
  • What I ultimately need to write is a single function (in C++) that I can plug in the information I start with {∠AOB ∠AOD ∠BOD, A^, B^, D^, A2B, A2D} and ultimately derive the equation of the plane ABCD. Ofc. I could get the plane easily if I had the 3D points A,B,D. For my application there are actually only a few possible values of the lengths - A2D is always 2.428 and A2B is either: 5.760, 11.895, 13.415 - I will know which one I am expecting each time - so three solutions for each case of A2B would be good, if a fully parametrised generic solution isn't possible. – ezekiel Apr 25 '22 at 09:47
  • Have added some example, measured values to the question – ezekiel Apr 25 '22 at 10:04
  • @ezekiel With numerical values for the parameters, NSolve produces results without difficulty. See my sample numerical answer. – bbgodfrey Apr 25 '22 at 15:07
  • ah - sorry, I think I'm still not being clear - I need to be able to write a generic solution that will work for arbitrary A^, B^, D^ values. I don't know in advance what those will be - it will depend on the 2D detection from which they are derived. – ezekiel Apr 25 '22 at 22:16
  • @ezekiel My original answer provides the generic solution. Admittedly, it is very complicated. – bbgodfrey Apr 25 '22 at 23:04
  • ok, thanks! I guess I still don't quite understand - the output of s3 gives us 8 equations like: sobd = Sqrt(Root( f(AOB, AOD, BOD, A2B, A2D, Slot(1))) - I'm not sure how to implement this is a c++ function - what is 'Slot(1)' and 'Root' in this context ? Does it mean that for any given input values A^, B^, D^, you would have to newly solve each equation "find the roots" in order to find sobd (and then ultimately |A| etc.) ? – ezekiel Apr 26 '22 at 08:44
  • @ezekiel Yes it does mean that. There is no simple solution. However, obtaining numerical solutions is fast and easy, using the procedure I added to the answer. – bbgodfrey Apr 27 '22 at 01:37
  • ok thanks, I'll give it a few more days just in case somebody finds a way, and if not will mark this is accepted. – ezekiel Apr 27 '22 at 10:07
4

I think you should stay away from angles. You already have the direction of the vectors $\vec{e}_a$, $\vec{e}_b$, and $\vec{e}_d$ so you know that your vertices are going to be the vectors $a \vec{e}_a$, $b \vec{e}_b$, and $d \vec{e}_d$, in which $a$, $b$, and $d$ are lengths that you would like to compute. The sides of the rectangle are the vectors

$ \begin{aligned} \vec{v}_{ba} &= b \vec{e}_b - a \vec{e}_a, & \vec{v}_{da} &= d \vec{e}_d - a \vec{e}_a \end{aligned} $

and they should satisfy the constraints

$ \begin{aligned} \vec{v}_{ba} . \vec{v}_{da} &= 0, & \|\vec{v}_{ba}\|^2 &= L_{ba}^2, & \|\vec{v}_{da}\|^2 &= L_{da}^2. \end{aligned} $

Those are polynomials in $a$, $b$, and $d$ that you should solve. To make things simpler first rotate the direction vectors as in

$ \begin{aligned} \begin{bmatrix} e_a & e_b & e_d \end{bmatrix} = Q R, \quad Q^T Q = I, \quad R = \begin{bmatrix} 1 & r_{21} & r_{31} \\ 0 & r_{22} & r_{32} \\ 0 & 0 & r_{33} \end{bmatrix}. \end{aligned} $

The constraints can now be expressed in terms of the rotated vectors

ea = {1, 0, 0};
eb = {r21, r22, 0};
ed = {r31, r32, r33};

as the equations

vba = b eb - a ea;
vda = d ed - a ea;
eqs = {
  vba . vda == 0,
  vba . vba == lba^2,
  vda . vda == lda^2
}

which are

{(-a + b r21) (-a + d r31) + b d r22 r32 == 0, (-a + b r21)^2 + b^2 r22^2 == lba^2, (-a + d r31)^2 + d^2 r32^2 + d^2 r33^2 == lda^2}

Assuming that a suitable value of a is in hand, the second and third equations can be solved easily in terms of b and d because they are quadratic

solbda = Solve[eqs[[{2, 3}]], {b, d}]

The result is a list a rules with the four possible combinations of the two roots of each equation. For example, the first solution is

{b -> (a r21 - Sqrt[lba^2 r21^2 - a^2 r22^2 + lba^2 r22^2])/( r21^2 + r22^2), d -> (2 a r31 - Sqrt[ 4 a^2 r31^2 - 4 (a^2 - lda^2) (r31^2 + r32^2 + r33^2)])/( 2 (r31^2 + r32^2 + r33^2))}

As for a suitable a, one can in principle substitute the solutions from b and d above into the first equation and obtain polynomials in a that need to be solved. A better route is to eliminate b and d directly from the system of equations, which can be done by calculating a Grobner Basis

gb = GroebnerBasis[eqs, {a}, {b, d}];

The result turns out to be a single polynomial in $a^2$ of order 4 whose coefficients

coeff = CoefficientList[gb[[1]], a];
coeff[[{1, 3, 5, 7, 9}]] // Simplify

are

{lba^4 lda^4 (r21 r31 + r22 r32)^4, -2 lba^2 lda^2 (r21 r31 + r22 r32)^2 (lda^2 r22^2 (r31^2 + r32^2) + lba^2 (r22^2 r32^2 + r21^2 (r32^2 + r33^2))), lda^4 r22^4 (r31^2 + r32^2)^2 + lba^4 (r22^2 r32^2 + r21^2 (r32^2 + r33^2))^2 + 2 lba^2 lda^2 r22^2 (r22^2 r32^2 (-r31^2 + r32^2 - r33^2) + 2 r21 r22 r31 r32 (2 r32^2 + r33^2) + r21^2 (-r32^2 (r32^2 + r33^2) + r31^2 (r32^2 + 2 r33^2))), -2 r22^2 r33^2 (lda^2 r22^2 (r31^2 - r32^2) + lba^2 (-r22^2 r32^2 + r21^2 (r32^2 + r33^2))), r22^4 r33^4}

If there is one "formula" you are looking for then that is the one. Once you solve this polynomial for a, plug the values back into the solution for b and d to find which one is actually the solution to your problem. There may be roots that do not lead to solutions. Illustrating the process with your data:

AA = {-0.0359537, 0.0186775, 0.999179};
BB = {0.101493, 0.0185931, 0.994662};
DD = {-0.0359263, -0.0432848, 0.998417};
LAB = 5.76;
LAD = 2.438;

first calculate the rotation

M = Transpose[{AA, BB, DD}];
{Q, R} = QRDecomposition[M];

to obtain the vectors in R which in this case result in the matrix

$ \left( \begin{array}{ccc} 1. & 0.990544 & 0.99808 \\ 0. & -0.137195 & 0.0000416619 \\ 0. & 0. & 0.0619372 \\ \end{array} \right) $

That is the data for your problem:

data = Join[Thread[{r21, r31} -> R[[1, {2, 3}]]], Thread[{r22, r32} -> R[[2, {2, 3}]]],{r33 -> R[[3, 3]], L1 -> LAB, L2 -> LAD}];

which I rationalize before calculating the polynomial roots

rdata = Rationalize[data, 1/1000000000]

to obtain

{r21 -> 41899/42299, r31 -> 32237/32299, r22 -> -(5923/43172), r32 -> 1/24003, r33 -> 1871/30208, lba -> 144/25, lda -> 1219/500}

Next solve the quartic polynomial in $a^2$:

p = coeff[[{1, 3, 5, 7, 9}]] . {1, a2, a2^2, a2^3, a2^4};
sola2 = Solve[p == 0 /. rdata, a2];
sola2 // N

of which only the first two solutions are real and positive

{{a2 -> 1542.88}, {a2 -> 1543.69}, {a2 -> 1729.64 - 0.963126 I}, {a2 -> 1729.64 + 0.963126 I}}

The square-root of those are your candidate values for a:

sola = {{a -> Sqrt[a2 /. sola2[[1]]]}, {a -> Sqrt[a2 /. sola2[[2]]]}}
sola // N

which in this case are

{{a -> 39.2795}, {a -> 39.2898}}

The values of b and c can be calculated using the solution in terms of a obtained before. In this example the solutions are obtained by picking

{i, j} = {4, 1};
{a, b, d} /. solbda[[i]] /. sola[[j]] /. rdata // N

which leads to

{39.2795, 40.9419, 39.3622}

or

{i, j} = {3, 2};
{a, b, d} /. solbda[[i]] /. sola[[j]] /. rdata // N

which is

{39.2898, 36.8882, 39.3624}

One can verify that the solutions also work on the original data, as in

{i, j} = {4, 1};
{Sqrt[(b BB - a AA) . (b BB - a AA)], Sqrt[(d DD - a AA) . (d DD - a AA)], b BB - a AA) . (d DD - a AA)} /. solbda[[i]] /. sola[[j]] /. rdata // N

which produces the values

{5.76, 2.438, -4.93366*10^-6}

satisfying the three desired equations.

Mauricio de Oliveira
  • 2,001
  • 13
  • 15
  • would you mind explaining why you Rationalize the input values? – ezekiel May 03 '22 at 15:42
  • 1
    Not an essential step. But working with coefficients of high order polynomials in floating point can produce large errors in the subsequent calculations. In this case, with order 4, not a deal breaker. – Mauricio de Oliveira May 03 '22 at 16:41
  • When I reproduce this - I get the same 2 candidate values for 'a' and then 4 solutions for each, so 8 in total. I don't quite understand how/why you have discounted 6 of them? Would you mind expanding a little? – ezekiel May 05 '22 at 08:45
  • 1
    The other solutions do not satisfy all the constraints. The length constraints are always satisfied because b and c make sure that that is the case for any a. However only some as satisfy the orthogonality constraints. Check them out. – Mauricio de Oliveira May 05 '22 at 18:13
  • ah ok! Does the orthogonality constraint come into the derivation at any other point? If not, does that mean that this same formula/method would work generally for non-rectangular shapes (with different, known, non 90° angles) – ezekiel May 05 '22 at 19:45
  • It does, by asking that the inner product be zero. You can ask it to be anything else you would like though and that would still be a polynomial constraint. – Mauricio de Oliveira May 05 '22 at 20:34