三边测量（2D）算法实现

Question

我正在尝试在 2D 中实现三边测量过程。与此相关的维基百科文章：Tilateration

我在这个网站上发现了一个很好的问题，其中算法得到了很好的解释：artifical intelligence

毕竟，我尝试用 C++ 实现算法。不幸的是，我遇到了一些问题...... 让我们看看我的实现。它只是一个函数：第一个输入是三个向量，每个向量代表一个具有 X、Y 坐标的 2D 点。其他 (r1,r2,r3) 输入变量代表每个点的距离/半径。

#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h> 
#include <vector>
using namespace std;

std::vector<double> trilateration(double point1[], double point2[], double point3[], double r1, double r2, double r3) {
    std::vector<double> resultPose;
    //unit vector in a direction from point1 to point 2
    double p2p1Distance = pow(pow(point2[0]-point1[0],2) + pow(point2[1]-point1[1],2),0.5);
    double exx = (point2[0]-point1[0])/p2p1Distance;
    double exy = (point2[1]-point1[1])/p2p1Distance;
    //signed magnitude of the x component
    double ix = exx*(point3[0]-point1[0]);
    double iy = exy*(point3[1]-point1[1]);
    //the unit vector in the y direction. 
    double eyx = (point3[0]-point1[0]-ix*exx)/pow(pow(point3[0]-point1[0]-ix*exx,2) + pow(point3[1]-point1[1]-iy*exy,2),0.5);
    double eyy = (point3[1]-point1[1]-iy*exy)/pow(pow(point3[0]-point1[0]-ix*exx,2) + pow(point3[1]-point1[1]-iy*exy,2),0.5);
    //the signed magnitude of the y component
    double jx = eyx*(point3[0]-point1[0]);
    double jy = eyy*(point3[1]-point1[1]);
    //coordinates
    double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
    double y = (pow(r1,2) - pow(r3,2) + pow(iy,2) + pow(jy,2))/2*jy - ix*x/jx;
    //result coordinates
    double finalX = point1[0]+ x*exx + y*eyx;
    double finalY = point1[1]+ x*exy + y*eyy;
    resultPose.push_back(finalX);
    resultPose.push_back(finalY);
    return resultPose;
}

正如我提到的，我关注了this 文章。我认为问题出在计算 y 坐标的部分。我也不确定最后一部分，我计算 finalX，finalY...

我的主要功能如下：

int main(int argc, char* argv[]){
    std::vector<double> finalPose;
    double p1[] = {4.0,4.0};
    double p2[] = {9.0,7.0};
    double p3[] = {9.0,1.0};
    double r1,r2,r3;
    r1 = 4;
    r2 = 3;
    r3 = 3.25;
    finalPose = trilateration(p1,p2,p3,r1,r2,r3);
    cout<<"X:::  "<<finalPose[0]<<endl;
    cout<<"Y:::  "<<finalPose[1]<<endl; 
    //x = 8, y = 4.1

}

结果应该在 X~8 和 Y~4.1 左右，但我得到 X = 13.5542 和 Y=-5.09038

所以我的问题是：我在划分 x 和 y 的计算时遇到问题。我想我可以解决算法直到 x，之后我在计算 y 时遇到问题。

y 的计算如下：y = (r12 - r32 + i2 + j2) / 2j - ix / j

我不知道我应该在这里使用哪个 i 和 j，因为我有两个 i (ix,iy) 和两个 j(jx,jy)。正如你所看到的，我使用了 iy 和 jy，但在行尾我使用了 ix，因为它与 x 相乘。 提前致谢！

Answer 1

我使用了几个辅助变量，但效果很好......

#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h> 
#include <vector>
using namespace std;

struct point 
{
    float x,y;
};

float norm (point p) // get the norm of a vector
{
    return pow(pow(p.x,2)+pow(p.y,2),.5);
}

point trilateration(point point1, point point2, point point3, double r1, double r2, double r3) {
    point resultPose;
    //unit vector in a direction from point1 to point 2
    double p2p1Distance = pow(pow(point2.x-point1.x,2) + pow(point2.y-   point1.y,2),0.5);
    point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
    point aux = {point3.x-point1.x,point3.y-point1.y};
    //signed magnitude of the x component
    double i = ex.x * aux.x + ex.y * aux.y;
    //the unit vector in the y direction. 
    point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
    point ey = { aux2.x / norm (aux2), aux2.y / norm (aux2) };
    //the signed magnitude of the y component
    double j = ey.x * aux.x + ey.y * aux.y;
    //coordinates
    double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
    double y = (pow(r1,2) - pow(r3,2) + pow(i,2) + pow(j,2))/(2*j) - i*x/j;
    //result coordinates
    double finalX = point1.x+ x*ex.x + y*ey.x;
    double finalY = point1.y+ x*ex.y + y*ey.y;
    resultPose.x = finalX;
    resultPose.y = finalY;
    return resultPose;
}

int main(int argc, char* argv[]){
    point finalPose;
    point p1 = {4.0,4.0};
    point p2 = {9.0,7.0};
    point p3 = {9.0,1.0};
    double r1,r2,r3;
    r1 = 4;
    r2 = 3;
    r3 = 3.25;
    finalPose = trilateration(p1,p2,p3,r1,r2,r3);
    cout<<"X:::  "<<finalPose.x<<endl;
    cout<<"Y:::  "<<finalPose.y<<endl; 
}

$ 输出是：

X:::  8.02188
Y:::  4.13021

Answer 2

在linked SO answer 中，i 和 j 的值是标量值并且计算方式与其他向量略有不同，这有点不清楚，也可能是不正确的。更明确地说，你应该有：

i = e_x · (P3 - P1) = e_xx (P3_x - P1_x ) + e_xy (P3_y - P1_y) = ix + iy

j = e_y · (P3 - P1) = e_yx (P3_x - P1_x ) + e_yy (P3_y - P1_y) = jx + jy

注意· 是这里两个向量的点积。因此，在您的代码中应该没有ix、iy、jx 或jy。

另外，在计算y 时，您应该将/2*j 的分母更改为：

 / (2*j)

否则，您将乘以 j 而不是除。进行这些更改后，[7.05, 5.74] 的结果更接近您的预期值。