ANSWERED on Sun 17 Jan 2010 - 7:50 pm UTC by mathtalk
Question: Geometry: Find a 3d location based on 6 angles and 1 length (4PointsAndACamera)
Home » Science and Mathematics » #3601
Please carefully read the Disclaimer and Terms & conditions.Priced at $50.00
The customer tipped the researcher $70.00
Actions: Add Comment
Asked by eldenc on Fri 8 Jan 2010 - 12:23 am UTC:
Geometry: Find a 3d location based on 6 angles and 1 length
(4PointsAndACamera)
Let A, B, C, and D be four points on the xy plane in a 3 Dimensional
Cartesian space.
Let M represent a point of unknown position in the positive xyz quadrant.
Given that:
A is as the origin (0,0,0)
B is on the x axis (r,0,0)
C is on the y axis (0,r,0)
D is on the xy plane (r,r,0)
where r > 0
The angles AMB, AMC, AMD, BMC, BMD, and CMD are known.
Optionally, if necessary, M is outside of the sphere of radius r;
i.e. x > r, y > r, z > r;
for the most part x>>r,y>>r,z>r;
thus making all of the angles acute.
Find the equations of the location of M:
Mx=f(r, Angle AMB, Angle AMC, Angle AMD, Angle BMC, Angle BMD, Angle
CMD)
My=g(r, Angle AMB, Angle AMC, Angle AMD, Angle BMC, Angle BMD, Angle
CMD)
Mz=h(r, Angle AMB, Angle AMC, Angle AMD, Angle BMC, Angle BMD, Angle
CMD)
Note: If it is more convenient or less ambiguous, D can be moved to another
location on the xy plane.
Note: I believe that this is over specified, I think the point D is not
needed; i.e
Mx=f(r, Angle AMB, Angle AMC, Angle BMC)
My=g(r, Angle AMB, Angle AMC, Angle BMC)
Mz=h(r, Angle AMB, Angle AMC, Angle BMC)
Question clarification by eldenc on Fri 8 Jan 2010 - 4:05 pm UTC:
As a side note, I am getting the angles around point M by using a WiiMote camera. The WiMote (in ubuntu linux 'sudo apt-get wmgui') returns the 4 brightest IR points in its field of view. I'm assuming that the distance between the IR points (sqrt((x1-x2)**2+(y1-y2)**2))) is proportional to the angle. (After ensuring that the pixels are square.) Is that assertion correct? I haven't determined the field of view angles, yet. I should have that figured out this weekend.
Request for clarification by Researcher mathtalk on Fri 8 Jan 2010 - 8:48 pm UTC:
The clarification about the Wiimote IR camera was helpful. Knowing a
little about the context I think allows me to address your problem more
clearly.
Let me tackle the question you raise in the clarification, about assuming
the angle is proportional to the apparent (pixelized) distance between two
(bright IR) reference points. The assumption is approximately true for
small angles/short distances, but if you want an accurate computation of
the location of the Wiimote camera, wider angles/longer distances are
better and will quite possibly invalidate that approximation. Also be
aware that the camera lens may distort angles/distances around the
periphery of its field of view.
a
/|
/ |
/ |
eye *---+
\ |
\ |
\|
b
Assume that the camera lens (eye) is directed at the midpoint between a and
b. Then the apparent distance between a and b is proportional to:
2 sin(mu/2)
for angle mu between the lines of sight to a and b, if the optics are
"flat" in that region.
As the angle mu tends to zero, the above expression is asymptotic to mu (as
can be seen from the Taylor series for sine).
What the constant of proportionality is between apparent distance
(pixelized) and the above amounts to asking what the apparent depth of the
field of view is, i.e. the dotted line "distance" perpendicular to the
segment ab shown above. As I think you have in mind, an experiment can
work out the constant.
Two more quick and related notes:
First, yes, the angles that M makes with respect to D and the other points
over-specify the problem of locating M. In principle the angles at M
formed by pairs AB, AC, and BC are sufficient to locate M at one of two
places, symmetric with respect to the xy-plane (containing A,B,C).
Having the over-specified information is not necessarily a drawback, in
that it can allow us to detect inconsistencies in measurements or to "best
fit" M to measurements with small "random" errors.
Second, the accuracy of the computation is impaired by Mx >> r and My >> r.
Think of the square ABDC as the "baseline" from which M is observed. The
most accurate positioning happens when M lies over the square (as deduced
from angle measurements) as one realizes if the limiting case as r
approaches zero (for fixed M) is contemplated.
I hope this "numerical sensitivity" effect will become clearer as we work
through the formulas to a solution for M. The technical term for locating
an unknown point from angles measured at that vertex is "resection".
Unfortunately this word has many unrelated meanings and applications, so it
is fairly weak as a search term. Related terms are "triangulating"
(locating the unknown point from angles at known vertices) and
"trilateration" (locating the unknown point from its distances to known
points).
[Three Point Resection Problem (in 2D)]
http://www.ferris.edu/faculty/burtchr/sure215/notes/resection.pdf
In three dimensions the locus of points M forming a specified positive
angle AMB with known points A.B is a surface of revolution about the axis
passing through A and B. The case where AMB is a right angle is
particularly nice -- the surface is a sphere with diameter AB.
In theory intersecting two closed surfaces produces one or more closed
curves (e.g. two spheres will intersect in a circle, a point, or not at
all), and intersecting a third surface restricts the possibilities to a
finite number of points (as a circle will intersect a sphere twice, once,
or not at all).
I will ask you to clarify if you are looking more for a mathematical
formulation or a programming approach to solving this.
regards, mathtalk
Question clarification by eldenc on Fri 8 Jan 2010 - 11:28 pm UTC:
I looked at the reference, but unfortunately my last Linear Algebra class
was 14 years ago so I'm a bit slow, I figured I should clarify prior to
understanding the entire text.
Yes the desired format of the answer is C code. (I should be able to handle
the conversion between the trig formula and C code, but if your
offering...(not required) :)
struct Point {float x; float y;};
Point p[4]; //p[0]=A, p[1]=B, etc....
After a little reflection, I exaggerated about the x>>r, sort'of.
I'm flying an RC model plane based on GPS. Great for flying...not so good
for landing (~2 football fields are required to deal with the GPS altitude
error, which can frequently be up to 10m, unless your willing to land hard)
So I think the numerical granularity issues will dissolve away since in the
beginning where the Wii Error is the greatest, far away, I have lots of
error to play with; not causing any issues. As I approach the landing site
(d,d,0) the angles between the points will increase, thus reducing my
error. To stay in the field of view of the Wii (~70 degrees) this may be
more like (2d,2d,0).
My goal is to put the sensor though an imaginary 2' by 2' box (at roughly
25mph) using 3 or 4 car headlights with r=10'.
FYI: The WiiMote IR camera can report at a 5ms clip, maybe faster (I
haven't pushed it).
Question clarification by eldenc on Sat 9 Jan 2010 - 1:08 am UTC:
Initial development, I plan on comparing the GPS to my Wii Position as a sanity check. Then use the Wii data to make me fly on the glide slope, a ray from the landing site into the sky usually at about 3 degrees. In my case, the projection of this ray onto the xy plane will follow x=y.
Request for clarification by Researcher mathtalk on Sat 9 Jan 2010 - 7:12 am UTC:
Do you have a method of actually distinguishing A,B,C,D in the camera's field of view? That is, mathematically you want to determine the camera's position from the angles subtended by AB, AC, etc. knowing the locations of A,B,C,D. But how do you know which pair of points is being observed in the camera's view? I'm imagining that you could have LED's that flash at different rates or perhaps in varying combinations. regards, mathtalk
Question clarification by eldenc on Sat 9 Jan 2010 - 3:11 pm UTC:
Man you keep late hours! Unfortunately, I don't have LEDs that are bright enough (I would need a board of several hundred, which I may do later); and the incandescents that are bright enough (and broad banded into the IR) could only be flashed as a 1 second rate or slower, way too slow; Xeon,Neon, etc are at the wrong frequency range. So I'm using a geometric solution. I know: a) The camera will remain level and pointing at the origin to withing +/-30degrees (pitch/roll/yaw) (from my primary guidance) b) The camera is constrained to the positive quadrant With that I can sort on y yielding p0=A, p3=D, B<->C=p[1|2]. Now sort p[1,2] on x p[1]=B and p[2]=C Yes, this works in code on the plane (in the 'lab' anyway) As to the format of the answer, I'm more awake now, I think trig functions for Mx=f(r, Angle AMB, Angle AMC, Angle BMC) My=g(r, Angle AMB, Angle AMC, Angle BMC) Mz=h(r, Angle AMB, Angle AMC, Angle BMC) would be great. Additionally, if your willing, Matrix notation for allowing me to move the points around and how it is derived will be really useful later.
Question clarification by eldenc on Sat 9 Jan 2010 - 3:25 pm UTC:
The x and y for the sort are the pixel xy, not the world reference. For Your Amusement: http://paparazzi.enac.fr/wiki/User:EldenC
Question clarification by eldenc on Sat 9 Jan 2010 - 6:37 pm UTC:
Ha, While I was taking a shower I realized that you were going to chid me that if the camera is on the xz plane and I am tilted then point B would windup with a pixelized y value that is greater than point A. Yes that would be true. But my projected course on the xy plane is close (+/-10m) to x=y, so on the real system my lame sort algorithm does work. So you can assume that I know which point is A, B, C (and maybe D); thus I know AMB, AMC, and BMC. Each angle and the corresponding segment length should define an ellipsoid. So I should be able to obtain intersections of the 2 axis aligned ellipsoids (defined by AB and Angle AMB; AC and Angle AMC) and the non-aligned ellipsoid (BC and Angle BMC) to obtain 4(8?) answers. Then throw away all answers but the one in the +,+,+ quadrant. Looking at http://en.wikipedia.org/wiki/Ellipsoid So, I would guess that I need an equation for each ellipsoid in the form of T(x)Ax=1....then solve the 3 equations....
Question clarification by eldenc on Sat 9 Jan 2010 - 7:03 pm UTC:
FYI: I found a paper that looks about right...but there purchasing mechanism is broken! http://cat.inist.fr/?aModele=afficheN&cpsidt=15265349
Request for clarification by Researcher mathtalk on Sun 10 Jan 2010 - 4:36 am UTC:
That abstract does look promising for solving the 3D resection problem. As I understand, they propose to use resultants to eliminate all but one variable from a system of multivariate polynomials, then use numerical root finding on the remaining univariate polynomial. I suspect the numerical solution directly of the multivariate system will prove stable and accurate, but I'm concerned that the solution will suffer from sampling errors (pixelization) from the camera measurements. The surfaces generated by the angles and line segments are not ellipsoids, but that's a good guess. In two dimensions the locus of points consists of two branches symmetric wrt the line segment and mutually terminating at the endpoints of the line segment, and each branch is a circular arc. However only in the case of a right angle do the arcs join to form a surface of rotation that is an ellipsoid (sphere, actually). Recall the theorem from high school geometry that an interior angle of a circle has measure half the central angular measurement of the circular arc cut off by that angle. A picture is here: [Angles Within a Circle] http://www.algebralab.org/lessons/lesson.aspx?file=geometry_interiorangles.xml (see under "interior angle") Think of the angle AMB inscribed in a circle, so that AB is a chord and M a point varying along the circular arc between A and B. Reflect that part of the circle through line segment AB and you have both branches of the locus of points where the angle AMB is a fixed value. Rotating these branches (or one of them) around AB produces the locus of points in 3D. regards, mathtalk
Question clarification by eldenc on Mon 11 Jan 2010 - 7:01 am UTC:
I drew a Google Sketch Up of the problem using your algebra reference, taking a sample location...but I haven't figured out how to connect the circles yet. http://eldenc.tripod.com/a3dLocationFrom3Angles3Pts.skp (Is there a way to attach files in this clarify section?) Note that the circles are not accurately sized or placed.
Request for clarification by Researcher mathtalk on Mon 11 Jan 2010 - 7:05 am UTC:
As an update, I've begun my own write-up of the math and coding for such problems. Also I located a book in the local university library that seems to collect a lot of related research on the "orientation" problem for cameras (Gruen and Huang, 2001). It's a fairly expensive book, so we're in luck to find a borrowable copy. The symmetry of the square of observed points allows for certain shortcuts in the 3D resection problem. In addition my answer will cover the 2D resection problem and a numerical method for a general configuration of points in 3D. regards, mathtalk
Question clarification by eldenc on Mon 11 Jan 2010 - 7:56 am UTC:
If you choose to look at this file, I used several layers that you may want to turn on and off. because its getting a bit cluttered. I'm not sure I understand where to draw the circles. I chose to place the circles such that M and its line segment points (AB for the first one) are on the circumference of the circle. ... but that means that I do not know anything about the locations of the center of the circles. It would seem better to place the circles such that M is the center, but I don't think that's possible.
Question clarification by eldenc on Mon 11 Jan 2010 - 8:04 am UTC:
Cool! FYI: I posted while you where posting so disregard the previous clarifications
Request for clarification by Researcher mathtalk on Mon 11 Jan 2010 - 2:46 pm UTC:
There's an interesting journal article from the European Space Agency here: [Videogrammetry Applied to Environmental Testing for Space Programmes by P. Baussart and A. Meurat (2001)] http://adsabs.harvard.edu/full/2001ESASP.467...43B It's focus (no pun intended) is on stereogrammatic measurements (two cameras), but see the "second" section 2 for discussion of "spatial resection" and many of the issues (accuracy with acute angles, using overdetermined equations) which arise even with a single camera. See especially Fig. 7 for a symmetric four point configuration like you've proposed. regards, mathtalk
Question clarification by eldenc on Mon 11 Jan 2010 - 5:55 pm UTC:
Yes that looks about right. a couple of other resources suggested by a friend this morning (I haven't had time to look into them yet) are OpenCV (Open Camera vision) http://opencv.willowgarage.com/wiki/Welcome and SLAM (Simultaneous localization and mapping) http://www.google.com/url?sa=t&source=web&ct=res&cd=5&ved=0CBkQFjAE&url=http%3A%2F%2Fweb.mit.edu%2F16.412j%2Fwww%2Fhtml%2Flectures%2FL3_Introduction%2520to%2520SLAM.pdf&ei=s2BLS7PVAo_0sgPx47Vu&usg=AFQjCNG-JMH1YuJ2Tg8Y25mOB8vOnwA0BA
Answer by Researcher mathtalk on Sun 17 Jan 2010 - 7:50 pm UTC:
The problem posed here is to find the location:given the angles at M subtended by line segments between known points A,B,C,D:
where r > 0. Methods for solving such "spatial resection" problem are by and large "direct" or "indirect" (iterative) algorithms. Direct methods provide exact solutions after a fixed number of steps, while indirect methods repetitively improve an approximate solution until the desired accuracy is reached. The first known direct solution was given by German mathematician Grunert in 1841. [Lagrange had given an indirect solution in 1795.] We will present Grunert's method using three known points, with the proposal to use a fourth point to decide which among the resulting candidates is the correct solution. After the general method is stated, we interpolate remarks about how this specializes to the specific configuration A,B,C,D given above. Our choice of Grunert's method was informed by a paper "Analysis and Solutions of The Three Point Perspective Pose Estimation Problem" by Haralick, Lee, Ottenberg and Nölle [Proc. IEEE Conf. on Computer Vision and Pattern Recognition, p.590-598 (1991)]. Six direct methods are compared for accuracy when implemented in single- and double-precision arithmetic. Their conclusions put Grunert's method as second-best and show it should be quite satisfactory for present purposes if double-precision is used. * * * * * * * * * Grunert's method starts by finding lengths s_A,s_B,s_C of the unknown tetrahedral edges AM,BM,CM. This phase is called the "distance problem". From those lengths we then locate M, a phase known as the "bogenschnitt" (bow-shock) or "ranging problem". Let a,b,c be the side lengths of BC,AC,AB respectively. Let µ_a, µ_b, µ_c be the _cosines_ of the corresonding angles at M subtended by these three sides. The unknown point M forms a tetrahedron with opposing face ABC. It turns out the angles appear in the distance problem only through their cosines. Distance Problem ================ Grunert based a polynomial system on the law of cosines:
for each triangular face meeting at M. With three quadratic equations in three unknowns, one expects up to eight isolated solutions. In this case we can see that any solutions will pair up in a symmetry through the origin. If s_A,s_B,s_C is a solution, then so is -s_A,-s_B,-s_C. Therefore we will have at most four "physical" solutions to choose from. Observe that the left-hand sides above are homogeneous of degree 2 in the unknowns. Let multipliers u,v be defined:
Then after substituting in (1)-(3) and dividing both sides by s_A^2:
Equating first and last pairs of these expressions:
Equation (6) may be used to express u^2 thusly:
Substituting this in equation (7) gives u as a rational expression in v:
where t = (a^2 - c^2)/b^2. If this expression for u is substituted back into (6), clearing denominators gives a fourth degree polynomial equation in v:
Any positive real root for v in (9) may be substituted in (8) to give u and in (5b) to give s_A. Accordingly from (4) we find s_B and s_C. Although solving quartic polynomials in closed form is a possibility (Cardano/Ferrari,1545), such direct methods involve complex arithmetic (even for finding real roots) and a large number of steps. Accordingly an indirect method would be a better match to your project needs, a topic we can discuss later in conjunction with other implementation details. (See caveat below about "singular" configurations.) Note that for our specific coordinates of A,B,C, we have a = r sqrt(2) and b = c = r. With this choice (6) and (7) both describe hyperbolas, and the constant t = 1, reducing the numerator in (8) to first degree. Nevertheless the resulting polynomial equation (9) is still quartic. Specializing the discussion to points A,B,C being vertices of the isoceles right triangle (0,0,0),(r,0,0),(0,r,0) gives a^2 = 2r^2 and b^2 = c^2 = r^2. Thus (8) becomes u = (µ_b v - 1)/(µ_a v - µ_c), which in (6) yields for (9):
One caveat affects both the general and specialized problems. If µ_a v = µ_c, then our expression (8) for u involves division by zero. When the quantities are nearly equal, cancellation affects the error magnitude. So we need to beware "singular" configurations in which unknown v = s_C/s_A approximates the known µ_c/µ_a. Ranging Problem =============== For each positive real solution s_A,s_B,s_C there exists a candidate M. That is, the distances constrain M to the intersection of three spheres centered on A,B,C respectively. Two spheres intersecting in more than one point intersect in a circle, and such circle lies in a plane whose equation is quickly deduced from the equations of the spheres. Indeed consider the intersection of the sphere of radius s_A centered on A with the sphere of radius s_B centered on B. The plane containing their circle of intersection will be perpendicular to line AB, and the circle will be centered somewhere along line AB (not necessarily at a point between A and B). Likewise the spheres centered on A and on C will intersect (if in more than one point) in a circle lying in a plane perpendicular to line AC. M must lie in both planes, which are not parallel, so that two of M's coordinates can be expressed parametrically in terms of the third. Intersecting that line with any of the three spheres gives two possible points M, symmetric with respect to the plane containing ABC. In this case our specific choice of A,B,C makes that computation especially easy. A is the origin and B is (r,0,0). Equations for the respective spheres are:
Subtracting the latter from the former gives the plane:
which implies the value of x_M:
Likewise the intersection of spheres centered on A and C implies:
As we presume that z_M > 0, our final coordinate is:
Distinguish the correct location M ================================== Finally for each candidate M we check its angles involving point D. If it is correct, the dot products of (M-D) and (M-A) should be the product of lengths s_A times s_D times the cosine of the angle at M subtended by AD, where s_D is the distance from M to D). With four coplanar points A,B,C,D there should only be two solutions for M, symmetric with respect to the plane containing ABDC. The constraint z_M > 0 distinguishes the two possibilities in our case. regards, mathtalk
Answer clarification by Researcher mathtalk on Mon 18 Jan 2010 - 2:49 pm UTC:
"In troth, Sirs, I shall not hold you much longer, for I will only say thus to you. That in truth I could have desired some little time longer, because I would have put then that I have said in a little more order, and a little better digested than I have done. And, therefore, I hope that you will excuse me." - from remarks by King Charles I, Jan. 30, 1648, on the occasion of his beheading ERRATUM ======= There's nothing better than a worked example to discover errors in one's formulas! Here I have made a sign error in the coefficient K_3 above, which should have been:The example I writing up is for an isoceles right triangle with r = 1 and:
which I hope will illustrate Grunert's method and the pitfalls of measurement errors for the angles. I hope there are no further errors of formulation to be discovered, but we may see... regards, mathtalk
Answer clarification by Researcher mathtalk on Wed 20 Jan 2010 - 2:30 pm UTC:
Yes, there are better ways to tackle the roots of the quartic. The quadratic formula is quite practical, and I've done a Python implementation of the solution by radicals for a cubic (Cardano): [Q: for mathtalk-ga only: Need more help with cubic equations] http://answers.google.com/answers/threadview/id/441538.html but I've never tried to do Ferrari's formula for the quartic. I believe that it would require Cardano's method as a subroutine to solve the auxiliary cubic. My thought is to tackle the positive roots of the quartic by numerical approximation. The general approach is a "divide-and-conquer" technique of forming intervals that isolate the positive roots, then using some "standard" root finding methods like bisection/secant/Newton/Laguerre. For isolating the roots of a polynomial it can be observed that between two real roots there must be a root of its derivative. That is, if f(z) = f(w) = 0 for z < w, there must exist t strictly between z and w s.t. f'(t) = 0 by the Mean Value Theorem. Thus the roots of f' separate the roots of f, and the roots of f" separate the roots of f', when the roots are all distinct. Now since f" will be a quadratic in our case, it is easily solved. The cubic f' must have at least one real root, and if f has four distinct real roots then f' will actually have three real roots by the above. Once one of the real roots of f' is found, it can be factored out (deflation) leaving a quadratic polynomial to solve. While deflation can introduce some rounding errors in the remaining roots, that is not a terrible thing here because we want the roots of f' only for producing the isolating intervals for roots of f, and modest inaccuracy of f' roots will not change that result. The implementation of root finding methods, even for (general) polynomials, is a tricky topic that has been picked over by many bright people before us. So we should try to take advantage of their work and only reinvent the wheel if necessary. I'll look first at the Quartic Solver written in C by Andrew Steiner linked here (GPL licensing): [GSL - GNU Scientific Library] http://www.gnu.org/software/gsl/ Look for the Quartic link about two-thirds of the way down the page. regards, mathtalk
Answer clarification by Researcher mathtalk on Wed 20 Jan 2010 - 11:51 pm UTC:
At a glance the code linked above contains two C programs, solve_quartic.c and zsolve_quartic.c. The latter finds all roots, where the former is "simplified" to finding only real roots of a quartic polynomial. So solve_quartic.c definitely seems to be worth a closer look and a bit of testing on my example problem. regards, mathtalk
Comment by eldenc on Sun 24 Jan 2010 - 9:47 am UTC:
My code no worky :-(
All of the point collections given to calulatePostionsFromAngles(...)
should have returned x, y, and z values that are greater than 1 since they
are all greater than (about 5x) the size of r (r=AB=AC).
Did you say that you had worked an example?
If you have the intermediate answers to that example, I would like to plug
them into the code below to see if my error is in the transformation that I
am feeding the angles into the function, or I mis-adapted your math into
the C-code. (If you wish to execute my code, then see the notes near the
top of the file.) PS. To give you credit in the code that I eventually
check-in as something more exacting than mathtalk, then send me an email.
A sample of it's out put is:
=============================================
For points:
A=(211,369)
B=(70,396)
C=(292,481)
D=(155,502)
Given the angles about M
a_radians=0.340091,b_radians=0.197747,c_radians=0.205388
(u_a=0.942724), (u_b=0.980512),
(u_c=0.978982)
the quartic problem: v**4 + -4.03794v**3 + 6.02756v**2 +
-4.09991v**1 + 1.0316 = 0
numsolutions=2, 1234=0.597214,1.70468,0,0
v[0]=0.597214 u[0]=0.996276
v[1]=1.70468 u[1]=1.0691
(sa,sb,sc)[0]=(0.227407,0.22656,0.135811)
(sa,sb,sc)[1]=(0.612728,0.655066,1.0445)
M(x,y,z)[0]=(1.00085,1.0916,nan)
M(x,y,z)[1]=(0.957662,0.568224,nan)
M=(0,0,0)
=====================================================
=============================================================
/*
* $Id: whereAmI.c 3326 2010-01-23 08:05:29Z eldenc $
*
* Copyright (C) 2010 Elden Crom, uclue.com mathtalk
*
* This file is (will be) part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
//////////////////////////////////////////////////////////////////////////
// for my purposes I pulled in gsl_poly_solve_quartic
// and gsl_poly_solve_quadratic from
// http://www.network-theory.co.uk/download/gslextras/Quartic/
// and
// cvs -d :pserver:anonymous@pserver.git.sv.gnu.org:/gsl.git checkout -d
gsl HEAD
// respectively, removed the includes except
// #include <math.h> and added
// #define GSL_MAX(a,b) ((a) > (b) ? (a) : (b))
//
// The compile via
// gcc whereAmI.c solve_quartic.c solve_quadratic.c -lm
//////////////////////////////////////////////////////////////////////////
#include <stdio.h>
#include <math.h>
/* solve_quartic.c - finds the real roots of
* x^4 + a x^3 + b x^2 + c x + d = 0
*/
int
gsl_poly_solve_quartic (double a, double b, double c, double d,
double *x0, double *x1, double *x2, double *x3);
#define TEST_WHEREAMI
#ifdef TEST_WHEREAMI
#define dprintf(args...) printf(args)
#define dprintf_loop(loop_max,args...) {unsigned int
i=0;for(i=0;i<loop_max;i++){printf(args);}}
int testTolf64(const char *msg,double act,double exp, double
tol_pct);
void report();
#else
#define dprintf(args...) ;
#endif //TEST_WHEREAMI
//////////////////////////////////////////////////////////////////////////////
// The Function preforms "direct spatial resection" to
// transform the angles into x,y,z co-ordinates of an unknown point M.
// The units of x, y, and z are r.
//
//Geometry: Find a 3d location based on 6 angles and 1 length
//(4PointsAndACamera)
//
//Let A, B, C, and D be four points on the xy plane in a 3 Dimensional
//Cartesian space.
//Let M represent a point of unknown position in the positive xyz
quadrant.
//
//Given that:
//
// A is as the origin (0,0,0)
// B is on the x axis (r,0,0)
// C is on the y axis (0,r,0)
// where r > 0
//
// The angles AMB, AMC, and BMC are supplied.
//
// Looking strait down the Z-axis A,B and C appear as
// A----C--- positive Y-Axis
// |
// |
// B
// |
// positive X-Axis
//
//
// Note: In most applications the user will scale each co-ordinate by r
// in order to obtain the real world units for M(x,y,z).
//
//The function returns upto 4 possible solutions into
//the arrays aMx[4],double aMy[4],double aMz[4]
// @param a_radians --- the angle
// @return the number of possible solutions
int calulatePostionsFromAngles(double a_radians ,double b_radians
,double c_radians,
double *aMx, double *aMy, double *aMz){
double u_a=cos(a_radians);
double u_b=cos(b_radians);
double u_c=cos(c_radians);
dprintf(" Given the angles about M
a_radians=%g,b_radians=%g,c_radians=%g\n",a_radians,b_radians,c_radians);
dprintf(" (u_a=%g), (u_b=%g),
(u_c=%g)\n",u_a,u_b,u_c);
double k0,k1,k2,k3,k4;
//from http://uclue.com/?xq=3601&form=comment#form by mathtalk
//These coefficients correspond to A=(0
k0=2.0*u_c*u_c-1.0;
k1=-2.0*(u_a*u_c+u_b*(2.0*u_c*u_c-1.0));
k2=u_a*u_a+6.0*u_a*u_b*u_c-u_b*u_b;
k3=-2.0*u_a*(u_c+u_a*u_b);//the corrected one
k4=u_a*u_a;
double a,b,c,d;
a= k3/k4; //gsl_poly_solve_quartic expects the coefficient of x**4 to
be 1
b= k2/k4; //so divide each by k4
c= k1/k4;
d= k0/k4;
double v[4]={0,0,0,0};
int numsolutions;
//gsl_poly_solve_quartic expects x^4 + a x^3 + b x^2 + c x + d = 0
numsolutions=gsl_poly_solve_quartic(a,b,c,d,&v[0],&v[1],&v[2],&v[3]);
dprintf(" the quartic problem: v**4 + %gv**3 + %gv**2 +
%gv**1 + %g = 0\n",a,b,c,d);
dprintf(" numsolutions=%d,
1234=%g,%g,%g,%g\n",numsolutions,v[0],v[1],v[2],v[3]);
//ps these agree with a quartic web calculator I found at
// http://www.freewebs.com/brianjs/ultimateequationsolver.htm
//Now that we have solved the quartic determine the lengths of the
sides
double u[4]={0,0,0,0};
//double t;
//double r=1;
////A B AB=r;AC=r;CB=sqrt(AB*AB+AC*AC)
////
////C
//b=r; //AC
//c=r; //AB
//a=sqrt(c*c+b*b); //BC
//t=(a*a-c*c)/(b*b); Our Special Case t=(r**2+r**2-r**2)/r**2=1
int i;
for(i=0;i<numsolutions;i++){
//Equation 8 from http://uclue.com/?xq=3601&form=comment#form by
mathtalk
//u[i]=( (t-1.0)*v[i]*v[i] - 2.0*t*u_b*v[i] + (t+1.0) ) /
// (2.0*u_c-u_a*v[i]);
//Our special case (t=1)
u[i]=( u_b*v[i] - 1.0 ) / ( u_a*v[i]-u_c );
}
dprintf_loop(numsolutions," v[%d]=%g
u[%d]=%g\n",i,v[i],i,u[i]);
double sa[4]={0,0,0,0},
sb[4]={0,0,0,0},
sc[4]={0,0,0,0};
for(i=0;i<numsolutions;i++){
//Equations 4 and 5 from
http://uclue.com/?xq=3601&form=comment#form by mathtalk
//sa[i]=( u[i]*u[i]+v[i]*v[i] - 2.0*u_a*u[i]*v[i] ) /
// a*a;
//specialized for our case a*a=1
sa[i]=( u[i]*u[i]+v[i]*v[i] - 2.0*u_a*u[i]*v[i] );
sb[i]=u[i]*sa[i];
sc[i]=v[i]*sa[i];
}
//print out the lengths of the sides
dprintf_loop(numsolutions,"
(sa,sb,sc)[%d]=(%g,%g,%g)\n",i,sa[i],sb[i],sc[i]);
/////now we have up to 4 solutions for the lengths of MA,MB,MC
(sa,sb,sc respectively)
double Mx[4]={0,0,0,0},
My[4]={0,0,0,0},
Mz[4]={0,0,0,0};
for(i=0;i<numsolutions;i++){
//Equations 10 and 11 from
http://uclue.com/?xq=3601&form=comment#form by mathtalk
//Mx[i]=( sa[i]-sb[i]+r*r ) / (2*r);
//My[i]=( sa[i]-sc[i]+r*r ) / (2*r);
//specialized for our case (r=1)
Mx[i]=( sa[i]-sb[i]+1 );
My[i]=( sa[i]-sc[i]+1 );
Mz[i]=sqrt(sa[i]*sa[i]-Mx[i]*Mx[i]-My[i]*My[i]);
}
//print out the results
dprintf_loop(numsolutions,"
M(x,y,z)[%d]=(%g,%g,%g)\n",i,Mx[i],My[i],Mz[i]);
return numsolutions;
}
double distance(int ax, int ay, int bx, int by){
int x=ax-bx;
int y=ay-by;
return sqrt(x*x+y*y);
}
int calcLocationM(int ax,int ay,
int bx,int by,
int cx,int cy,
int dx,int dy,
double *Mx,double *My,double *Mz){
dprintf("For points:\n A=(%d,%d)\n B=(%d,%d)\n C=(%d,%d)\n
D=(%d,%d)\n",ax,ay,bx,by,cx,cy,dx,dy);
//this is specific to the Wii Mote Camera
double radians_per_pixel=0.00143066; //atan(12.5 cm /54.0 cm)/159
pixels
//angles
double a_radians=distance(bx,by,cx,cy)*radians_per_pixel; //the angle
subtended by bc from 3d point M
double b_radians=distance(ax,ay,cx,cy)*radians_per_pixel;
double c_radians=distance(ax,ay,bx,by)*radians_per_pixel;
//the full complement of results (all but one are invalid)
double aMx[4],aMy[4],aMz[4];
int i;
for(i=0;i<4;i++) {aMx[i]=aMy[i]=aMz[i]=0;}
int
numRes=calulatePostionsFromAngles(a_radians,b_radians,c_radians,aMx,aMy,aMz);
//////// Distinguish the correct location M from the possible
solutions
double ud=distance(ax,ay,bx,by)*radians_per_pixel;
//int i;
//int found1;
//for(i=0;i<numRes;i++){
// ????????????????????????????????????????
// //if(aMz <= 0) continue;//redundant...with sqrt that z was
caculated for
// //So I need to do calulatePostionsFromAngles() again to determine
s_D?
//}
*Mx=aMx[0];
*My=aMy[0];
*Mz=aMz[0];
dprintf("M=(%g,%g,%g)\n\n",*Mx,*My,*Mz);
}
#ifdef TEST_WHEREAMI
struct TestCases2dPoints {
int ax,ay,
bx,by,
cx,cy,
dx,dy;
double Mx,My,Mz;
};
int main(int argc,char**argv){
struct TestCases2dPoints tc[]={
// a b c d
Mx,My,Mz (r=5.5 in)
{485.000,607.000, 344.000,661.000, 617.000,690.000, 472.000,736.000,
/*endpts*/5.078,5.078,3.273},// x=y R=39.5,z=18
//////Roll, pitch, heading change same pos
{656.000,520.000, 548.000,600.000, 780.000,588.000, 667.000,662.000,
/*endpts*/5.206,5.206,5.273},// x=y R=40.5,z=29
{656.000,520.000, 548.000,600.000, 780.000,588.000, 667.000,662.000,
/*endpts*/5.206,5.206,5.273},// x=y R=40.5,z=29
{547.000,363.000, 434.000,438.000, 662.000,438.000, 546.000,505.000,
/*endpts*/5.206,5.206,5.273},// x=y R=40.5,z=29 (same diff pitch)
{522.000,304.000, 391.000,323.000, 593.000,419.000, 461.000,428.000,
/*endpts*/5.206,5.206,5.273},// x=y R=40.5,z=29 (same diff roll)
{515.000,412.000, 394.000,473.000, 620.000,500.000, 499.000,552.000,
/*endpts*/5.206,5.206,5.273},// x=y R=40.5,z=29 (remove ruler)
{803.000,539.000, 694.000,627.000, 934.000,604.000, 819.000,683.000,
/*endpts*/5.206,5.206,5.273},//heading
{211.000,369.000, 70.000,396.000, 292.000,481.000, 155.000,502.000,
/*endpts*/5.206,5.206,5.273} //heading
};
int numRes,i;
for(i=0;i<sizeof(tc)/sizeof(struct TestCases2dPoints);i++){
double Mx,My,Mz;
numRes=calcLocationM(tc[i].ax,tc[i].ay,
tc[i].bx,tc[i].by,
tc[i].cx,tc[i].cy,
tc[i].dx,tc[i].dy,
&Mx,&My,&Mz);
//testTolf64("Check value of Mx",Mx,tc[i].Mx,0.5);
//testTolf64("Check value of My",Mx,tc[i].My,0.5);
//testTolf64("Check value of Mz",Mx,tc[i].Mz,0.5);
}
report();
}
//struct TestCasesAngles {
// double a_radians,b_radians,c_radians;
// double Mx[4],My[4],Mz[4];
//};
//int main(int argc,char**argv){
// struct TestCasesAngles tc[]={{ 1.000, 2.000, 3.000,{},{},{} },
// { 1.000, 2.000, 3.000,{},{},{} }
// };
// double aMx[4],aMy[4],aMz[4];
// int i;
// for(i=0;i<4;i++) {aMx[i]=aMy[i]=aMz[i]=0;}
// int numRes,i;
// for(i=0;i<sizeof(tc)/sizeof(struct TestCasesAngles);i++){
//
numRes=calulatePostionsFromAngles(tc[i].a_radians,tc[i].b_radians,tc[i].c_radians
// aMx,aMy,aMz);
// }
//}
////////////////////////////////The test harness
int num_tests=0;
int num_pass=0;
int num_fail=0;
int testTolf64(const char *msg,double act,double exp, double tol_pct){
double delta=exp-act;
double tol = abs((tol_pct/100.0)*exp);
int passed=0;
num_tests++;
if (fabs(delta) < tol){
num_pass++;
passed = 1;
printf("PASSED ");
} else {
num_fail++;
printf("FAILED ");
}
printf("%s\n\t actual=%f\texpected=%f\ttolerance:
+/-%f\n",msg,act,exp,tol);
printf("\t\t delta = %f\n",delta);
return passed;
}
void report(){
printf(" summary:\n");
printf(" # of tests complete = %d\n",(int)num_tests);
printf(" # of tests failed = %d\n",(int)num_fail );
printf(" # of tests passes = %d\n",(int)num_pass );
if(num_pass > 0 && num_pass==num_tests){
printf("OverAll results for test: PASSED -- yeah\n");
}else{
printf("OverAll results for test: FAILED\n");
}
num_tests=num_pass=num_fail=0;//zeroing test variables
}
#endif //TEST_WHEREAMI
Answer clarification by Researcher mathtalk on Sun 24 Jan 2010 - 3:22 pm UTC:
The first thing I see is that A,B,C,D no longer form a square. The quartic formula I gave for v was specialized to the case when A,B,C are (0,0,0),(r,0,0),(0,r,0). [D hasn't entered into the problem formulation at that stage.] I can give you the quartic for three general points A,B,C, and you will see that the coefficients depend on the side lengths c=length(AB), b=length(AC), a=length(BC). This dependence "disappeared" in the expressions for K_i, i=0,..4 above because I'd plugged in their values in terms of r, and in fact the dependence is only on the ratios of (squares of) the lengths a,b,c. The algebra is daunting enough even in the simplified case that I needed Maxima (the open source CAS) to keep me straight, and even then I failed to catch one sign error. So I'll definitely have to rely on a symbolic calculator for the general equation. Here is my example and its intermediate results: When r = 1 and M = (1/4,3/4,4), the cosines of angles are: µ_a = cos(Angle BMC) = (M-B).(M-C)/(|MB|*|MC|) = 0.940274968 µ_b = cos(Angle AMC) = (M-A).(M-C)/(|MA|*|MC|) = 0.969578653 µ_c = cos(Angle AMB) = (M-A).(M-B)/(|MA|*|MB|) = 0.940274968 and consequently my quartic has coefficients: K_4 = 0.884117015 K_3 = -3.539472195 K_2 = 5.310273647 K_1 = -3.538568001 K_0 = 0.883650733 There are four distinct real roots, all reasonably close to 1. The root v we are interested in is v = 0.984847609, which corresponds by (8) to: u = 1.014926198 and thus by (5b) and (4) to: s_A = 4.077376608 s_B = 4.138236339 s_C = 4.015594601 The coordinates of M can then be computed from (10),(11),(12): x_M = 0.25 y_M = 0.75 z_M = 4.00 regards, mathtalk
Answer clarification by Researcher mathtalk on Sun 24 Jan 2010 - 4:15 pm UTC:
Let me describe my own "test harness" a bit further, since I don't find C programming "agile" enough for exploratory work. I used an Excel spreadsheet for most formulas (OpenOffice Calc would be equal to the task) and to solve the quartic equations: [Quartic Root Calculator by Stephen R. Schmitt] http://home.att.net/~srschmitt/script_quartic.html with scripting enabled and the "No rounding" option/radio button. Mr. Schmitt's Javascript code, which is visible in a browser through View|Source, uses the Lin-Bairstow algorithm starting with randomized guesses. Because of the nature of the "resection" application, it is not coincidental that v = s_C/s_A would be close to 1. If the distance of M from the three points A,B,C is large in comparison to the intra-triangle distances a,b,c, then ratios u and v will naturally be about 1. My point is that we "know" enough about the roots of the quartic to be able to avoid randomized guesses which might lead to occasional divergence in root finding. However with an educated guess about roots, the Lin-Bairstow algorithm would be an attractive algorithm. Essentially it would use Newton-Raphson to factor (in real arithmetic) the quartic into a pair of (easily solved) quadratic equations. regards, mathtalk
Comment by eldenc on Sun 24 Jan 2010 - 8:20 pm UTC:
I think the problem happens at the calculations of the K values...
I stripped down the C code to just calculate the K values (below) and this
is what I get:
Given the angles about M a_radians= 0.347359179000,b_radians=
0.247292756000,c_radians= 0.347359179000
u_a= 0.940274967965
u_b= 0.969578653117
u_c= 0.940274967965
k0= 0.768234030764
k1= -3.257960664417
k2= 5.087360160632
k3= -3.482676000707
k4= 0.884117015382
k4 agrees but the rest do not.
Is your applications order of precedence not C/Algebra like?
(times before +/-?)
Can you copy and paste the formulas for k... in plain text?
=======================================================
//File justKvals.c
// compile with 'gcc justKvals.c -lm'
//
#include <stdio.h>
#include <math.h>
#define dprintf(args...) printf(args)
int calulatePostionsFromAngles(double a_radians
,double b_radians
,double c_radians
,double *aMx, double *aMy, double *aMz){
double u_a=cos(a_radians);
double u_b=cos(b_radians);
double u_c=cos(c_radians);
#define GG "%16.12f"
dprintf(" Given the angles about M a_radians="GG",b_radians="GG","
"c_radians="GG"\n",a_radians,b_radians,c_radians);
double k0,k1,k2,k3,k4;
//from http://uclue.com/?xq=3601&form=comment#form by mathtalk
dprintf(" u_a="GG"\n",u_a);
dprintf(" u_b="GG"\n",u_b);
dprintf(" u_c="GG"\n",u_c);
k0=2.0*u_c*u_c-1.0;
k1=-2.0*(u_a*u_c+u_b*(2.0*u_c*u_c-1.0));
k2=u_a*u_a+6.0*u_a*u_b*u_c-u_b*u_b;
k3=-2.0*u_a*(u_c+u_a*u_b);//the corrected one
k4=u_a*u_a;
dprintf(" k0="GG"\n",k0);
dprintf(" k1="GG"\n",k1);
dprintf(" k2="GG"\n",k2);
dprintf(" k3="GG"\n",k3);
dprintf(" k4="GG"\n",k4);
}
struct TestCasesAngles {
double a_radians,b_radians,c_radians;
double Mx[4],My[4],Mz[4];
};
int main(int argc,char**argv){
struct TestCasesAngles tc[]={{ 0.347359179,/*u_a=0.940274968*/
0.247292756,/*u_b=0.969578653*/
0.347359179,/*u_c=0.940274968*/
{},{},{} }
//,{ 1.000, 2.000, 3.000,{},{},{} }
};
double aMx[4],aMy[4],aMz[4];
int i,k,numRes;
for(i=0;i<sizeof(tc)/sizeof(struct TestCasesAngles);i++){
for(k=0;k<4;k++) {aMx[k]=aMy[k]=aMz[k]=0;}
numRes=calulatePostionsFromAngles(tc[i].a_radians,
tc[i].b_radians,
tc[i].c_radians,
aMx,aMy,aMz);
}
}
Answer clarification by Researcher mathtalk on Sun 24 Jan 2010 - 9:42 pm UTC:
Sorry, I made a cut-and-paste error when I gave you µ_c. It should have not have been identical to µ_a, but instead: mu_c = 0.970476876 I think that explains the problem. It certainly explains the difference in values for K_0. Here are some plaintext (C syntax?) formulas for the coefficients: K_0 = 2*mu_c*mu_c - 1 K_1 = -2*(mu_a*mu_c + mu_b*(2*mu_c*mu_c - 1)) K_2 = mu_a*mu_a + 6*mu_a*mu_b*mu_c - mu_b*mu_b K_3 = -2*mu_a*(mu_c + mu_a*mu_b) K_4 = mu_a*mu_a This includes the sign correction to K_3 posted earlier. regards, mathtalk
Answer clarification by Researcher mathtalk on Sun 24 Jan 2010 - 9:48 pm UTC:
I checked the formulas and they match your code. -mt
Comment by eldenc on Sun 24 Jan 2010 - 10:56 pm UTC:
getting closer but no cigar yet....
your examples k2 (K_2 = 5.310273647) is off in the second significant
digit wrt mine (k2 = 5.252565299146). Considering the u_a, u_b and u_c
are the same out to the 8 significant this seems odd.
k2 = u_a* u_a + 6.0* u_a* u_b* u_c - u_b* u_b; //mine (added spaces)
K_2 = mu_a*mu_a + 6 *mu_a*mu_b*mu_c - mu_b*mu_b //yours (added spaces)
Given the angles about M a_radians= 0.347359179000,b_radians=
0.247292756000,c_radians= 0.243596173000
...Force the angles to the example..
u_a= 0.940274968000
u_b= 0.969578653000
u_c= 0.970476876000
k0= 0.883650733701
k1= -3.538568003261
k2= 5.252565298780
k3= -3.539472196915
k4= 0.884117015447
Answer clarification by Researcher mathtalk on Sun 24 Jan 2010 - 11:18 pm UTC:
You are right about that! The value for K_2 should be 5.252565295. My Excel spreadsheet formula pointed to the wrong cell for the first square in that expression, getting µ_c^ instead of µ_a^2 for the somewhat larger result. I'll redo my quartic solution next as this must change roots v somewhat. -mt
Comment by eldenc on Mon 25 Jan 2010 - 1:06 am UTC:
Currently, mine app says....
=======================================================
Given the angles about M a_radians= 0.347359178898,b_radians=
0.247292756478,c_radians= 0.243596173163
(u_a= 0.940274968000), (u_b=
0.969578653000), (u_c= 0.970476876000)
u_a= 0.940274968000
u_b= 0.969578653000
u_c= 0.970476876000
k0= 0.883650733701
k1= -3.538568003261
k2= 5.252565298780
k3= -3.539472196915
k4= 0.884117015447
the quartic problem: v**4 + -4.003397893122v**3 +
5.941029532298v**2 + -4.002375184997v**1 + 0.999472601774 = 0
numsolutions=2, v[0..4]= 0.605130619212, 1.651992830452,
0.000000000000, 0.000000000000
v[0]= 0.605130619212 u[0]= 1.029367193268
v[1]= 1.651992830452 u[1]= 1.032403419608
(sa,sb,sc)[0]=( 0.254382307596, 0.261852801987, 0.153934523312)
(sa,sb,sc)[1]=( 0.587615875187, 0.606656638959, 0.970737212868)
M(x,y,z)[0]=( 0.992529505609, 1.100447784284, nan)
M(x,y,z)[1]=( 0.980959236228, 0.616878662318, nan)
=============================================================
The roots agree with those of
http://www.freewebs.com/brianjs/ultimateequationsolver.htm
Answer clarification by Researcher mathtalk on Mon 25 Jan 2010 - 2:40 am UTC:
Okay, I checked my handwritten notes (copied from Maxima on my Linux box) and the coefficient K_2 does have mu_c^2, not mu_a^2 as the leading term: K_2 = mu_c*mu_c + 6*mu_a*mu_b*mu_c - mu_b*mu_b So in the case of the given angles, K_2 = 5.310273647, and the quartic was correct in having these four real roots: 1.054650618676 1.026262454132 0.984848883367 0.937635936807 Of these the actual v = 0.984848883367 corresponds to M = (0.25,0.75,4.0). regards, mt
Comment by eldenc on Mon 25 Jan 2010 - 4:48 am UTC:
Yeah, we're close and after fixing a few issues in my code one of the solutions looks pretty good: M(x,y,z)[1]=( 0.249999787814, 0.750000060576, 3.999999981443) now I can kick out M[3] easily (negative length), but I don't know the length of s_D (it has 4, well 3, possible lengths) so I don't know how to apply "Distinguish the correct location M" PS I'm putting a tar file of my current code at http://eldenc.tripod.com/resection4knowPointsAndAngles.20100124b.tar.gz (The python's not done yet, so no visuals using a WiiMote yet) ======================= a.out output ================================== Given the angles about M a_radians= 0.347359178898,b_radians= 0.247292756478,c_radians= 0.243596173163 (u_a= 0.940274968000), (u_b= 0.969578653000), (u_c= 0.970476876000) u_a= 0.940274968000 u_b= 0.969578653000 u_c= 0.970476876000 k0= 0.883650733701 k1= -3.538568003261 k2= 5.310273650183 k3= -3.539472196915 k4= 0.884117015447 the quartic problem: v**4 + -4.003397893122v**3 + 6.006301832678v**2 + -4.002375184997v**1 + 0.999472601774 = 0 numsolutions=4, v[0..4]= 0.937636277934, 0.984847604681, 1.026264197407, 1.054649813100 v[0]= 0.937636277934 u[0]= 1.023040340709 v[1]= 0.984847604681 u[1]= 1.014926211009 v[2]= 1.026264197407 u[2]= 0.900079053377 v[3]= 1.054649813100 u[3]= 1.065238180219 (sa,sb,sc)[0]=( 4.050955774111, 4.144291175346, 3.798323094112) (sa,sb,sc)[1]=( 4.077376587503, 4.138236370809, 4.015594565586) (sa,sb,sc)[2]=( 3.979975802405, 3.582292852693, 4.084506672553) (sa,sb,sc)[3]=( 3.858895022036, 4.110642310930, 4.069782913762) M(x,y,z)[0]=( 0.117546668879, 1.491492178271, 3.764555292010) M(x,y,z)[1]=( 0.249999787814, 0.750000060576, 3.999999981443) M(x,y,z)[2]=( 2.003692652636, 0.078506314801, 3.437915051312) M(x,y,z)[3]=( -0.503154708657, -0.336031087029, 3.811166388233) =================================================================
Answer clarification by Researcher mathtalk on Mon 25 Jan 2010 - 3:01 pm UTC:
Distinguishing the "correct" M is based on knowledge of the angles at M
subtended by AD,BD,CD (the three angles not used so far). Ordinarily we
will only need one of those angles to tell which of the candidate locations
is right.
So for now let's just test the angle AMD, or its proxy the cosine:
cos(Angle AMD) = (M-A).(M-D)/(|M-A|*|M-D|)
for simplicity as A is the origin and D is (1,1,0). Note M-D has the same
length as M-A with M = (0.25,0.75,4.0):
cos(Angle AMD) = (0.25*(0.25-1) + 0.75*(0.75-1) + 4*4)/SQRT(16.625*16.625)
= 0.939849624
As before the locus of points M which make this angle form a "toroidal"
surface around the axis AD. If by coincidence more than one candidate
seems to meet the mark, we still have angles BMD and CMD to fall back on.
regards, mt
Comment by eldenc on Mon 25 Jan 2010 - 5:07 pm UTC:
In your example case, what are the angles Angle AMD, Angle BMD, Angle CMD?
Answer clarification by Researcher mathtalk on Mon 25 Jan 2010 - 5:39 pm UTC:
I did a quick calculation based on the vector cosine formula: angle | cosine | radians ------------------------------------ AMD | 0.939849624 | 0.348606514 BMD | 0.970476876 | 0.243596174 CMD | 0.969578653 | 0.247292755 Hope that helps!
Answer clarification by Researcher mathtalk on Mon 25 Jan 2010 - 7:39 pm UTC:
Too quick! I found a mistake in my formulas. Here are the corrected respective cosines and angles for all four roots as found by the Web page you linked above. I suspect it's helpful to see the effects of the errors introduced: v = 1.054650618676, M = (0.276074578,-0.336042042,3.834305562) angle | cosine | radians ----------------------------------- AMD | 0.939384396 | 0.349965926 BMD | 0.970385078 | 0.243976478 CMD | 0.970301465 | 0.244322374 v = 1.026262454132, M = (0.261800644,0.07853333,3.970585612) angle | cosine | radians ----------------------------------- AMD | 0.94014532 | 0.347739831 BMD | 0.970435765 | 0.243766562 CMD | 0.970547765 | 0.243302091 v = 0.984848883367, M = (0.250000162,0.749978965,4.000002589) angle | cosine | radians ----------------------------------- AMD | 0.939849661 | 0.348606407 BMD | 0.970476875 | 0.243596177 CMD | 0.969578711 | 0.24729252 v = 0.937635936807, M = (0.253229519,1.491497084,3.757864854) angle | cosine | radians ----------------------------------- AMD | 0.937225156 | 0.356210179 BMD | 0.970463502 | 0.243651616 CMD | 0.966062773 | 0.261269744 We should compare these with the "exact" angles: angle | cosine | radians ----------------------------------- AMD | 0.939849624 | 0.348606514 BMD | 0.970476876 | 0.243596174 CMD | 0.969578653 | 0.247292755 based on: v = 0.984847609, M = (0.25,0.75,4.0). Note that this "real" v differs from the closest root above by ~10^-6. As a result the agreement in the angles AMD,BMD,CMD between the calculated and "real" values is roughly proportional, while disagreements with "false" roots begins by at least the fourth decimal place for any of the three angles chosen (angle BMD having the smallest differences to detect). regards, mathtalk
Answer clarification by Researcher mathtalk on Mon 25 Jan 2010 - 8:10 pm UTC:
Actually I found one more mistake in my formulas, related to what I thought I fixed last, but it will be a few hours before I have time to recalc. Basically one of my spreadsheet formulas was linking back to an "exact" value of u, rather than using the u based on calculated root v. This would have affected side length s_B and through it the x-coordinate, probably reducing the actual variability with different roots. regards, mt
Comment by eldenc on Mon 25 Jan 2010 - 9:26 pm UTC:
success on the example, points,
Since I know there will be error in the results, I'm choosing the one with
the least err. I calculate the error against all 3 D angles and sum each
absolute error together; then pick the one with the smallest error:
===========================================================
int findBest(double *aMx,double *aMy,double *aMz,int numRes,
double AMD_radians,double BMD_radians,double CMD_radians){
int i;
#define VEC_MINUS(RES,Q,P)
{RES.x=Q.x-P.x;RES.y=Q.y-P.y;RES.z=Q.z-P.z;}
#define VEC_DOT(Q,P) (Q.x*P.x+Q.y*P.y+Q.z*P.z)
#define VEC_MAGSQ(Q) (Q.x*Q.x+Q.y*Q.y+Q.z*Q.z)
//#define VEC_MAG(Q) sqrt(Q.x*Q.x+Q.y*Q.y+Q.z*Q.z)
struct Point3d {double x,y,z;};
struct Point3d a={0,0,0};
struct Point3d b={1,0,0};
struct Point3d c={0,1,0};
struct Point3d d={1,1,0};
double cAMD=cos(AMD_radians);
double cBMD=cos(BMD_radians);
double cCMD=cos(CMD_radians);
double err_min=1.7976931348623158e+308; //max double
int best_yet=-1;
for(i=0;i<numRes;i++){
if(aMx[i] < 0 || aMy[i]<0 || aMz[i] < 0)
continue;
struct Point3d m={aMx[i],aMy[i],aMz[i]};
struct Point3d mLessD;
VEC_MINUS(mLessD,m,d);//mLessD
struct Point3d mLessW;//where W is a,b,c
VEC_MINUS(mLessW,m,a);
double error_cAMD = cAMD - VEC_DOT(mLessW,mLessD) /
sqrt(VEC_MAGSQ(mLessW)*VEC_MAGSQ(mLessD));
VEC_MINUS(mLessW,m,b);
double error_cBMD = cBMD - VEC_DOT(mLessW,mLessD) /
sqrt(VEC_MAGSQ(mLessW)*VEC_MAGSQ(mLessD));
VEC_MINUS(mLessW,m,c);
double error_cCMD = cCMD - VEC_DOT(mLessW,mLessD) /
sqrt(VEC_MAGSQ(mLessW)*VEC_MAGSQ(mLessD));
dprintf1(" M=(%g,%g,%g)\n",aMx[i],aMy[i],aMz[i]);
dprintf1(" error_cBMD "GG"\n",error_cBMD);
dprintf1(" error_cAMD "GG"\n",error_cAMD);
dprintf1(" error_cCMD "GG"\n",error_cCMD);
double error = fabs(error_cAMD)
+ fabs(error_cBMD)
+ fabs(error_cCMD);
dprintf1(" error sum abs "GG"\n",error);
if(error < err_min){
err_min=error;
best_yet=i;
}
}
return best_yet;
}
=================================================================
Given the angles about M a_radians= 0.347359178898,b_radians=
0.247292756478,c_radians= 0.243596173163
M(x,y,z)[0]=( 0.117546668879, 1.491492178271, 3.764555292010)
M(x,y,z)[1]=( 0.249999787814, 0.750000060576, 3.999999981443)
M(x,y,z)[2]=( 2.003692652636, 0.078506314801, 3.437915051312)
M(x,y,z)[3]=( -0.503154708657, -0.336031087029, 3.811166388233)
M=(0.117547,1.49149,3.76456)
error_cBMD -0.000451615092
error_cAMD 0.002439361691
error_cCMD 0.003017927362
error sum abs 0.005908904145
M=(0.25,0.75,4)
error_cBMD -0.000000000337
error_cAMD -0.000000000068
error_cCMD -0.000000000056
error sum abs 0.000000000461
M=(2.00369,0.0785063,3.43792)
error_cBMD 0.007713507165
error_cAMD 0.005092855010
error_cCMD -0.002262297516
error sum abs 0.015068659690
M=(0.25,0.75,4)
PASSED Does calcLocationMFrom6Angles() think the solution is correct
actual=1.000000 expected=1.000000 tolerance: +/-0.001000
delta = 0.000000
PASSED Mx
actual=0.250000 expected=0.250000 tolerance: +/-0.000250
delta = -0.000000
PASSED My
actual=0.750000 expected=0.750000 tolerance: +/-0.000750
delta = 0.000000
PASSED Mz
actual=4.000000 expected=4.000000 tolerance: +/-0.004000
delta = -0.000000
summary:
# of tests complete = 4
# of tests failed = 0
# of tests passes = 4
OverAll results for test: PASSED -- yeah
Comment by eldenc on Mon 25 Jan 2010 - 9:33 pm UTC:
Hopefully, tonight I'll get a chance to apply the formula against the real
data from the camera. I think my previous data set may have been corrupted
by an IR reflective ruler.
========================================================
#define DISTANCE(ax,ay,bx,by) sqrt((ax-bx)*(ax-bx)+(ay-by)*(ay-by))
int calcLocationM(double ax,double ay,
double bx,double by,
double cx,double cy,
double dx,double dy,
double *Mx,double *My,double *Mz){
dprintf1("For points:\n"
" A=(%5.1f,%5.1f)\n"
" B=(%5.1f,%5.1f)\n"
" C=(%5.1f,%5.1f)\n"
" D=(%5.1f,%5.1f)\n",ax,ay,bx,by,cx,cy,dx,dy);
//this is specific to the Wii Mote Camera
double radians_per_pixel=0.00143066; //atan(12.5 cm /54.0 cm)/159
pixels
//angles -- eg BMC_radians=the angle subtended by bc from 3d point M
//the first 3 are for determining the candiate positions
double AMB_radians=DISTANCE(ax,ay,bx,by)*radians_per_pixel;
double AMC_radians=DISTANCE(ax,ay,cx,cy)*radians_per_pixel;
double BMC_radians=DISTANCE(bx,by,cx,cy)*radians_per_pixel;
//the second 3 are for isolating to 1 solution
double AMD_radians=DISTANCE(ax,ay,dx,dy)*radians_per_pixel;
double BMD_radians=DISTANCE(bx,by,dx,dy)*radians_per_pixel;
double CMD_radians=DISTANCE(cx,cy,dx,dy)*radians_per_pixel;
int success=calcLocationMFrom6Angles(
BMC_radians,AMC_radians,AMB_radians,
AMD_radians,BMD_radians,CMD_radians,
Mx,My,Mz);
dprintf1("success=%d M=(%g,%g,%g)\n\n",success,*Mx,*My,*Mz);
return success;
}
Answer clarification by Researcher mathtalk on Tue 26 Jan 2010 - 12:38 pm UTC:
Below are the angles involving D for numerically computed roots v, redone because of the error mentioned above (using "exact" u with computed v). This error did not affect the "exact" angles for the true M, so see the earlier post for those. v = 1.054650618676, M = (-0.503133775,-0.336042042,3.811164221) angle | cosine | radians ------+--------------+------------- AMD | 0.945961212 | 0.330250282 BMD | 0.972886341 | 0.23339699 CMD | 0.973124346 | 0.232365694 v = 1.026262454132, M = (2.003142516,0.07853333,3.438242245) angle | cosine | radians ------+--------------+------------- AMD | 0.934756768 | 0.363222484 BMD | 0.962766421 | 0.273740597 CMD | 0.971838272 | 0.237886149 v = 0.984848883367, M = (0.250007551,0.749978965,4.000002127) angle | cosine | radians ------+--------------+------------- AMD | 0.939849634 | 0.348606485 BMD | 0.970476849 | 0.243596283 CMD | 0.969578691 | 0.247292603 v = 0.937635936807, M = (0.117546398,1.491497084,3.764552602) angle | cosine | radians ------+--------------+------------- AMD | 0.937410232 | 0.355679076 BMD | 0.970928492 | 0.241716648 CMD | 0.966560692 | 0.259335119 Distinguishing the approximations to M is now a good bit easier than the earlier results suggested. The best has angles that deviate from the exact angles by only 1.5E-7 or so, while discrepancies for other roots are over 1.0E-3 with regard to any of the three angles involving D. It's also an opportune time to collect our corrections to formulas for the coefficients of the quartic:
Comment by eldenc on Sat 30 Jan 2010 - 9:11 am UTC:
While I haven't figured out the correct multiplier for the pixels to radians conversion (its close), over all it seems to work well. Thanks! You can playback some of my wiimote recordings via wget http://eldenc.tripod.com/resection4knowPointsAndAngles.20100129B.tar.gz read the top of msg_special.py for how to install gnu_plot python Then run it via python msg_special.py upAndDownAtXeqYeq5.txt Or if you have a wiimote handy you can install wmgui (sudo apt-get install wmgui, if your in ubuntu), lswm and record your own via python msg_special.py 00:27:09:33:BB:01 (get the number from lswm). Two more questions, in testing it appears that this works beyond the first quadrant do you have an off the cuff answer for how far? Also at what angle from the origin is formula the most accurate? And for my curiosity, How crazy is the math for arbitrary know points? Thanks for your time and effort, and WOW that's kinda cool!
Accepted and rated by eldenc on Sat 30 Jan 2010 - 9:17 am UTC:
While the answer was much more complicated that I thought it would be, mathtalk figured it out and went the extra mile to help me verify his solution and my code. Thanks!
Answer clarification by Researcher mathtalk on Mon 1 Feb 2010 - 3:47 pm UTC:
We do have a couple of Wii controllers, though my wife, daughter, and her friends would probably rather I got a spare one to experiment with! I think the general case will not be too bad to write. I was fearful of trying to "debug" it in my initial answer, but now that we've tested the isoceles right triangle case pretty thoroughly, I think we can leverage those answers to validate the general case. As to the accuracy of the method, there are two things to keep in mind. As the angle of the camera's line of sight to the plane in which the triangle lies becomes increasingly acute, we lose information due to foreshortening of one or more legs of the triangle. [We also lose information at a longer distance from the target, but in my optimistic thinking this is less serious because (depending on how fast the plane flies) longer distance means more time left to adjust the flight path.] I think I can quantify this in terms of how "sensitive" (size of derivatives) the observed angles are to changing the camera location. I did scratch this out somewhat in my legal pad, so let me clean it up a bit and I'll post the analysis. The other accuracy issue has to do with the numerical methods involved. As mentioned before, Grunert's approach introduces a "singularity" through the rational expression for u in terms of v, so that for v near this singularity the computation of u is unreliable as presented. Some of the other methods that have been proposed/studied avoid this, cf. the paper by Haralick et al cited in my original answer, but are more complicated to implement. Since the plane is in motion, my (again possibly overoptimistic) thinking was that the passage near the singular value for v (an artifact of computation and not a physical effect) would be brief enough not to matter. [In our worked example the singular value for v lies closest to the second root of the quartic for v, and the third root is the "correct" one as detected by using the fourth point.] I also have a bunch of references gathered at the earliest stage of working on this interesting question, set aside until we got the crux of the problem solved since they bear on such ancillary issues as the calibration of the camera angles. Since you're now "focused" on that, I hope it will be useful to have them. regards, mathtalk
Comment by eldenc on Mon 1 Feb 2010 - 8:50 pm UTC:
Yea, if your using the wii mote that has already been associated with a Wii that is in range, it will steal it away from your ubuntu 9.10 bluetooth connection when the wii is powered on (I think they have some 'special' bluetooth commands). (My kid's seemed to think it was funny, so they would do it intentionally!) So I got two for me :-)....one to pull apart and put just the camera on the plane, and one for testing this algorithm. Hopefully, I'll get some flight derived data this week end. The before mentioned python/c data is from a box of Christmas lights n a burrito box.
Comment by User myoarin on Tue 2 Feb 2010 - 5:56 pm UTC:
Wow! Congratulations! (not that I understood anything beyond that it was about trying to land a remote controlled model plane) It just lets me wonder (capital W) how a human with two eyes can stand somewhere on a tennis court (part of the mental calculation) and see a ball arching over the net (tilt of head and curve and direction of the ball calculated) and from the flight of the ball judge whether it will land behind him on the court or beyond the base line. Of course, depth perception with two eyes helps - and a lifetime of experience - but still ... That is not a suggestion that a second camera and trying to integrate the images would be a better or easier solution.
Comment by eldenc on Wed 3 Feb 2010 - 11:07 pm UTC:
There are a few issues with using multiple camera's, 1) the accuracy of the viewing angle between the cameras is critical, which is difficult given that to improve accuracy you want to be as far apart as possible, but, of course, the farther apart they are...holding them rigidly becomes exponentially more difficult (especially on a foam A/C!) 2) with closely spaced cameras, you need higher resolutions to obtain the same accuracy, and finally 3) [the driver for me], the wii mote cameras are on a particular I2C address, which would require that I have 2 different i2c buses, but alas I only have one on my current system, 4)the simpler the system the more reliable it is. On tennis court issue, we are not really using our stereoscopic view to determine distance, we are mostly using our knowledge of the size of the ball and the pattern of flight that we have seen many times before and for players better than me, they are guessing the balls flight pattern from watching the opponent's swing. We are also using time correlation of the movement, not simply one image.
Comment by eldenc on Wed 3 Feb 2010 - 11:16 pm UTC:
So do the 'singularities' form a ellipsoid, a ray, or are they discreet points? I don't think I've seen any yet. (But maybe I miss-labeled the singularities as a hardware issues)
Comment by User myoarin on Thu 4 Feb 2010 - 9:39 pm UTC:
Hi Eldenc, Thanks for your input. Makes sense. I hope your plane lands where you want it to. Myo
Actions: Add Comment
Bookmark it!
|
Frequently Asked Questions | Terms & Conditions | Disclaimer | Privacy Policy | Contact Us | Spread the word! © 2010 Uclue Ltd |
Comment by eldenc on Wed 20 Jan 2010 - 5:02 am UTC: