Least Squares Fit on Line in polar representation

Question

I'm working on the basis of a robot localization system, and truth be told, I'm not sure if this is a maths or an implementation issue. I'm trying to fit a line to a set of polar coordinates using Least Square Fitting. The line is represented in polar form. I've been given two equations from our supervisor, one to find the angle and one to find the distance to the origin. (see image)

https://i.stack.imgur.com/jUEZ5.png

I've tried to implement the equation in C++.

struct Pt {
    double d, angle;
};

// Polar coordinates of the points (-2, 1) and (5, 1) respectively.
std::vector<Pt> points = { { 2.2360679774997898, 2.6779450445889870 }, { 5.0990195135927845, 0.19739555984988075 } };

double a, r = 0;

double n = points.size();
double sumOfWeights = n;

double num1 = 0, num2 = 0, den1 = 0, den2 = 0;
for (int i = 0; i < n; i++) {
    double iP = points[i].d, iTheta = points[i].angle;

    num1 += iP * iP * sin(2 * iTheta);
    den1 += iP * iP * cos(2 * iTheta);

    for (int j = 0; j < n; j++) {
        double jP = points[j].d, jTheta = points[j].angle;

        num2 += iP * jP * cos(iTheta) * sin(jTheta);
        den2 += iP * jP * cos(iTheta + jTheta);

    }
}

a = 0.5 * atan2((num1 - (2.0 / sumOfWeights) * num2), (den1 - (1.0 / sumOfWeights) * den2));

for (int i = 0; i < n; i++) r += points[i].d * cos(points[i].angle - a);
r /= sumOfWeights;

I then give it the polar representations of the points (-2,1) and (5,1) and it results in an angle of 0 and distance of 1.5, which is incorrect, as the line should have an angle of pi/2 and a distance to the origin of 1, right?

where did the formula posted in your question come from? As far as I can tell, it doesn't seem to be the correct formula for the desired line. But do you know where it came from? I'm interested in such a formula. — wevrem, Jan 23 '23 at 05:08

Damien · Accepted Answer · 2018-11-06T13:08:42.653

1

I can't see any issue in your code.

In your example, the line you are looking for has equation y = 1.

In my understanding, this correspond to an angle equal to 0, not pi/2. You indicated that an angle of 0 is what your program provides.

Pi/2 is the angle of the nearest point of the line to the origin.

However, there is an issue for the distance calculation r. In the following, I will use complex notation to facilitate the analysis: $$z_i = \rho_i \exp{j\theta_i}$.$ .

For $w_i = 1$ , your distance calculation corresponds to:

$r = \frac{1}{N} \Re(\sum_i z_i e^{-a})$

For a = 0, this corresponds to the real part of the average of the points, 1.5 indeed in your example. It is what your program provides.

A correct value of 1 will be obtained by replacing the real part by the imaginary part, i.e. replacing cos(.) by sin(.).

However, I cannot guarantee that this will work in all the cases. I invite you to check the formulas on internet, or with your supervisor. Sorry not being able to do better for the moment.

Note: complex notations also allows to simplify calculations. Here is an example, even if I understand it is not your priority at the time being:

$2 \sum_i {\sum_j {w_i w_j \rho_i \rho_j cos \theta_i sin \theta_j}} = \Im(\sum_i w_i z_i)^2$

Note: I was not able to have Tex equations displayed correctly in a first step. Thanks to Bob__'s comment, I was able to improve it. Not perfect still ...

edited Nov 06 '18 at 13:08

answered Nov 06 '18 at 09:48

Damien

4,809
4
15
20

1

Please note that SO doesn't provide MathJax to render Tex encoded formulas. You can provide images of those, or follow this answer's workaround: https://stackoverflow.com/a/47798853/4944425 – Bob__ Nov 06 '18 at 10:47
@ Bob__ Thanks for the information and for the link. I will try to insert HTML code ... First time I will play with HTML – Damien Nov 06 '18 at 10:59
I don't think this answer is correct. OP is trying to perform linear regression on a set of points and is aiming to retrieve a representation of a line in polar coordinates. The answer you have provided will find the centroid of the set of points and report this centroid in polar form. These two are not the same and only worked for this specific example due to coincidence. – Wesley Nov 17 '18 at 20:21
@Welsey I based my analysis on the document, the link of which is provided in the question. The formula I gave for `r` is an exact traduction of the formula provided in this document, after simplification thanks to use of complex. It is not exactly the centroid, it is (originally) the real part of the average _after_ correction of the angle `a` previously calculated. What is confusing here is that this angle is equal to 0. Moreover, I indicated that Re(.) should be replaced by Im(.). Anyway, I may indeed have made a mistake. Don't hesitate to comment if you disagree – Damien Nov 17 '18 at 20:42
@Wesley In my understanding, the PO has tested the solution before accepting it... There has been a delay between my answer and acceptance. But I can't be totally sure on this point. I have also tested the solution on a slightly different scenario – Damien Nov 17 '18 at 20:47
Ah, I mistook the $$$a$$$ in the formula as $$$\theta_i$$$ – Wesley Nov 17 '18 at 21:10
@Wesley A key point is that after correction by angle a, the line is parallel to Ox axis. Therefore, the imaginary part of any of its points corresponds to the distance of the line to the origin. – Damien Nov 17 '18 at 21:14

Least Squares Fit on Line in polar representation

1 Answers1