कम से कम वर्ग ध्रुवीय प्रतिनिधित्व में लाइन पर फिट होते हैं

मैं एक रोबोट स्थानीयकरण प्रणाली के आधार पर काम कर रहा हूं, और सच कहा जाए, तो मुझे यकीन नहीं है कि यह गणित है या कार्यान्वयन का मुद्दा है। मैं कम से कम स्क्वायर फिटिंग का उपयोग करके ध्रुवीय निर्देशांक के एक सेट में एक रेखा फिट करने की कोशिश कर रहा हूं। रेखा को ध्रुवीय रूप में दर्शाया गया है। मुझे हमारे पर्यवेक्षक से दो समीकरण दिए गए हैं, एक कोण खोजने के लिए और दूसरा मूल बिंदु की दूरी ज्ञात करने के लिए। (छवि देखें)

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

मैंने सी ++ में समीकरण को लागू करने का प्रयास किया है।

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;

मैं फिर इसे बिंदुओं (-2,1) और (5,1) का ध्रुवीय निरूपण देता हूं और इसके परिणामस्वरूप 0 का कोण और 1.5 की दूरी होती है, जो गलत है, क्योंकि रेखा में pi/2 का कोण होना चाहिए और 1 की उत्पत्ति की दूरी, है ना?