我正在尝试在二维空间中实现三边测量过程。相关的维基百科文章在这里：三边测量

我在这个网站上找到了一个很好的问题，算法解释得很清楚：人工智能

总之，我尝试用C++实现这个算法。不幸的是，我遇到了一些问题……让我们看看我的实现。这只是一个函数：前三个输入是向量，每个向量代表一个具有X,Y坐标的二维点。其他输入变量（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;    //从点1到点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;    //x分量的符号大小    double ix = exx*(point3[0]-point1[0]);    double iy = exy*(point3[1]-point1[1]);    //y方向上的单位向量     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);    //y分量的符号大小    double jx = eyx*(point3[0]-point1[0]);    double jy = eyy*(point3[1]-point1[1]);    //坐标    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;    //结果坐标    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;}

如我所述，我遵循了这篇文章。我认为问题出在计算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，但在行尾由于与x相乘，我使用了ix。提前感谢！

回答：

在链接的SO回答中，有点不清楚，并且可能不正确的是，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]，这更接近你期望的值。