概述
空间3点投影定位算法
本文博客链接:http://blog.csdn.net/jdh99,作者:jdh,转载请注明.
环境：
主机:WIN7
开发环境:Qt
说明：
<<仿GPS的4星定位程序>>(http://blog.csdn.net/jdh99/article/details/7349771)提供了空间4点定位1点的算法.实际中此算法需要4个基站有较大的高度差,如果在同一高度则定位误差很大.实际中,定位基站一般装在同一平面.利用平面投影可以将空间定位转换为平面定位从而避免这个问题.
具体做法:
每个基站具有平面坐标(x,y)以及一个高度h.测到距离d后,对距离进行平面投影处理:d = sqrt(d^2 - h^2).接下来就是平面定位.
测试程序:
界面widget.ui:
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>Widget</class>
<widget class="QWidget" name="Widget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>561</width>
<height>372</height>
</rect>
</property>
<property name="windowTitle">
<string>Widget</string>
</property>
<widget class="QPushButton" name="pushButton">
<property name="geometry">
<rect>
<x>60</x>
<y>270</y>
<width>75</width>
<height>23</height>
</rect>
</property>
<property name="text">
<string>计算</string>
</property>
</widget>
<widget class="QLineEdit" name="p1x">
<property name="geometry">
<rect>
<x>60</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="d1">
<property name="geometry">
<rect>
<x>360</x>
<y>70</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>210</x>
<y>20</y>
<width>181</width>
<height>21</height>
</rect>
</property>
<property name="font">
<font>
<pointsize>16</pointsize>
</font>
</property>
<property name="text">
<string>空间3点投影定位</string>
</property>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>70</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第1点:</string>
</property>
</widget>
<widget class="QLabel" name="label_3">
<property name="geometry">
<rect>
<x>300</x>
<y>70</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLabel" name="label_4">
<property name="geometry">
<rect>
<x>10</x>
<y>110</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第2点:</string>
</property>
</widget>
<widget class="QLabel" name="label_5">
<property name="geometry">
<rect>
<x>300</x>
<y>110</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLabel" name="label_6">
<property name="geometry">
<rect>
<x>10</x>
<y>150</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第3点:</string>
</property>
</widget>
<widget class="QLabel" name="label_7">
<property name="geometry">
<rect>
<x>300</x>
<y>150</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLineEdit" name="p2x">
<property name="geometry">
<rect>
<x>60</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="d2">
<property name="geometry">
<rect>
<x>360</x>
<y>110</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1.414</string>
</property>
</widget>
<widget class="QLineEdit" name="p3x">
<property name="geometry">
<rect>
<x>60</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="d3">
<property name="geometry">
<rect>
<x>360</x>
<y>150</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLabel" name="label_10">
<property name="geometry">
<rect>
<x>310</x>
<y>250</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>定位:</string>
</property>
</widget>
<widget class="QLineEdit" name="outx">
<property name="geometry">
<rect>
<x>350</x>
<y>270</y>
<width>113</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="outy">
<property name="geometry">
<rect>
<x>350</x>
<y>290</y>
<width>113</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_11">
<property name="geometry">
<rect>
<x>320</x>
<y>270</y>
<width>21</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>x:</string>
</property>
</widget>
<widget class="QLabel" name="label_12">
<property name="geometry">
<rect>
<x>320</x>
<y>290</y>
<width>21</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>y:</string>
</property>
</widget>
<widget class="QLineEdit" name="p1y">
<property name="geometry">
<rect>
<x>100</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p1z">
<property name="geometry">
<rect>
<x>140</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p2y">
<property name="geometry">
<rect>
<x>100</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p2z">
<property name="geometry">
<rect>
<x>140</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p3y">
<property name="geometry">
<rect>
<x>100</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p3z">
<property name="geometry">
<rect>
<x>140</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<connections/>
</ui>
矩阵头文件_matrix.h:
#ifndef _MATRIX_H
#define _MATRIX_H
//头文件
#include <stdio.h>
#include <stdlib.h>
//矩阵数据结构
//二维矩阵
struct _Matrix
{
int m;
int n;
float *arr;
};
//矩阵方法
//设置mn
void matrix_set(struct _Matrix *m,int mm,int nn);
//设置m
void matrix_set_m(struct _Matrix *m,int mm);
//设置n
void matrix_set_n(struct _Matrix *m,int nn);
//初始化
void matrix_init(struct _Matrix *m);
//释放
void matrix_free(struct _Matrix *m);
//读取i,j坐标的数据
//失败返回-31415,成功返回值
float matrix_read(struct _Matrix *m,int i,int j);
//写入i,j坐标的数据
//失败返回-1,成功返回1
int matrix_write(struct _Matrix *m,int i,int j,float val);
//矩阵运算
//成功返回1,失败返回-1
int matrix_add(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);
//C = A - B
//成功返回1,失败返回-1
int matrix_subtract(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);
//C = A * B
//成功返回1,失败返回-1
int matrix_multiply(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);
//行列式的值,只能计算2 * 2,3 * 3
//失败返回-31415,成功返回值
float matrix_det(struct _Matrix *A);
//求转置矩阵,B = AT
//成功返回1,失败返回-1
int matrix_transpos(struct _Matrix *A,struct _Matrix *B);
//求逆矩阵,B = A^(-1)
//成功返回1,失败返回-1
int matrix_inverse(struct _Matrix *A,struct _Matrix *B);
//矩阵拷贝:A = B
//成功返回1,失败返回-1
int matrix_copy(struct _Matrix *A,struct _Matrix *B);
//求方程根,A * X = B,
//成功返回1,答案保存在C中,失败返回-1
//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1
int matrix_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);
//利用克莱姆法则求方程根,A * X = B,
//成功返回1,答案保存在C中,失败返回-1
//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1
int matrix_det_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);
//定位函数
//PI:输入3点坐标,格式:是3 * 2维
//D:4点距离未知点距离数组
//PO:输出坐标
//成功返回1,失败返回-1
int locate(struct _Matrix *PI,float *D,float *PO);
#endif
矩阵函数_matrix.cpp:
#include "_matrix.h"
//矩阵方法
//设置mn
void matrix_set(struct _Matrix *m,int mm,int nn)
{
m->m = mm;
m->n = nn;
}
//设置m
void matrix_set_m(struct _Matrix *m,int mm)
{
m->m = mm;
}
//设置n
void matrix_set_n(struct _Matrix *m,int nn)
{
m->n = nn;
}
//初始化
void matrix_init(struct _Matrix *m)
{
m->arr = (float *)malloc(m->m * m->n * sizeof(float));
}
//释放
void matrix_free(struct _Matrix *m)
{
free(m->arr);
}
//读取i,j坐标的数据
//失败返回-31415,成功返回值
float matrix_read(struct _Matrix *m,int i,int j)
{
if (i >= m->m || j >= m->n)
{
return -31415;
}
return *(m->arr + i * m->n + j);
}
//写入i,j坐标的数据
//失败返回-1,成功返回1
int matrix_write(struct _Matrix *m,int i,int j,float val)
{
if (i >= m->m || j >= m->n)
{
return -1;
}
*(m->arr + i * m->n + j) = val;
return 1;
}
//矩阵运算
//成功返回1,失败返回-1
int matrix_add(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C)
{
int i = 0;
int j = 0;
//判断是否可以运算
if (A->m != B->m || A->n != B->n ||
A->m != C->m || A->n != C->n)
{
return -1;
}
//运算
for (i = 0;i < C->m;i++)
{
for (j = 0;j < C->n;j++)
{
matrix_write(C,i,j,matrix_read(A,i,j) + matrix_read(B,i,j));
}
}
return 1;
}
//C = A - B
//成功返回1,失败返回-1
int matrix_subtract(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C)
{
int i = 0;
int j = 0;
//判断是否可以运算
if (A->m != B->m || A->n != B->n ||
A->m != C->m || A->n != C->n)
{
return -1;
}
//运算
for (i = 0;i < C->m;i++)
{
for (j = 0;j < C->n;j++)
{
matrix_write(C,i,j,matrix_read(A,i,j) - matrix_read(B,i,j));
}
}
return 1;
}
//C = A * B
//成功返回1,失败返回-1
int matrix_multiply(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C)
{
int i = 0;
int j = 0;
int k = 0;
float temp = 0;
//判断是否可以运算
if (A->m != C->m || B->n != C->n ||
A->n != B->m)
{
return -1;
}
//运算
for (i = 0;i < C->m;i++)
{
for (j = 0;j < C->n;j++)
{
temp = 0;
for (k = 0;k < A->n;k++)
{
temp += matrix_read(A,i,k) * matrix_read(B,k,j);
}
matrix_write(C,i,j,temp);
}
}
return 1;
}
//行列式的值,只能计算2 * 2,3 * 3
//失败返回-31415,成功返回值
float matrix_det(struct _Matrix *A)
{
float value = 0;
//判断是否可以运算
if (A->m != A->n || (A->m != 2 && A->m != 3))
{
return -31415;
}
//运算
if (A->m == 2)
{
value = matrix_read(A,0,0) * matrix_read(A,1,1) - matrix_read(A,0,1) * matrix_read(A,1,0);
}
else
{
value = matrix_read(A,0,0) * matrix_read(A,1,1) * matrix_read(A,2,2) +
matrix_read(A,0,1) * matrix_read(A,1,2) * matrix_read(A,2,0) +
matrix_read(A,0,2) * matrix_read(A,1,0) * matrix_read(A,2,1) -
matrix_read(A,0,0) * matrix_read(A,1,2) * matrix_read(A,2,1) -
matrix_read(A,0,1) * matrix_read(A,1,0) * matrix_read(A,2,2) -
matrix_read(A,0,2) * matrix_read(A,1,1) * matrix_read(A,2,0);
}
return value;
}
//求转置矩阵,B = AT
//成功返回1,失败返回-1
int matrix_transpos(struct _Matrix *A,struct _Matrix *B)
{
int i = 0;
int j = 0;
//判断是否可以运算
if (A->m != B->n || A->n != B->m)
{
return -1;
}
//运算
for (i = 0;i < B->m;i++)
{
for (j = 0;j < B->n;j++)
{
matrix_write(B,i,j,matrix_read(A,j,i));
}
}
return 1;
}
//求逆矩阵,B = A^(-1)
//成功返回1,失败返回-1
int matrix_inverse(struct _Matrix *A,struct _Matrix *B)
{
int i = 0;
int j = 0;
int k = 0;
struct _Matrix m;
float temp = 0;
float b = 0;
//判断是否可以运算
if (A->m != A->n || B->m != B->n || A->m != B->m)
{
return -1;
}
/*
//如果是2维或者3维求行列式判断是否可逆
if (A->m == 2 || A->m == 3)
{
if (det(A) == 0)
{
return -1;
}
}
*/
//增广矩阵m = A | B初始化
matrix_set_m(&m,A->m);
matrix_set_n(&m,2 * A->m);
matrix_init(&m);
for (i = 0;i < m.m;i++)
{
for (j = 0;j < m.n;j++)
{
if (j <= A->n - 1)
{
matrix_write(&m,i,j,matrix_read(A,i,j));
}
else
{
if (i == j - A->n)
{
matrix_write(&m,i,j,1);
}
else
{
matrix_write(&m,i,j,0);
}
}
}
}
//高斯消元
//变换下三角
for (k = 0;k < m.m - 1;k++)
{
//如果坐标为k,k的数为0,则行变换
if (matrix_read(&m,k,k) == 0)
{
for (i = k + 1;i < m.m;i++)
{
if (matrix_read(&m,i,k) != 0)
{
break;
}
}
if (i >= m.m)
{
return -1;
}
else
{
//交换行
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,k,j);
matrix_write(&m,k,j,matrix_read(&m,k + 1,j));
matrix_write(&m,k + 1,j,temp);
}
}
}
//消元
for (i = k + 1;i < m.m;i++)
{
//获得倍数
b = matrix_read(&m,i,k) / matrix_read(&m,k,k);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j);
matrix_write(&m,i,j,temp);
}
}
}
//变换上三角
for (k = m.m - 1;k > 0;k--)
{
//如果坐标为k,k的数为0,则行变换
if (matrix_read(&m,k,k) == 0)
{
for (i = k + 1;i < m.m;i++)
{
if (matrix_read(&m,i,k) != 0)
{
break;
}
}
if (i >= m.m)
{
return -1;
}
else
{
//交换行
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,k,j);
matrix_write(&m,k,j,matrix_read(&m,k + 1,j));
matrix_write(&m,k + 1,j,temp);
}
}
}
//消元
for (i = k - 1;i >= 0;i--)
{
//获得倍数
b = matrix_read(&m,i,k) / matrix_read(&m,k,k);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j);
matrix_write(&m,i,j,temp);
}
}
}
//将左边方阵化为单位矩阵
for (i = 0;i < m.m;i++)
{
if (matrix_read(&m,i,i) != 1)
{
//获得倍数
b = 1 / matrix_read(&m,i,i);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) * b;
matrix_write(&m,i,j,temp);
}
}
}
//求得逆矩阵
for (i = 0;i < B->m;i++)
{
for (j = 0;j < B->m;j++)
{
matrix_write(B,i,j,matrix_read(&m,i,j + m.m));
}
}
//释放增广矩阵
matrix_free(&m);
return 1;
}
//矩阵拷贝:A = B
//成功返回1,失败返回-1
int matrix_copy(struct _Matrix *A,struct _Matrix *B)
{
int i = 0;
int j = 0;
if (A->m != B->m || A->n != B->n)
{
return -1;
}
for (i = 0;i < A->m;i++)
{
for (j = 0;j < A->n;j++)
{
matrix_write(B,i,j,matrix_read(A,i,j));
}
}
return 1;
}
//求方程根,A * X = B,
//成功返回1,答案保存在C中,失败返回-1
//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1
int matrix_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C)
{
int i = 0;
int j = 0;
int k = 0;
struct _Matrix m;
float temp = 0;
float b = 0;
//判断是否可以运算
if (A->m != A->n || B->n != 1 || A->m != B->m ||
C->n != 1 || A->m != C->m)
{
return -1;
}
/*
//如果是2维或者3维求行列式判断是否可逆
if (A->m == 2 || A->m == 3)
{
if (det(A) == 0)
{
return -1;
}
}
*/
//增广矩阵m = A | B初始化
matrix_set_m(&m,A->m);
matrix_set_n(&m,A->m + 1);
matrix_init(&m);
for (i = 0;i < m.m;i++)
{
for (j = 0;j < m.n;j++)
{
if (j <= A->n - 1)
{
matrix_write(&m,i,j,matrix_read(A,i,j));
}
else
{
matrix_write(&m,i,j,matrix_read(B,i,0));
}
}
}
//高斯消元
//变换下三角
for (k = 0;k < m.m - 1;k++)
{
//如果坐标为k,k的数为0,则行变换
if (matrix_read(&m,k,k) == 0)
{
for (i = k + 1;i < m.m;i++)
{
if (matrix_read(&m,i,k) != 0)
{
break;
}
}
if (i >= m.m)
{
return -1;
}
else
{
//交换行
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,k,j);
matrix_write(&m,k,j,matrix_read(&m,k + 1,j));
matrix_write(&m,k + 1,j,temp);
}
}
}
//消元
for (i = k + 1;i < m.m;i++)
{
//获得倍数
b = matrix_read(&m,i,k) / matrix_read(&m,k,k);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j);
matrix_write(&m,i,j,temp);
}
}
}
//变换上三角
for (k = m.m - 1;k > 0;k--)
{
//如果坐标为k,k的数为0,则行变换
if (matrix_read(&m,k,k) == 0)
{
for (i = k + 1;i < m.m;i++)
{
if (matrix_read(&m,i,k) != 0)
{
break;
}
}
if (i >= m.m)
{
return -1;
}
else
{
//交换行
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,k,j);
matrix_write(&m,k,j,matrix_read(&m,k + 1,j));
matrix_write(&m,k + 1,j,temp);
}
}
}
//消元
for (i = k - 1;i >= 0;i--)
{
//获得倍数
b = matrix_read(&m,i,k) / matrix_read(&m,k,k);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j);
matrix_write(&m,i,j,temp);
}
}
}
//将左边方阵化为单位矩阵
for (i = 0;i < m.m;i++)
{
if (matrix_read(&m,i,i) != 1)
{
//获得倍数
b = 1 / matrix_read(&m,i,i);
//行变换
for (j = 0;j < m.n;j++)
{
temp = matrix_read(&m,i,j) * b;
matrix_write(&m,i,j,temp);
}
}
}
//求得解
for (i = 0;i < C->m;i++)
{
matrix_write(C,i,0,matrix_read(&m,i,m.n - 1));
}
//释放增广矩阵
matrix_free(&m);
return 1;
}
//利用克莱姆法则求方程根,A * X = B,
//成功返回1,答案保存在C中,失败返回-1
//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1
int matrix_det_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C)
{
struct _Matrix m;
float det_m;
float det_m_temp;
int i = 0;
int j = 0;
//初始化m
matrix_set_m(&m,A->m);
matrix_set_n(&m,A->n);
matrix_init(&m);
//得到A的行列式值
det_m = matrix_det(A);
//判断是否有效
if (det_m == 0)
{
matrix_free(&m);
return -1;
}
for (i = 0;i < 2;i++)
{
//得到新的行列式
matrix_copy(A,&m);
for (j = 0;j < 2;j++)
{
matrix_write(&m,j,i,matrix_read(B,j,0));
}
det_m_temp = matrix_det(&m);
//求解
matrix_write(C,i,0,det_m_temp / det_m);
}
matrix_free(&m);
return 1;
}
//定位函数
//PI:输入3点坐标,格式:是3 * 2维
//D:3点距离未知点距离数组
//PO:输出坐标
//成功返回1,失败返回-1
int locate(struct _Matrix *PI,float *D,float *PO)
{
int i = 0;
int j = 0;
struct _Matrix A;
struct _Matrix B;
struct _Matrix C;
float temp = 0;
//判断是否可以运算
if (PI->m != 3 || PI->n != 2)
{
return -1;
}
//初始化ABC矩阵
matrix_set_m(&A,2);
matrix_set_n(&A,2);
matrix_init(&A);
matrix_set_m(&B,2);
matrix_set_n(&B,1);
matrix_init(&B);
matrix_set_m(&C,2);
matrix_set_n(&C,1);
matrix_init(&C);
//初始化A矩阵
for (i = 0;i < 2;i++)
{
for (j = 0;j < 2;j++)
{
temp = matrix_read(PI,i + 1,j) - matrix_read(PI,i,j);
matrix_write(&A,i,j,temp);
}
}
//初始化B矩阵
for (i = 0;i < 2;i++)
{
temp = matrix_read(PI,i + 1,0) * matrix_read(PI,i + 1,0);
temp += matrix_read(PI,i + 1,1) * matrix_read(PI,i + 1,1);
temp -= matrix_read(PI,i,0) * matrix_read(PI,i,0);
temp -= matrix_read(PI,i,1) * matrix_read(PI,i,1);
temp -= D[i + 1] * D[i + 1] - D[i] * D[i];
temp /= 2;
matrix_write(&B,i,0,temp);
}
//解方程
//if (matrix_solve(&A,&B,&C) > 0)
if (matrix_det_solve(&A,&B,&C) > 0)
{
PO[0] = matrix_read(&C,0,0);
PO[1] = matrix_read(&C,1,0);
return 1;
}
return -1;
}
widget.h:
#ifndef WIDGET_H
#define WIDGET_H
#include <QWidget>
#include "public.h"
#include "_four_point_locate.h"
#include "_Matrix.h"
namespace Ui {
class Widget;
}
class Widget : public QWidget
{
Q_OBJECT
public:
explicit Widget(QWidget *parent = 0);
~Widget();
private:
Ui::Widget *ui;
_Four_Point_Locate four_point_locate;
_Matrix pi;
float d[3];
float p[2];
private slots:
void on_pushButton_clicked();
};
#endif // WIDGET_H
widget.cpp:
#include "widget.h"
#include "ui_widget.h"
#include "math.h"
Widget::Widget(QWidget *parent) :
QWidget(parent),
ui(new Ui::Widget)
{
ui->setupUi(this);
//初始化矩阵
matrix_set(&pi,3,2);
matrix_init(&pi);
}
Widget::~Widget()
{
delete ui;
}
void Widget::on_pushButton_clicked()
{
bool ok;
float h = 0;
int i = 0;
int j = 0;
float temp = 0;
//距离
d[0] = ui->d1->text().toFloat(&ok);
d[1] = ui->d2->text().toFloat(&ok);
d[2] = ui->d3->text().toFloat(&ok);
//获得坐标
matrix_write(&pi,0,0,ui->p1x->text().toFloat(&ok));
matrix_write(&pi,0,1,ui->p1y->text().toFloat(&ok));
h = ui->p1z->text().toFloat(&ok);
d[0] = sqrt(d[0] * d[0] - h * h);
matrix_write(&pi,1,0,ui->p2x->text().toFloat(&ok));
matrix_write(&pi,1,1,ui->p2y->text().toFloat(&ok));
h = ui->p2z->text().toFloat(&ok);
d[1] = sqrt(d[1] * d[1]- h * h);
matrix_write(&pi,2,0,ui->p3x->text().toFloat(&ok));
matrix_write(&pi,2,1,ui->p3y->text().toFloat(&ok));
h = ui->p3z->text().toFloat(&ok);
d[2] = sqrt(d[2] * d[2] - h * h);
qDebug() << "pi" << matrix_read(&pi,0,0) << matrix_read(&pi,0,1);
qDebug() << "pi" << matrix_read(&pi,1,0) << matrix_read(&pi,1,1);
qDebug() << "pi" << matrix_read(&pi,2,0) << matrix_read(&pi,2,1);
qDebug() << "d:" << d[0] << d[1] << d[2];
if (locate(&pi,d,p) > 0)
{
//成功
ui->outx->setText(QString::number(p[0]));
ui->outy->setText(QString::number(p[1]));
}
else
{
//失败
ui->outx->setText("fail");
ui->outy->setText("fail");
}
}
运行效果:
最后
以上就是无奈巨人为你收集整理的空间3点投影定位算法的全部内容，希望文章能够帮你解决空间3点投影定位算法所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复