c++linear-regression least-squares polar-coordinates

Least Squares Fit on Line in polar representation

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.sstatic.net/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?

Solution

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 ...