`
ydbc
  • 浏览: 722735 次
  • 性别: 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投影寻踪算法（含代码)

    可以用来做评价 投影寻踪matlab算法 数据建模 综合评价法

    高斯投影算法

    用C++实现的高斯投影和坐标转换的算法,是我花了一段时间的成果

    扫描辐射源的最大似然定位算法

    该算法将各个时差量测分别投影到目标状态空间,通过对目标状态进行修正、迭代和融合实现对扫描速率和目标状态的最大似然估计,同时避免联合估计方法对运动目标的定位精度不足的问题.此外,针对扫描辐射源定位的特点,...

    多媒体传感器网络中被动目标定位算法 (2011年)

    与传统的视觉目标定 位方法不同,该算法无需进行训练或特征识别,而是各个节点基于机器视觉理论,将目标投影为 定位平面上的一条直线,并使用 Hough变换将该直线映射为 Hough空间中的一个离散点,最后 对离散点进行正弦...

    基于对称性投影的大视场小目标三维定位算法

    针对大视场下气瓶阀门的自动化操作,提出一种基于对称性投影的三维定位算法。采用级联分类器对阀门进行初定位与分割,通过随机采样一致性(RANSAC)算法提取阀门平面法向,利用规则刚体结构的对称性,以一种轴向投影与...

    基于单幅图像目标空间定位的算法研究 (2011年)

    在照相机、摄像机内部参数未知的情况下,提出了一种新的由单幅二维投影图像来进行空间定位的方法,即采用目标上共面四线与其在二维图像上投影相匹配的策略,来实现目标的定位。其基本步骤包括:图像预处理;用Prewitt算子...

    论文研究-基于保局投影的离线签名识别.pdf

    为了提高脉冲星空间定位精度和减少计算量，提出了一种新的脉冲星定位算法。该算法先用三颗脉冲星产生脉冲模糊点，再引入新的脉冲星消除脉冲模糊点直到脉冲整周数唯一。然后，采用TDOA定位技术消除航天器时钟的漂移。...

    基于优化的鱼眼镜头成像模型的空间点定位

    对该成像模型进行反投影推导，得到三维点坐标，实现空间目标的精确定位。实验结果表明，当被测物分别处于鱼眼图像中心区域和边缘区域时，该算法的相对定位误差远远小于高阶奇次多项式法和光学折射模型的方法，减小了...

    python2.7栅格数据批量转换投影

    python2.7栅格数据批量转换投影:ProjectRaster_management (in_raster, out_raster, out_coor_system, {resampling_type}, {cell_size}, {geographic_transform}, {Registration_Point}, {in_coor_system}) ...

    论文研究-一种面向扩展空间对象的密度聚类算法.pdf

    在箱号定位阶段，运用基于边缘检测和数学形态学的改进算法，能自适应确定形态学结构元素的大小，将箱号区域连通成一个区域，并能解决集装箱文字纵向排列的问题。字符分割阶段用数学形态学方法消除干扰边缘和噪声，...

    基于2DFLD的手背静脉识别算法

    基于2DFLD的手背静脉识别算法 手背静脉识别技术采用非接触式,为了避免在...库,用2DFLD方法提取静脉特征空间,再将测试图像投影到该特征子空间上,最后用最近邻欧氏距离方法进行匹配。 实验结果表明,该方法识别率达98%。

    论文研究-一类不确定系统的鲁棒自适应控制算法.pdf

    提出了一种圆形容器中液位定位算法。该算法合理运用了边缘检测、灰度投影、Hough变换等方法实现了对液位的粗定位和精定位，并在检测过程中，根据液位线的特点，利用坐标变换将液位线变为标准的抛物线，减少了Hough...

    OpenCV实现人脸识别算法

    PCA方法的基本原理是:利用K-L变换抽取人脸的主要成分，构成特征脸空间，识别时将测试图像投影到此空间，得到一组投影系数，通过与各个人脸图像比较进行识别。 主要包括两个阶段：训练阶段+识别阶段

    简单有效的运动汽车投影阴影分割算法

    利用阴影的光谱属性,同时根据阴影的几何特征及阴影区域内的点和汽车的空间位置、形状等相关特点,提出一种基于小波变换多分辨率特性的阴影分割算法,该算法能有效地分割出阴影与目标之间的分界线。先利用阴影的光谱...

    全景图拼接技术研究-全景图拼接技术研究.part1.rar

    主要有以下四个方面:实现了用于生成统一空间面的柱面投影算法和球面投影算法，解决了全景视觉一致性问题;在图像整合方面，实现了基于特征的图像整合算法，通过特征点的提取和匹配，以一种稳健的选择策略进行过滤，...

    论文研究-一种基于MOBICENTS的多媒体会议系统.pdf

    利用单幅图像结合CAD三维模型实现钣金零件空间方位的2D-3D自动定位。沿CAD三维模型在影像上投影的外轮廓统计影像边缘特征点的数目，根据极值位置确定零件方位的初始参数；提出基于广义点理论的最邻近直线迭代的算法...

    基于圆心真实图像坐标计算的高精度相机标定方法

    基于此,提出将标靶图像逆向投影至空间虚拟矩阵以获得真实圆心像点的迭代标定算法。首先,使用椭圆中心提取方法进行平面相机标定;其次,由标定参数和拍摄图像进行逆向投影获得虚拟物理标靶图像,在近似圆虚拟图像上完成...

    基于改进接收信号强度指示的四面体模型井下定位研究 (2015年)

    针对现有节点定位算法精度和覆盖率低、能耗...将该算法应用于煤矿井下的节点定位，通过与基于投影模型的井下节点定位算法的实验仿真对比表明，该算法具有更高的定位精度和定位覆盖率，更低的节点能耗和网络成本开销。

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