三点定位算法--用于平面2D定位

项目中用到三边定位，c#中没有MATLAB solve函数来解方程，只好人工去解方程了。

[csharp] view plain copy

class Program
{
static void Main(string[] args)
{
Point p1 = new Point() { X = 0, Y = 2, Distance = Math.Sqrt(5) };
Point p2 = new Point() { X = 2, Y = 2, Distance = Math.Sqrt(5) };
Point p3 = new Point() { X = 1, Y = 0, Distance = Math.Sqrt(0) };
var p= GetPiontByThree(p1, p2, p3);
Console.WriteLine("Point x:{0}",p.X);
Console.WriteLine("Point y:{0}", p.Y);
Console.ReadKey();
}
/// <summary>
/// 三点绝对定位
/// </summary>
private static Point GetPiontByThree(Point p1, Point p2, Point p3)
{
/* Math.Pow(y1-Y)+Math.Pow(X-x1)=Math.Pow(D1)
* Math.Pow(y2-Y)+Math.Pow(X-x2)=Math.Pow(D2)
* Math.Pow(y3-Y)+Math.Pow(X-x3)=Math.Pow(D3)
* 1-3.2-3解得：
* 2 * (p1.X - p3.X)x + 2 * (p1.Y - p3.Y)y = Math.Pow(p1.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p1.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p1.Distance, 2);
* 2 * (p2.X - p3.X)x + 2 * (p2.Y - p3.Y)y = Math.Pow(p2.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p2.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p2.Distance, 2);
* 简化：
* 2Ax+2By=C
* 2Dx+2Ey=F
* 简化：
* x=(BF-EC)/(2BD-2AE)
* y=(AF-DC)/(2AE-2BD)
*/
var A = p1.X - p3.X;
var B = p1.Y - p3.Y;
var C = Math.Pow(p1.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p1.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p1.Distance, 2);
var D = p2.X - p3.X;
var E = p2.Y - p3.Y;
var F = Math.Pow(p2.X, 2) - Math.Pow(p3.X, 2) + Math.Pow(p2.Y, 2) - Math.Pow(p3.Y, 2) + Math.Pow(p3.Distance, 2) - Math.Pow(p2.Distance, 2);
var x = (B * F - E * C) / (2 * B * D - 2 * A * E);
var y = (A * F - D * C) / (2 * A * E - 2 * B * D);
Point P = new Point() { X = x, Y = y, Distance = 0 };
return P;
}
}
public class Point
{
public double X { get; set; }
public double Y { get; set; }
//表示指定点，据此点的距离
public double Distance { get; set; }
}

三点定位算法--用于平面2D定位相关推荐

最小二乘2D三点定位算法
最小二乘(2D)三点定位算法 (pi−p0)T(pi−p0)=ri2(p_i - p_0)^T(p_i - p0) = r_i^2 (pi−p0)T(pi−p0)=ri2 arg⁡min⁡p0 ...
基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量
上回说到,我们回顾了机器视觉的基础知识,然后一顿操作之后得到了相机内参A和畸变系数dist. 现在我们开始定位目标的坐标. 这个过程包括几个子部分: 1. 标记检测 2. 指定世界坐标系, ...
算法工程师_浅谈算法工程师的职业定位与发展
随着大数据和以深度学习为代表的人工智能技术的飞速发展,算法工程师这个职业逐渐成为国内互联网行业的标配.2016年3月,谷歌旗下DeepMind公司的围棋程序"AlphaGo"战胜职 ...
目标检测经典论文——R-CNN论文翻译（纯中文版）：用于精确物体定位和语义分割的丰富特征层次结构（技术报告（第5版））
目标检测经典论文翻译汇总:[翻译汇总] 翻译pdf文件下载:[下载地址] 此版为纯中文版,中英文对照版请稳步:[R-CNN中英文对照版] 用于精确物体定位和语义分割的丰富特征层次结构技术报告(第5版 ...
RANSAC 算法拟合平面
RANSAC 算法拟合平面视觉库为VisionPro 1.输入参数 Distance Threshold:对于拟合算法的每次迭代,测量候选平面与每个3D输入点之间的距离.比这个阈值更远的点被认为是离 ...
图像处理基本算法车牌识别与定位
图像处理基本算法车牌识别与定位标签: 图像处理 2013-02-20 22:35 4416人阅读评论(1) 收藏举报分类: opencv(27) 版权声明:本文为博主原创文章, ...
谷歌地图如何将地球经纬度坐标系统转换成平面2D坐标系统
一.赤道二.本初子午线三.经纬坐标如何将地球经纬度坐标系统转换成平面2D坐标的? 经纬度坐标转bai平面二维du坐标,是通过坐标投影zhi,将经dao纬度的曲面坐zhuan标值,投影变换为平面sh ...
【算法+图像处理】2D卷积与快速卷积算法C语言实现
卷积算法在图像处理中有着广泛的应用,通常使用的去噪算法.边缘增强算法等的实现都使用到了2D卷积算法.这里介绍一下2D卷积算法和快速卷积算法的C语言实现. 卷积定义即步骤: 1)滑动核,使其中心位于 ...
matlab chan算法定位,MATLAB实现基于Chan氏算法的三维TDOA定位
% 功能:基于chan算法的TDOA三维定位 function [zp] = Chan_3(Noise,MS) %基站数目 BSN = 7; %基站位置,每一列为一个基站位置 BS = [0, 2*s ...

三点定位算法--用于平面2D定位

三点定位算法--用于平面2D定位相关推荐

最新文章

热门文章