在同一行上的3个圆的交点 - Trilateration

时间:2015-05-06 15:42:24

标签: c++ geolocation trilateration

我遇到了一个问题:我想计算三个圆圈的交集。据我所知,我应该使用所谓的Trilateration。我用c ++实现了它。 article表示此解决方案有三个限制。

  1. 所有三个中心都在z = 0,
  2. 的平面上
  3. 球体中心P1位于原点
  4. 并且球心P2位于x轴上
  5. 至于第一个“标准”,我无所事事,因为我不使用Z坐标,因为所有三个中心都在z = 0的平面上。 之后,我将点转换为第一个点(P1)的中心位于原点的位置。之后,我将P2(和P3也)点旋转到P2在x轴上的位置。

    我的问题是:当所有三个中心都在X轴上时,我试图计算交叉区域的中心(通过使用wiki's article的算法计算),但计算不能完成。另一方面,没有任何限制,说“圆圈的中心不能在一条线上...... 让我们直观地看待我的问题,我只是画了它:

    intersection of three circles

    如您所见,三个圆在坐标为(4,3)的同一点上相互交叉。因此,存在这些点的解决方案,这些点都在同一条线上(在该示例中,在x轴上)。 圆的参数如下(x,y,半径):

    P1: (0,0,5)
    P2: (4,0,3)
    P3: (8,0,5)
    

    正如我所提到的,我成功实现了Trilateration的算法,但它无法解决这个例子,结果是NaN(非数字)。当然我已经用另一个圆圈(不在同一条线上)尝试了这个算法,它运行正常。

    所以我开始调试我的实现,我发现了问题所在。我的实现如下:

    #include <iostream>
    #include <fstream>
    #include <sstream>
    #include <math.h> 
    #include <vector>
    std::ifstream infile("coordinateList.txt");
    using namespace std;
    
    std::vector<double> trilateration2D(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]);
        double i = ix+iy;
        //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]);
        double j = jx + jy;
        //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[0]+ x*exx + y*eyx;
        double finalY = point1[1]+ x*exy + y*eyy;
        resultPose.push_back(finalX);
        resultPose.push_back(finalY);
        return resultPose;
    }
    int main(int argc, char* argv[]){
        std::vector<double> finalPose;
        double p1[] = {0,0};
        double p2[] = {4,0};
        double p3[] = {8,0};
        double r1,r2,r3;
        r1 = 5;
        r2 =3;
        r3 = 5;
        finalPose = trilateration2D(p1,p2,p3,r1,r2,r3);
        cout<<"X:::  "<<finalPose[0]<<endl;
        cout<<"Y:::  "<<finalPose[1]<<endl;
    
    }
    

    当程序计算 eyx eyy 时,除法中有零,不用说,除零不是“允许”。 eyx eyy 是y方向上的单位向量,但是在这样的圆圈定位中,y方向上没有单位向量。

    我的问题是:这个算法有第四个限制吗?在我看来,它会被写入文章,所以:你能否为这个问题建议其他解决方案,在这个问题上,当所有点都在同一条线上时,可以计算交点? 提前谢谢!

0 个答案:

没有答案