www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/pointToLine.m

    % Author: Jai Juneja
% Date: 22/03/2013
%
% Find the shortest Euclidean distance from a point to a line.
% Inputs:
%   pt  :   point in space
%   v1  :   first point along line
%   v2  :   second point along line
%           pt, v1 and v2 must all be at least 3x1 vectors
% Outputs:
%   d   :   shortest distance from point pt to line
function d = pointToLine(pt, v1, v2)
    a = v1 - v2;
    b = pt - v2;
    d = norm(cross(a,b)) / norm(a);
end