Horrific System of Equations

This topic is 3477 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

Recommended Posts

Hi! I'm working on an interesting computer graphics project, but I'm stuck with some math and I need your help desperately. I'm trying to figure out the approximate position and orientation of a camera from a picture. I figured that if I had some cues in the picture, for example, if I had three points in the picture and knew the real distances between each pair (i.e. the 3D distances, not the distances between their projections on the picture), I could use that to get three equations and use the perspective projection formula to get six more, resulting in a system of 9 equations with 9 unknowns, with the unknows being the real coordinates of the points in 3D space. I can later use those coordinates to compute everything I need. (I don't think it would work with any less than three points, so it's 9 equations in the best case.) If non of the above makes any sense to you, it's ok. I'm just going to write the system down: D1^2 = (x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2 D2^2 = (x2 - x3)^2 + (y2 - y3)^2 + (z2 - z3)^2 D3^2 = (x3 - x1)^2 + (y3 - y1)^2 + (z3 - z1)^2 Sx1 = x1 / z1 Sx2 = x2 / z2 Sx3 = x3 / z3 Sy1 = y1 / z1 Sy2 = y2 / z2 Sy3 = y3 / z3 So the unknowns are {x1, x2, y3, y1, y2, y3, z1, z2, z3}. D_ are the distances between the points in 3D space, S__ are the 2D coordinates of the three points after perspective projection. Those are given. It doesn't look too complicated and technically should be solvable, but it gets really hard. I can't solve it, neither can Maple 11. I think the main thing that bugs me is those powers of 2. I even had this crazy idea to replace the top three lines with some linear approximations of the distances, which should make the whole thing much easier. (Too bad I just don't know of any...) Please, help me. I need a solution to this desperately. An exact solution, a numerical solution, an approximate solution... any would do. (Maybe there's even some numeric algorithm I could implement and use to solve this?) Thank you very much for your time. Any help would be very appreciated. Oh, and pardon my bad English. :)

Share on other sites
Hi,

This can get solved quickly provided you do some developments and try to reduce to a manageable number of equations:

I started with your 4th equation and expressed it as:
z1 = x1/Sy1 (A1)
Then I looked at your 7th equation and expressed it as:
Sy1 = y1 * Sx1 / x1 or as x1 = y1 * Sx1 / Sy1 (B1)
Note that it can also be expressed as z1 = y1 / Sy1 (C1)
This can done for the second and third set of coordinates.

Now, plugging back these results into equation number 1, we get:
D1^2 = (A1 - A2)^2 + (C1 - C2)^2 + (B1-B2)^2
You can do this for the 2nd and third equation. You are left with three sets of equations with only three variables (y1, y2, y3).

To end, you just need to develop each of these three equations by remembering that:
(a-b)^2 = a^2 - 2ab + b^2

Then you simplify and you should be all set to solve your problem.

Hope that helps.

Ghostly yours,
Red.

Share on other sites
Dear god, no! Don't do this with scalar equations! Recovering position from point correspondences is a well-studied problem, with approaches that are downright pleasant. Try this book. Chapters 4, 5, and 6 will be most useful to you.

Share on other sites
@Red Ghost:
I've tried something similar, but it still gets really nasty.
Here are the three equations I came up with:

D1^2 - (x1 - x2)^2 - (Sy1 * x1 / Sx1 - Sy2 * x2 / Sx2)^2 + (x1 / Sx1 - x2 / Sx2)^2 = 0
D2^2 - (x2 - x3)^2 - (Sy2 * x2 / Sx2 - Sy3 * x3 / Sx3)^2 + (x2 / Sx2 - x3 / Sx3)^2 = 0
D3^2 - (x3 - x1)^2 - (Sy3 * x3 / Sx3 - Sy1 * x1 / Sx1)^2 + (x3 / Sx3 - x1 / Sx1)^2 = 0

Now, if I try to work out x3 from the 3rd equation, I get two horrid possible solutions, which I then need to plug into the 2nd equation, making it un-managable. Perhaps I'm just doing something stupid...?
I've tried grouping the knowns and denoting them with simple symbols, but there's too many of them. :(

@Sneftel:
That looks quite interesting. I'll check it out.

Thank you, guys.

Share on other sites
Quote:
 Original post by StasBHi! I'm working on an interesting computer graphics project, but I'm stuck with some math and I need your help desperately. I'm trying to figure out the approximate position and orientation of a camera from a picture. I figured that if I had some cues in the picture, for example, if I had three points in the picture and knew the real distances between each pair (i.e. the 3D distances, not the distances between their projections on the picture), I could use that to get three equations and use the perspective projection formula to get six more, resulting in a system of 9 equations with 9 unknowns, with the unknows being the real coordinates of the points in 3D space. I can later use those coordinates to compute everything I need. (I don't think it would work with any less than three points, so it's 9 equations in the best case.) If non of the above makes any sense to you, it's ok. I'm just going to write the system down:D1^2 = (x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2D2^2 = (x2 - x3)^2 + (y2 - y3)^2 + (z2 - z3)^2D3^2 = (x3 - x1)^2 + (y3 - y1)^2 + (z3 - z1)^2Sx1 = x1 / z1Sx2 = x2 / z2Sx3 = x3 / z3Sy1 = y1 / z1Sy2 = y2 / z2Sy3 = y3 / z3So the unknowns are {x1, x2, y3, y1, y2, y3, z1, z2, z3}.D_ are the distances between the points in 3D space, S__ are the 2D coordinates of the three points after perspective projection. Those are given.It doesn't look too complicated and technically should be solvable, but it gets really hard.I can't solve it, neither can Maple 11.I think the main thing that bugs me is those powers of 2. I even had this crazy idea to replace the top three lines with some linear approximations of the distances, which should make the whole thing much easier. (Too bad I just don't know of any...)Please, help me. I need a solution to this desperately. An exact solution, a numerical solution, an approximate solution... any would do. (Maybe there's even some numeric algorithm I could implement and use to solve this?)Thank you very much for your time. Any help would be very appreciated. Oh, and pardon my bad English. :)

You could use the Newton-Raphson method. You must define 9 functionsfi(x1,x2,x3,y1,y2,y3,z1,z2,z3). To derive f1(xi,yi,zi), put all the terms of the first equation in the right member:    D1^2 = (x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2<=>(x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2 - D1^2 = 0=>  f1(x1,x2,x3,y1,y2,y3,z1,z2,z3) := (x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2 - D1^2.The first equation can then be rewritten f1(x1,x2,x3,y1,y2,y3,z1,z2,z3) = 0.You should do that for all the remaining equations.You define after a vectorial function( in french "fonction vectorielle", I don't know the correct term in english ) whose components are those fi(x1,x2,...).You'll have:F(x1,x2,...)=[f1(x1,x2,...)              f2(x1,x2,...)              f3(x1,x2,...)              ...              f9(x1,x2,...)]F(x1,x2,...) is a column vector( dimensions (9x1), in this case ). Your system of equations can now be rewritten:F(x1,x2,...) = [0                0                .                .                .                0]Well, to solve your system of equations, you'll need the Jacobian of F(x1,x2,...). By definition, it is the square matrix (J)ij, i = 1,2,...,9 and j = 1,...,9 with(J)ij = partial derivative of fi(x1,x2,...,y3) with respect to xi, i = 1,2,...,9Here is how those xi are defined:        x1:=x1;        x2:=x2;        x3:=x3;        x4:=y1;        x5:=y2;        x6:=y3;        x7:=z1;        x8:=z2;        x9:=z3;Read the symbols ":=" "is defined as". Well, the derivation of that Jacobian by hand could have been time consuming since that square matrix has 81 component( 9 x 9 ), but hopefully, many terms are equal to zero( sparse Matrix ).To make the equations shorter, we'll make another definition:                  X:=(x1,x2,x3,y1,y2,y3,z1,z2,z3)The system can be rewritten as followed:        F(X) = 0 ( 0 being the null vector )Having done that, now, we will apply the following algorithm:Xn+1 = Xn - inv(J(Xn)).F(Xn) (*)

Xn and Xn+1 being, of course, columns vectors( dimension(9x1) ). You could choose for X1 any vector( most of the time a good approximation of the solution, but if, and apparently that's the case, we don't have one, we can take anything! ). You will then compute F(X1)( you simply replace all xi with their value in F(x1,x2,...) ), J(X1), the Jacobian evaluated at X1( all xi in the matrix are replaced by their corresponding value ) and its inverse( hence the Jacobian must be inversible ). Take the difference Xn - inv(J(Xn)).F(Xn) to find a new approximation of the solution, Xn+1. Restart the process. You can stop the iterative process if Xn and Xn+1 are close enough, thats is to say, if Xn+1 - Xn < delta_X, with delta_X taken as small as wanted.

Note : you can check the correctness of the equation (*) by analysing the dimenstions of the variables involved

(9x1) = (9x1) - (9x9)x(9x1) (ixj) := i rows and j columns.

English is not my mother tongue, so sorry if I'm not clear enough...

[Edited by - johnstanp on July 11, 2008 6:55:31 AM]

Share on other sites
Quote:
 Original post by StasB@Red Ghost:I've tried something similar, but it still gets really nasty.Here are the three equations I came up with:D1^2 - (x1 - x2)^2 - (Sy1 * x1 / Sx1 - Sy2 * x2 / Sx2)^2 + (x1 / Sx1 - x2 / Sx2)^2 = 0D2^2 - (x2 - x3)^2 - (Sy2 * x2 / Sx2 - Sy3 * x3 / Sx3)^2 + (x2 / Sx2 - x3 / Sx3)^2 = 0D3^2 - (x3 - x1)^2 - (Sy3 * x3 / Sx3 - Sy1 * x1 / Sx1)^2 + (x3 / Sx3 - x1 / Sx1)^2 = 0Now, if I try to work out x3 from the 3rd equation, I get two horrid possible solutions, which I then need to plug into the 2nd equation, making it un-managable. Perhaps I'm just doing something stupid...?I've tried grouping the knowns and denoting them with simple symbols, but there's too many of them. :(@Sneftel:That looks quite interesting. I'll check it out.Thank you, guys.

Actually the method I described could be used for this reduced system.

Share on other sites
I'm giving up on this one.
Even if I was able to solve this system, it has too many possible solutions to be of any use.
Anyway, thanks for all your help.

Share on other sites
Quote:
 Original post by StasBI'm giving up on this one.Even if I was able to solve this system, it has too many possible solutions to be of any use.Anyway, thanks for all your help.

I've provided the method but if you want, I can implement it. I just didn't do it because I don't have a copy of MatLab or any other scientific software at home, to do the matrix computations easily.
But I can donwload a student version or an open source software and then implement the algorithm. Or even write a C++ program that would solve that specific system of equations. If you really need it, it won't bother me to actually implement it: the equations are simple enough that doing it won't be time consuming.
If you're really interested, tell me.

Share on other sites
Thank you very much! I'm just not sure if I should bother you with this, because I'm no longer sure if my equations would do the job. What I'm trying to do is to detect the 3D coordinates of three points given their perspective projections and the distances between them. I'm pretty sure the equations I formulated are correct, but they don't really solve my problem. Each point has a single exact location, but the equations yield many possible solutions and I don't know how I would chose the real one even if I could find all the solutions. If I understand this right, the method you suggest would return only one of the many possible solutions depending on the initial guess?

Share on other sites
Quote:
 Original post by StasBThank you very much! I'm just not sure if I should bother you with this, because I'm no longer sure if my equations would do the job. What I'm trying to do is to detect the 3D coordinates of three points given their perspective projections and the distances between them. I'm pretty sure the equations I formulated are correct, but they don't really solve my problem. Each point has a single exact location, but the equations yield many possible solutions and I don't know how I would chose the real one even if I could find all the solutions. If I understand this right, the method you suggest would return only one of the many possible solutions depending on the initial guess?

Well, it will give "a" solution provided there is not a unique one...The solution would depend on the first approximation given(X1(x11,y11,z11,x21,...,z31)). It's the same as finding the root of a function( that's actually what it is ): you can have multiple zeros. That's not necessarily the case( it could depends on the constants you plug in your system of equations ), but since your problem is a geometric one, it can be guessed. I guess that the presence of squares and divides will result in multiple solutions...But this is not a demonstration, just a guess: nothing proves me that I am right to do it.

If there are multiple solutions, adding constraint equations, would remove the unwanted ones. But then, things get more complicated.[/edit]

[Edited by - johnstanp on July 11, 2008 9:56:17 AM]

Share on other sites
Many solutions mean that I can have two or more identical triangles positioned and oriented differently but still look the same to the camera, right?
Is that really possible?
[EDIT]
I thought the same triangle can't be positioned somewhere else and still look the same to the camera. You're right. I need more equations to constrain the number of solutions, but I can't think of any right now. Maybe it's possible to construct a figure which would look different from any point of view?

Share on other sites
Quote:
 Original post by StasBMany solutions mean that I can have two or more identical triangles positioned and oriented differently but still look the same to the camera, right?Is that really possible?[EDIT]I guess I just went the wrong way about this due to my an incorrect assumption.I thought the same triangle can't be positioned somewhere else and still look the same to the camera. You're right. I need more equations to constrain the number of solutions, but I can't think of any right now. Maybe it's possible to construct a figure which would look different from any point of view?

Actually, there are( --could be-- ) multiple solutions to the system of equations you've provided. We haven't discussed the assumptions or hypotheses made to derive them. So the problem could be linked to the system of equations you've provided. I could have said something about it if I was an "expert" in computer graphics programming( I've studied mechanics and mechanical systems but not computer graphics ).

Anyway, I've got the book "Mathematics for 3D Game Programming and Computer Graphics" and some pages are devoted to projections and matrix projections. I'll read them and see how I would have tackled the problem...

Share on other sites
Thank you, but I think it wouldn't help.
My problem has no good solution.
Any solution would involve the distances between the points, any involvement of distances would involve squared unknows, any squared unknowns would cause more than one solution. (At least I think so.)
I think I'll try a different approach using stereoscopy.

Share on other sites
I think I figured out what's going on here!!!
I think there are only two real solutions to my equations: One for a triangle in front of the camera, one for a triangle behind. A couple of years ago, I solved this equation as a challenge from my math teacher: sqrt(4 + sqrt(4 + sqrt(4 - x))) = x
After getting rid of all the square roots, I got an 8th degree polynominal, with eight solutions, most of which were complex, but a few of which were real. The original equation had only one real solution. This comes to show how we get many extra solutions in the process of getting rid of square roots. For example, sqrt(x) = 4, so x^2=16, so we get x = {4,-4}, but only 4 is a real solution. I think this is exactly how I get the extra solutions.

Let's take a look at this again:
D1^2 - (x1 - x2)^2 - (Sy1 * x1 / Sx1 - Sy2 * x2 / Sx2)^2 + (x1 / Sx1 - x2 / Sx2)^2 = 0
D2^2 - (x2 - x3)^2 - (Sy2 * x2 / Sx2 - Sy3 * x3 / Sx3)^2 + (x2 / Sx2 - x3 / Sx3)^2 = 0
D3^2 - (x3 - x1)^2 - (Sy3 * x3 / Sx3 - Sy1 * x1 / Sx1)^2 + (x3 / Sx3 - x1 / Sx1)^2 = 0

We can get x3 from eq. 3. Two possible solutions. Since these are solutions to a quadratic equation, they have a square root somewhere inside. When we plug that into eq. 2, we get a square root in there, so when we want to get x2 from eq. 2, we have to square the whole thing, getting a polynominal of the 4th degree, giving us four possible solutions, containing square roots again. The same happens when we plug the solutions into eq. 1, giving us 8 solutions!
I think I could filter the un-needed solutions with just a bunch of inequlities, like in sqrt(x) = 4, x > 0, just like I did when solving my math teacher's challenge. By the way, you seem to be a very knowledgeable person. This might be interesting for you: sqrt(4 + sqrt(4 + sqrt(4 - x))) = x
Try solving it. ;D

Share on other sites
Quote:
 Original post by StasBThis might be interesting for you: sqrt(4 + sqrt(4 + sqrt(4 - x))) = xTry solving it. ;D

Do not overestimate me. :D
Well, this equation yields an equation of the eigth order, as you've already stated it. So there exist eight solutions. By hand( using recursion with my pocket calculator ), I find a real solution x~2.507019... Solving such equations is not an easy task: the laziest way would be to plot the polynomial function f(x) = x^8 - 16x^6 + 88x^4 - 192x^2 + x + 140 for values in the range ]0,4[ and zooming in the regions where the function cross the x axis...Why the polynomial? We can do that with f(x)=sqrt(4+sqrt(4+sqrt(4-x)))-x, and we'd better do that with the original function( when using the polynomial, I found a solution x~2.3772, but it's obviously not a solution of the original equation )

Another one would be to use Newton-Raphson for this case, that is to say, xn+1 = xn + f(xn)/(df(xn)/dx)) ( df(xn)/dx being the derivative function of f(x) evaluated at x=xn ) for finding a "real" solution( or real solutions when trying many initial guesses). There are numerical methods to solve this kind of equations: using a scientific software removes the need to even know how to actually find those roots. But it could be an excellent exercise: I've thought using substitution, without any succes, for now. But that's a good challenge, so i'll try finding the solutions, mathematically and not numerically.

[Edited by - johnstanp on July 11, 2008 6:50:55 PM]

Share on other sites
Quote:
 Original post by StasBI think I figured out what's going on here!!!I think there are only two real solutions to my equations: One for a triangle in front of the camera, one for a triangle behind.

Actually, no...If you have an equilateral triangle, you have many directions from which, the triangle look the same. This is why I wrote that the number of solutions is related to the constants plugged( the Di values ), the length of the edges of the triangle.

Share on other sites
I don't know what you're talking about. If you have an equilateral triangle facing the camera directly, it would look equilateral. Rotate it a little bit around any axis and it will no longer look equilateral to the camera. Translate if a little bit and it would be displaced and/or become smaller. The number of solutions does not depend on Di. (Unless it's a degenerate triangle, in which case there might be no soltion.) I think I finally solved the damn thing. I've modified my equations a little and started working towards the solution manually, untill I reached the point where I have a single equation and a single variable. It's way too ugly to solve mathematically, but I think it could be solved numerically. (I know how to use the Newton-Raphson method for a single equation.) I just hope I haven't missed a minus or a plus anywhere, like I always do. If it happened this time, it just won't work and I'll never know why, so I'll have to recalculate everything... I went through hell to solve it and I'm too scared to test it because I might find out I have do it all over again. It's always the small details that get me... That's how I always manage to fail math at school. :(

Share on other sites
The 4th topic on this linked page ("4. Model Based Object Pose") seems to be exactly the sort of thing you're looking for:
http://www.cfar.umd.edu/~daniel/Site_2/Research.html

Share on other sites
Quote:
 Original post by StasBI don't know what you're talking about. If you have an equilateral triangle facing the camera directly, it would look equilateral. Rotate it a little bit around any axis and it will no longer look equilateral to the camera. Translate if a little bit and it would be displaced and/or become smaller. The number of solutions does not depend on Di. (Unless it's a degenerate triangle, in which case there might be no soltion.) I think I finally solved the damn thing. I've modified my equations a little and started working towards the solution manually, untill I reached the point where I have a single equation and a single variable. It's way too ugly to solve mathematically, but I think it could be solved numerically. (I know how to use the Newton-Raphson method for a single equation.) I just hope I haven't missed a minus or a plus anywhere, like I always do. If it happened this time, it just won't work and I'll never know why, so I'll have to recalculate everything... I went through hell to solve it and I'm too scared to test it because I might find out I have do it all over again. It's always the small details that get me... That's how I always manage to fail math at school. :(

For the equilateral triangle, there is no way to distinguish edges, nor vertices: so you could see the same thing on different positions and orientations. Just imagine that the triangle is rotated around edge 1 with angle alpha, in front of you. If the triangle was rotated around edge 2 by the same amount, you would see the same thing, if it was done in front of you, assuming edge2 occupied the same position as edge1: so the projection of the triangle doesn't allow you to determine, where the camera was.
That's related to the axes of symmetry of the figure.
I had an idea: maybe taking two or three figures would reduce uncertainty...In the real world we determine the particularity of a point of view, from the relative disposition of objects. Or maybe, looking at the colors(pixels) in the triangle , would remove uncertainty.

And to end, well, actually, you could content yourself with one solution...

Share on other sites
Quote:
 Original post by Maze MasterThe 4th topic on this linked page ("4. Model Based Object Pose") seems to be exactly the sort of thing you're looking for:http://www.cfar.umd.edu/~daniel/Site_2/Research.html

Yes, it seems.

Share on other sites
Quote:
Original post by johnstanp
Quote:
 Original post by StasBI don't know what you're talking about. If you have an equilateral triangle facing the camera directly, it would look equilateral. Rotate it a little bit around any axis and it will no longer look equilateral to the camera. Translate if a little bit and it would be displaced and/or become smaller. The number of solutions does not depend on Di. (Unless it's a degenerate triangle, in which case there might be no soltion.) I think I finally solved the damn thing. I've modified my equations a little and started working towards the solution manually, untill I reached the point where I have a single equation and a single variable. It's way too ugly to solve mathematically, but I think it could be solved numerically. (I know how to use the Newton-Raphson method for a single equation.) I just hope I haven't missed a minus or a plus anywhere, like I always do. If it happened this time, it just won't work and I'll never know why, so I'll have to recalculate everything... I went through hell to solve it and I'm too scared to test it because I might find out I have do it all over again. It's always the small details that get me... That's how I always manage to fail math at school. :(

For the equilateral triangle, there is no way to distinguish edges, nor vertices: so you could see the same thing on different positions and orientations. Just imagine that the triangle is rotated around edge 1 with angle alpha, in front of you. If the triangle was rotated around edge 2 by the same amount, you would see the same thing, if it was done in front of you, assuming edge2 occupied the same position as edge1: so the projection of the triangle doesn't allow you to determine, where the camera was.
That's related to the axes of symmetry of the figure.
I had an idea: maybe taking two or three figures would reduce uncertainty...In the real world we determine the particularity of a point of view, from the relative disposition of objects. Or maybe, looking at the colors(pixels) in the triangle , would remove uncertainty.

And to end, well, actually, you could content yourself with one solution...

I think you're wrong, but nobody said I have to use an equilateral triangle anyway. What triangle I use is entirely my choice, as long as it tells me the position of the camera. ;D

Share on other sites
Quote:
 Original post by StasBI think you're wrong, but nobody said I have to use an equilateral triangle anyway. What triangle I use is entirely my choice, as long as it tells me the position of the camera. ;D

I simply took the equilateral triangle to illustrate my point, that is to say, how those Di could play in the undetermination of the position and the orientation of the camera: I never said that you intended to use it...
Now, if you think using an arbitrary triangle would do the job, I have nothing to say: I cannot prove the opposite. Just like I cannot prove your affirmation.
That's your choice and any good numerical method would give you a( or "the" ) solution. You can even find the solution on paper. I just hope, you will be satisfied with your solution.

[Edited by - johnstanp on July 15, 2008 6:45:05 AM]

Share on other sites
After reading some papers (turns out there are plenty of them, dealing with my problem exactly), I was surprised to discover that getting several solutions is unavoidable, especially with only three points. So I guess the best I can do is chose one randomly and hope it's the right one. Could you please show me how to implement that Newton-Raphson method for solving a system, like you suggested earlier?

Share on other sites
Quote:
 Original post by StasBAfter reading some papers (turns out there are plenty of them, dealing with my problem exactly), I was surprised to discover that getting several solutions is unavoidable, especially with only three points. So I guess the best I can do is chose one randomly and hope it's the right one. Could you please show me how to implement that Newton-Raphson method for solving a system, like you suggested earlier?

I haven't read the messages in the topic, yesterday...Well, I'll implement the Newton-Raphson for this specific system: just give me a day, to do all the programming and testing...I'll post the code here.

Share on other sites
Well, I've finished implementing the Newton-Raphson for this particular system of equations. I've implemented a Matrix class( actually a square matrix class ), a vector class( N elements ) and a function class( not exactly a functor, but anyway ).
The code is quite clear, I hope except for the LU decomposition necessitated by the computation of the matrix inverse of the Jacobian: the code is simply a rewriting of the code provided by "Numerical recipes in C", chap2 p.46.

Here's the code:

Vector.hpp
#ifndef _VECTOR_HPP_#define _VECTOR_HPP_#include<iostream>#include<fstream>using std::endl;using std::istream;using std::ostream;using std::ofstream;using std::ios;using std::right;using std::fixed;using std::scientific;using std::setiosflags;using std::setw;using std::setprecision;template<typename Real,unsigned int N>class Vector{      public:             static Real EPS;                          Vector():x_(){}             explicit Vector( const Real[N] );                          const Real& operator[]( unsigned int )const;             Real& operator[]( unsigned int );             bool operator==( const Vector<Real,N>& )const;             Vector& operator-=( const Vector<Real,N>& );             bool operator>( const Real& )const;                          template<typename Real1,unsigned int N1>             friend ostream& operator<<( ostream& , const Vector<Real1,N1>& );                   private:              Real x_[N];};template<typename Real,unsigned int N>Vector<Real,N>::Vector( const Real x[N] ){    for( unsigned int i = 0 ; i < N ; ++i )    {        x_ = x;    }}template<typename Real,unsigned int N>const Real& Vector<Real,N>::operator[]( unsigned int i )const{    return x_;}template<typename Real,unsigned int N>Real& Vector<Real,N>::operator[]( unsigned int i ){    return x_;}template<typename Real,unsigned int N>bool Vector<Real,N>::operator==( const Vector<Real,N>& v )const{    for( unsigned int i = 0 ; i < N ; ++i )    {        if( fabs( x_ - v.x_ ) > EPS )        {            return false;        }    }        return true;}  template<typename Real,unsigned int N>bool Vector<Real,N>::operator>( const Real& val )const{    for( unsigned int i = 0 ; i < N ; ++i )    {        if( fabs( x_ ) < val )        {            return false;        }    }        return true;}template<typename Real,unsigned int N>Vector<Real,N>& Vector<Real,N>::operator-=( const Vector<Real,N>& v ){    for( unsigned int i = 0 ; i < N ; ++i )    {        x_ -= v.x_;    }        return *this;} template<typename Real1,unsigned int N1>ostream& operator<<( ostream& out , const Vector<Real1,N1>& v ){    out << "(";        for( unsigned int i = 0 ; i < N1 ; ++i )    {        out << setiosflags( ios::right | ios::fixed | ios::scientific );        out << setw( 12 ) << setprecision( 6 ) << v.x_;                if( i < N1 - 1 )        {            out << ",";        }            }        out << ")";              return out;      }template<typename Real,unsigned int N> Real Vector<Real,N>::EPS = 1.0e-10;#endif

Matrix.hpp

#ifndef _MATRIX_HPP_#define _MATRIX_HPP_#include<cmath>#include <iostream>#include <iomanip>#include "Vector.hpp"using std::cout;using std::endl;using std::istream;using std::ostream;using std::ofstream;using std::ios;using std::right;using std::fixed;using std::scientific;using std::setiosflags;using std::setw;using std::setprecision;bool test();template<typename Real,unsigned int N>class Matrix{      public:             static Real TINY;             static Real EPS;                          Matrix():a_(){}             explicit Matrix( const Real[N][N] );                          const Real& operator()( unsigned int , unsigned int )const;             Real& operator()( unsigned int , unsigned int );             const Matrix operator*( const Matrix<Real,N>& )const;             const Vector<Real,N> operator*( const Vector<Real,N>& )const;             bool operator==( const Matrix<Real,N>& )const;                          Real determinant()const;             const Matrix inverse()const;             bool isIdentity()const;             const Vector<Real,N> solve( const Vector<Real,N>& )const;                          template<typename Real1,unsigned int N1>             friend ostream& operator<<( ostream& , const Matrix<Real1,N1>& );                   private:              Real a_[N][N];                            void LUDecomposition( Matrix<Real,N>& , Vector<int,N>& , Real& )const;                       const Vector<Real,N> LUBackwardSubstitution( const Vector<Real,N>& ,                                                            const Vector<int,N>& )const;};template<typename Real,unsigned int N>Matrix<Real,N>::Matrix( const Real a[N][N] ){    for( unsigned int i = 0 ; i < N ; ++i )    {        for( unsigned int j = 0 ; j < N ; ++j )        {             a_[j] = a[j];        }    }}template<typename Real,unsigned int N>const Real& Matrix<Real,N>::operator()( unsigned int i , unsigned int j )const{    return a_[j];}template<typename Real,unsigned int N>Real& Matrix<Real,N>::operator()( unsigned int i , unsigned int j ){    return a_[j];}template<typename Real,unsigned int N>const Matrix<Real,N> Matrix<Real,N>::operator*( const Matrix<Real,N>& m )const{    Matrix<Real,N> res;        for( unsigned int i = 0 ; i < N ; ++i )    {        for( unsigned int j = 0 ; j < N ; ++j )        {             for( unsigned int k = 0 ; k < N ; ++k )             {                  res.a_[j] += a_[k] * m.a_[k][j];             }        }    }        return res;}template<typename Real,unsigned int N>const Vector<Real,N> Matrix<Real,N>::operator*( const Vector<Real,N>& v )const{    Vector<Real,N> res;        for( unsigned int i = 0 ; i < N ; ++i )    {         for( unsigned int j = 0 ; j < N ; ++j )         {             res += a_[j] * v[j];         }    }        return res;}template<typename Real,unsigned int N>bool Matrix<Real,N>::operator==( const Matrix<Real,N>& m )const{    for( unsigned int i = 0 ; i < N ; ++i )    {        for( unsigned int j = 0 ; j < N ; ++j )        {             if( fabs( a_[j] - m.a_[j] ) > EPS )             {                 return false;             }        }    }        return true;}template<typename Real,unsigned int N>Real Matrix<Real,N>::determinant()const{    Real d;    Vector<int,N> index;    Matrix<Real,N> LU;           LUDecomposition( LU , index , d );        for( unsigned int j = 0 ; j < N ; ++j )     {        d *= LU.a_[j][j];    }        return d;}template<typename Real,unsigned int N>const Matrix<Real,N> Matrix<Real,N>::inverse()const{    unsigned int i , j;    Matrix<Real,N> LU;    Matrix<Real,N> res;    Vector<int,N> index;    Vector<Real,N> col;    Real d;        LUDecomposition( LU , index , d );         for( j = 0 ; j < N ; ++j )    {         for( i = 0 ; i < N ; ++i )        {            col = ( Real )0.0;        }                col[j] = ( Real )1.0;        col = LU.LUBackwardSubstitution( col , index );                for( i = 0 ; i < N ; ++i )         {            res.a_[j] = col;        }    }        return res;}template<typename Real,unsigned int N>bool Matrix<Real,N>::isIdentity()const{    for( unsigned int i = 0 ; i < N ; ++i )    {         for( unsigned int j = 0 ; j < N ; ++j )         {              if( i != j )              {                  if( fabs( a_[j] ) > EPS )                   {                      return false;                  }              }              else              {                  if( fabs( a_[j] - 1 ) > EPS )                  {                      return false;                  }              }         }    }        return true;}  template<typename Real,unsigned int N>const Vector<Real,N> Matrix<Real,N>::solve( const Vector<Real,N>& b )const{    Matrix<Real,N> LU;    Vector<int,N> index;    Real d;        LUDecomposition( LU , index , d );        return LU.LUBackwardSubstitution( b , index );}  template<typename Real,unsigned int N>void Matrix<Real,N>::LUDecomposition( Matrix<Real,N>& result ,                                      Vector<int,N>& index ,                                      Real& d )const{    unsigned int imax;    Real big , dum , sum , temp;    Vector<Real,N> vv;    result = *this;      d = ( Real )1.0;        for(  unsigned int i = 0 ; i < N ; ++i )    {                                      big = ( Real )0;                for (  unsigned int j = 0 ; j < N ; j++ )        {            if( ( temp = fabs( result.a_[j] ) ) > big )            {               big = temp;                           }        }        assert( big != 0 );        vv = ( Real )1.0 / big;     }    for(  unsigned int j = 0 ; j < N ; ++j )    {        for(  unsigned int i = 0 ; i < j ; ++i )        {             sum = result.a_[j];            for(  unsigned int k = 0 ; k < i ; ++k )            {                sum -= result.a_[k] * result.a_[k][j];            }                        result.a_[j] = sum;        }        big = ( Real )0;        for (  unsigned int i = j ; i < N ; ++i )        {             sum = result.a_[j];             for (  unsigned int k = 0 ; k < j ; ++k )            {                sum -= result.a_[k] * result.a_[k][j];            }                        result.a_[j] = sum;                        if( ( dum = vv * fabs( sum ) ) >= big )            {                big = dum;                imax = i;            }        }                if ( j != imax ) //Do we need to interchange rows? Yes, do so        {              for(  unsigned int k = 0 ; k < N ; ++k )            {                 dum = result.a_[imax][k];                result.a_[imax][k] = result.a_[j][k];                result.a_[j][k] = dum;            }            d = -d;            vv[imax] = vv[j];        }        index[j] = imax;                if ( result.a_[j][j] == 0.0 )        {            result.a_[j][j] = TINY;        }        if ( j != N-1 )         {             dum = ( Real )1.0 / result.a_[j][j];                        for(  unsigned int i = j + 1 ; i < N ; ++i )            {                result.a_[j] *= dum;            }        }    } }template<typename Real,unsigned int N>const Vector<Real,N> Matrix<Real,N>::LUBackwardSubstitution( const Vector<Real,N>& b ,                                                             const Vector<int,N>& index )const{    int i , ip , j;    int ii = -1;    Real sum;    Vector<Real,N> sol = b;        for ( i = 0 ; i < N ; ++i )    {        ip = index;        sum = sol[ip];        sol[ip] = sol;           if ( ii > -1 )        {            for ( j = ii ; j < i ; j++ )            {                sum -= a_[j] * sol[j];            }        }        else        {             if( sum )            {                ii = i;             }        }                sol = sum;    }    for ( i = N-1 ; i > -1 ; --i )    {         sum = sol;          for ( j = i+1 ; j < N ; ++j )        {            sum -= a_[j] * sol[j];        }                sol = sum / a_;    }        return sol;}template<typename Real1,unsigned int N1>ostream& operator<<( ostream& out , const Matrix<Real1,N1>& m ){    for( unsigned int i = 0 ; i < N1 ; ++i )    {        for( unsigned int j = 0 ; j < N1 ; ++j )        {             out << setiosflags( ios::right | ios::fixed | ios::scientific );             out << setw( 12 ) << setprecision( 3 ) << m.a_[j];        }                out << endl;    }        return out;}template<typename Real,unsigned int N> Real Matrix<Real,N>::TINY = ( Real )1.0e-20;template<typename Real,unsigned int N> Real Matrix<Real,N>::EPS = ( Real )1.0e-10;                       #endif

Functor.hpp
#ifndef _FUNCTION_HPP_#define _FUNCTION_HPP_#include <cmath>#include <iostream>#include "Vector.hpp"using std::ostream;template<typename Real>class Functor{      public:             //Functor(){}             Functor( Real , Real , Real ,                       Real , Real , Real ,                       Real , Real , Real );                          const Vector<Real,9> evaluate( const Vector<Real,9>& )const;             const Matrix<Real,9> jacobian( const Vector<Real,9>& )const;             const Vector<Real,9> findZero( const Vector<Real,9>& , int , Real )const;                                                         template<typename Real1>             friend ostream& operator<<( ostream& , const Functor<Real1>& );                          template<typename Real1>             friend ofstream& operator<<( ofstream& , const Functor<Real1>& );                   private:              Real D1_ , D2_ , D3_;              Real Sx1_ , Sx2_ , Sx3_;              Real Sy1_ , Sy2_ , Sy3_;};template<typename Real>Functor<Real>::Functor( Real D1, Real D2, Real D3,                         Real Sx1, Real Sx2, Real Sx3,                         Real Sy1, Real Sy2, Real Sy3 )                        :D1_(D1), D2_(D2), D3_(D3), Sx1_(Sx1), Sx2_(Sx2), Sx3_(Sx3), Sy1_(Sy1), Sy2_(Sy2), Sy3_(Sy3){}template<typename Real>const Vector<Real,9> Functor<Real>::evaluate( const Vector<Real,9>& x )const{    //x[0] = x1;    //x[1] = y1;    //x[2] = z1;    //x[3] = x2;    //x[4] = y2;    //x[5] = z2;    //x[6] = x3;    //x[7] = y3;    //x[8] = z3;        Vector<Real,9> v;        v[0] = pow( x[0] - x[3] , 2 ) + pow( x[1] - x[4] , 2 ) + pow( x[2] - x[5] , 2 ) - D1_;// * D1_;    v[1] = pow( x[3] - x[6] , 2 ) + pow( x[4] - x[7] , 2 ) + pow( x[5] - x[8] , 2 ) - D2_;// * D2_;    v[2] = pow( x[6] - x[0] , 2 ) + pow( x[7] - x[1] , 2 ) + pow( x[8] - x[2] , 2 ) - D3_;// * D3_;    v[3] = x[0] - x[2] * Sx1_;    v[4] = x[3] - x[5] * Sx2_;    v[5] = x[6] - x[8] * Sx3_;    v[6] = x[1] - x[2] * Sy1_;    v[7] = x[4] - x[5] * Sy2_;    v[8] = x[7] - x[8] * Sy3_;        return v;}template<typename Real>const Matrix<Real,9> Functor<Real>::jacobian( const Vector<Real,9>& x )const{    Matrix<Real,9> m;        Real dx12 = ( x[0] - x[3] ) * ( Real )2.0;    Real dy12 = ( x[1] - x[4] ) * ( Real )2.0;    Real dz12 = ( x[2] - x[5] ) * ( Real )2.0;        m(0,0) = dx12;    m(0,1) = dy12;    m(0,2) = dz12;    m(0,3) = -dx12;    m(0,4) = -dy12;    m(0,5) = -dz12;        Real dx23 = ( x[3] - x[6] ) * ( Real )2.0;    Real dy23 = ( x[4] - x[7] ) * ( Real )2.0;    Real dz23 = ( x[5] - x[8] ) * ( Real )2.0;        m(1,3) = dx23;    m(1,4) = dy23;    m(1,5) = dz23;    m(1,6) = -dx23;    m(1,7) = -dy23;    m(1,8) = -dz23;        Real dx13 = ( x[0] - x[6] ) * ( Real )2.0;    Real dy13 = ( x[1] - x[7] ) * ( Real )2.0;    Real dz13 = ( x[2] - x[8] ) * ( Real )2.0;        m(2,0) = dx13;    m(2,1) = dy13;    m(2,2) = dz13;    m(2,6) = -dx13;    m(2,7) = -dy13;    m(2,8) = -dz13;        m(3,0) =  ( Real )1.0;    m(3,2) = -Sx1_;        m(4,3) = ( Real )1.0;    m(4,5) = -Sx2_;        m(5,6) = ( Real )1.0;    m(5,8) = -Sx3_;        m(6,1) = ( Real )1.0;    m(6,2) = -Sy1_;        m(7,4) = ( Real )1.0;    m(7,5) = -Sy2_;        m(8,7) = ( Real )1.0;    m(8,8) = -Sy3_;        return m;}template<typename Real> const Vector<Real,9> Functor<Real>::findZero( const Vector<Real,9>& Xo ,                                               int MAX ,                                               Real eps )const{    int count = 0;        Vector<Real,9> X = Xo;    Vector<Real,9> dX = jacobian(X).inverse() * evaluate(X);    while( dX > eps && count < MAX )    {        X -= dX;        dX = jacobian(X).inverse() * evaluate(X);                ++count;    }        assert( count < MAX );        ofstream out( "output.txt" , ios::app );    out << "####################################################################" << endl;    out << *this << endl;    out << "////////////////////////////////////////////////////////////////////" << endl;    out << "count = " << count << endl;    out << "Xo = " << Xo << endl;    out << "MAX = " << MAX << endl;    out << "eps = " << eps << endl;    out << "X = " << X << endl;    out << "f(X) = " << evaluate(X) << endl;    out.close();        return X;}template<typename Real>ostream& operator<<( ostream& out , const Functor<Real>& f ){    out << "D1 = " << f.D1_ << endl;    out << "D2 = " << f.D2_ << endl;    out << "D3 = " << f.D3_ << endl;        out << "Sx1 = " << f.Sx1_ << endl;    out << "Sx2 = " << f.Sx2_ << endl;    out << "Sx3 = " << f.Sx3_ << endl;        out << "Sy1 = " << f.Sy1_ << endl;    out << "Sy2 = " << f.Sy2_ << endl;    out << "Sy3 = " << f.Sy3_ << endl;        return out;}template<typename Real>ofstream& operator<<( ofstream& out , const Functor<Real>& f ){    out << "D1 = " << f.D1_ << endl;    out << "D2 = " << f.D2_ << endl;    out << "D3 = " << f.D3_ << endl;        out << "Sx1 = " << f.Sx1_ << endl;    out << "Sx2 = " << f.Sx2_ << endl;    out << "Sx3 = " << f.Sx3_ << endl;        out << "Sy1 = " << f.Sy1_ << endl;    out << "Sy2 = " << f.Sy2_ << endl;    out << "Sy3 = " << f.Sy3_ << endl;        return out;}                        #endif

Main.cpp
#include <cstdlib>#include <iostream>#include "Matrix.hpp"#include "Vector.hpp"#include "Functor.hpp"using namespace std;int main(int argc, char *argv[]){    srand( time( 0 ) );        Matrix<double,3> m;        m(0,0) = -1;    m(0,1) = 2;    m(0,2) = 1;    m(1,0) = 0;    m(1,1) = 8;    m(1,2) = 6;    m(2,0) = -2;    m(2,1) = 0;    m(2,2) = 5;        cout << "m = " << endl << m << endl;    double c[4] = { 0.0 , 10.0 , -11.0 };    Vector<double,3> b( c );    Vector<double,3> sol = m.solve( b );    cout << "sol = " << sol << endl;    cout << "m * sol == b ?" << ( ( m * sol ) == b ) << endl;    cout << "m.inverse() = " << endl << m.inverse() << endl;    cout << "m.determinant() = " << m.determinant() << endl;        //Functor<double> func( 12, 200, 65, 1.256, 2.56 , 0.23 , 5.23, 1.25e2 , 3.33 );    Functor<double> func( 120, 200, 101, 1.256, 1.56 , 1.023 , 2.523, 1.25 , 3.33 );    cout << "Function : " << endl << func << endl;    Vector<double,9> v;    int value;        for( unsigned int i = 0 ; i < 9 ; ++i )    {        value = rand() % 1001;        v = value;    }        cout << "v = " << v << endl;       Vector<double,9> zero = func.findZero( v , 20000 , 1.0e-6 );    cout << "zero = " << zero << endl;        return EXIT_SUCCESS;}

[Edited by - johnstanp on July 15, 2008 3:35:21 PM]