我遇到了一个问题:我想计算三个圆圈的交集。据我所知,我应该使用所谓的Trilateration。我用c ++实现了它。 article表示此解决方案有三个限制。
至于第一个“标准”,我无所事事,因为我不使用Z坐标,因为所有三个中心都在z = 0的平面上。 之后,我将点转换为第一个点(P1)的中心位于原点的位置。之后,我将P2(和P3也)点旋转到P2在x轴上的位置。
我的问题是:当所有三个中心都在X轴上时,我试图计算交叉区域的中心(通过使用wiki's article的算法计算),但计算不能完成。另一方面,没有任何限制,说“圆圈的中心不能在一条线上...... 让我们直观地看待我的问题,我只是画了它:
如您所见,三个圆在坐标为(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方向上没有单位向量。
我的问题是:这个算法有第四个限制吗?在我看来,它会被写入文章,所以:你能否为这个问题建议其他解决方案,在这个问题上,当所有点都在同一条线上时,可以计算交点? 提前谢谢!