C++ 三边测量（2D）算法的实现_C++_Algorithm_Geolocation_Artificial Intelligence_Trilateration

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

c++ algorithm geolocation artificial-intelligence

C++ 三边测量（2D）算法的实现,c++,algorithm,geolocation,artificial-intelligence,trilateration,C++,Algorithm,Geolocation,Artificial Intelligence,Trilateration,我试图在2D中实现三边测量过程。维基百科中与此相关的文章：我在这个网站上发现了一个很好的问题，算法得到了很好的解释：毕竟，我尝试在C++中实现算法。不幸的是我遇到了一些问题。。。让我们看看我的实现。它只是一个函数：第一个输入是三个向量，每个向量表示一个具有X、Y坐标的2D点。其他（r1、r2、r3）输入变量代表每个点的距离/半径 #include <iostream> #include <fstream> #include <sstream> #incl

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

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

毕竟，我尝试在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;
}

#包括
#包括
#包括
#包括
#包括
使用名称空间std；
标准：矢量三边测量（双点1[]，双点2[]，双点3[]，双r1，双r2，双r3）{
std：：载体结果糖；
//从点1到点2方向上的单位矢量
双p2p1Distance=pow（pow（点2[0]-点1[0]，2）+pow（点2[1]-点1[1]，2），0.5）；
double exx=（点2[0]-点1[0]）/p2p1距离；
双exy=（点2[1]-点1[1]）/p2p1距离；
//x分量的符号幅值
双ix=exx*（点3[0]-点1[0]）；
双iy=exy*（点3[1]-点1[1]）；
//y方向上的单位向量。
双eyx=（点3[0]-点1[0]-ix*exx）/pow（pow（点3[0]-点1[0]-ix*exx，2）+pow（点3[1]-点1[1]-iy*exy，2），0.5）；
双eyy=（点3[1]-1[1]-iy*exy）/pow（pow（点3[0]-1[0]-ix*exx，2）+pow（点3[1]-1[1]-iy*exy，2），0.5）；
//y分量的符号大小
双jx=eyx*（点3[0]-点1[0]）；
双jy=eyy*（点3[1]-点1[1]）；
//坐标
双x=（功率（r1,2）-功率（r2,2）+功率（p2p1距离，2））/（2*p2p1距离）；
双y=（pow（r1,2）-pow（r3,2）+pow（iy，2）+pow（jy，2））/2*jy-ix*x/jx；
//结果坐标
双finalX=point1[0]+x*exx+y*eyx；
double finalY=point1[1]+x*exy+y*eyy；
结果：向后推（finalX）；
结果：推后（最终）；
返回结果集；
}

正如我提到的，我遵循了这篇文章。我认为问题在于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

}

intmain（intargc，char*argv[]）{
std：：载体终糖；
双p1[]={4.0,4.0}；
双p2[]={9.0,7.0}；
双p3[]={9.0,1.0}；
双r1，r2，r3；
r1=4；
r2=3；
r3=3.25；
最终结果=三边测量（p1、p2、p3、r1、r2、r3）；
cout这有点不清楚，可能不正确，因为i
和j
的值是标量值，计算方式与其他向量量略有不同。更明确地说，您应该：
i=ex·（P3-P1）=exx（P3x-P1x）+exy（P3y-P1y）=ix+iy
j=ey·（P3-P1）=eyx（P3x-P1x）+eyy（P3y-P1y）=jx+jy
请注意，·
是此处两个向量的点积。因此，在代码中不应有ix
、iy
、jx
或jy

此外，在计算y
时，应将/2*j
的分母更改为：
 / (2*j)

否则，您将乘以j
，而不是除法。进行这些更改将得到[7.05，5.74]的结果
这更接近您的期望值。
这有点不清楚，可能不正确，因为i
和j
的值是标量值，计算方式与其他向量量略有不同。更明确地说，您应该：
i=ex·（P3-P1）=exx（P3x-P1x）+exy（P3y-P1y）=ix+iy
j=ey·（P3-P1）=eyx（P3x-P1x）+eyy（P3y-P1y）=jx+jy
请注意，·
是此处两个向量的点积。因此，在代码中不应有ix
、iy
、jx
或jy

此外，在计算y
时，应将/2*j
的分母更改为：
 / (2*j)

否则，您将乘以j
而不是除法。进行这些更改后，我得到的[7.05，5.74]
结果更接近您的预期值。
我使用了几个辅助变量，但效果很好
#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; 
}

我使用了几个辅助变量，但效果很好
#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; 
}

“有人能帮我吗？”你是否真的陈述了一个具体的问题（除了链接）或除此之外的问题？你完全明白了，很抱歉。我编辑了我的问题…“有人能帮我吗？”你是否真的陈述了一个具体的问题（除了链接）或者除此之外的问题？你完全明白了，很抱歉。我编辑了我的问题…哇，你是个天才！我感谢你的帮助，它起作用了！对我来说，我得到了正确的坐标（8.02,4.13）如果没有问题的话，我想问一个额外的问题。如果我想把它扩展到三维场景，其中球体相互相交，我唯一应该添加的是：z=±sqrt（r12-x2-y2）？以下问题与此相关：。您的建议是什么？提前感谢！对于3D情况，您似乎只需要向每个向量添加一个z分量（exz
，eyz
）然后计算Wikipedia页面上的ez
和z
。然后计算最终坐标将添加术语±z*ez
，其中ez
是一个向量（因此你需要ezx
，ezy
和ezz
，就像其他术语一样）。谢谢！正如我看到的“ez”可以用ex和ey的叉积来计算。我试着这么做，你认为它有什么好处？“ezx=exxeyx”，“ezy=exyeyy”，“ezz=exz*eyz”。在这些步骤之后，最终的Z坐标应该是：点1[2]+x*exz+y*eyz+Z*ezz（其中Z=±sqrt（r12-x2-y2））我说的对吗？顺便说一句，我真的很感激，我很喜欢这个网站，也很喜欢像你这样乐于助人的人！因为就我所知，如果我们想得到一个3D坐标