`
ydbc
  • 浏览: 738108 次
  • 性别: Icon_minigender_1
  • 来自: 大连
文章分类
社区版块
存档分类
最新评论

空间3点投影定位算法

 
阅读更多

空间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");
    }
}


运行效果:

分享到：
| UIbutton UIvew 事件
评论
发表评论

您还没有登录,请您登录后再发表评论

相关推荐

    matalb投影寻踪算法（含代码)

    在反向投影中，数据点被映射回原始空间，以获得更直观的解释。整个过程通常通过迭代优化来实现，例如使用梯度下降法或线性规划等方法。 2. **Matlab实现投影寻踪**： 在Matlab中，我们可以自定义函数来实现投影...

    摄像头定位算法

    摄像头定位算法是计算机视觉领域中的关键技术之一，它主要用于确定摄像头在三维空间中的位置和姿态，以便于进行场景分析、目标追踪、增强现实等应用。在本文中，我们将深入探讨摄像头定位算法的基本概念、常用方法...

    基于实神经网络的空间测向定位算法.pdf

    知识点1：三维空间测向定位算法 该算法解决了传统方法中存在的北偏差、投影变换对定位精度的影响等问题，实现了三维空间测向定位的高精度。 知识点2：实神经网络在三维空间测向定位中的应用 实神经网络代数算法...

    高斯投影算法

    高斯投影算法是一种在地图制图中广泛应用的数学...总之，高斯投影算法在现代地图制图、地理信息系统（GIS）和空间定位等领域扮演着重要角色。理解和掌握其原理及实现，对于提升IT专业人员在这些领域的技能至关重要。

    基于卷积神经网络的的室内机器人视觉定位算法.pdf

    在本文中，通过针孔相机模型，可以将三维空间中的点投影到二维图像上，然后利用深度学习技术对这些图像进行分析，从而推算出深度信息。 对于室内机器人视觉定位来说，获得准确的深度信息至关重要。传统方法依赖于...

    文档CT图像滤波反投影重建算法的研究.docx

    3. 反投影重建算法：这是基本的重建方法，通过反向执行投影过程来重构图像。反投影算法将投影数据转换回原始图像空间，通常包括滤波步骤以去除噪声。 4. 滤波反投影：该方法结合了傅立叶变换和滤波操作，可以有效地...

    基于灰度积分投影和模板方法的人眼定位

    - **高效性**：通过两次积分投影，大大减少了模板匹配所需搜索的空间，提高了算法的整体效率。 - **准确性**：利用模板匹配技术，能够更准确地定位人眼的位置，这对于后续的人脸识别步骤至关重要。 - **鲁棒性**：...

    机器人双目视觉系统的标定与定位算法

    本文提出了一种新的基于双目的定位算法——投影法。该方法利用双目视觉系统提供的立体视觉信息来计算物体的三维坐标。主要步骤如下： 1. **深度信息获取**：通过双目视觉系统获取物体的深度信息。深度信息可以通过...

    行业文档-设计装置-一种交互投影定位方法及交互投影设备.zip

    在文档中，可能会详细介绍一种基于计算机视觉的定位算法，如特征点匹配或结构光扫描。这些方法通过分析场景中的特定标记点或结构信息来确定用户与投影面的位置。此外，可能还会讨论深度相机的应用，如Kinect或...

    基于细节点投影的可撤销指纹模板生成算法.pdf

    3. **细节点投影**：将检测到的细节点投影到一个低维空间，通常使用特征向量表示。这个过程可以是通过计算特定的局部特征，例如邻域内纹路的方向直方图，或者是利用某种编码方法，如Bloom Filter或Locality ...

    基于投影算法的二维条码识别

    ### 基于投影算法的二维条码识别...通过利用投影算法进行条码的定位、预处理和识别，不仅提高了识别的速度和准确性，还增强了系统的鲁棒性和适应性。随着技术的不断发展和完善，预计未来该技术将在更多的领域得到应用。

    基于积分投影的快速人脸定位

    本文介绍了一种基于积分投影的快速人脸定位算法。该算法首先通过积分投影预测人脸可能的位置与范围，进而构建出人脸模板，并在此基础上利用模板匹配的方法完成人脸的精确定位。这种方法不仅能够提高定位速度，还能...

    空间平滑MUSIC算法

    在传统的MUSIC算法中，信号和噪声被分别投影到两个正交子空间上，然后构建一个伪谱，通过最小化谱峰来估计信号源的方向。然而，由于实际环境中的噪声通常是多变且不均匀的，单纯依赖于单个传感器可能会导致估计精度...

    工程科技卫星导航定位算法与程序设计_第课_坐标系算法.pdf

    课程由武汉大学卫星导航定位技术研究中心的刘晖副教授主讲，旨在深入理解单点定位算法，特别是坐标算法的设计与实现。 坐标系统在卫星导航定位中扮演着至关重要的角色。课程首先介绍了坐标系统的概念，它是一种利用...

    基于GPS与图像融合的智能车辆高精度定位算法.pdf

    这一过程涉及到三维空间中的点与二维图像投影的关系，通过求解最小化重投影误差来确定最佳的位姿估计。 实验结果表明，该算法在长达5公里的路段上进行了测试，并在不同天气条件下以及不同的智能车辆平台上运行，...

    基于无人机吊舱单目相机的目标定位算法与C++代码实现.zip

    《基于无人机吊舱单目相机的目标定位算法与C++代码实现》 在现代科技领域，无人机（Unmanned Aerial Vehicles, UAV）的应用越来越广泛，从军事侦察到民用物流，从环境监测到影视拍摄，无人机都扮演着重要的角色。而...

Magicbox
Global site tag (gtag.js) - Google Analytics