自定义功能函数-matlab

matlab中反复重写的函数。

lee_DrawLine

画线段

%% 说明

%  画2维线段---线方程参数,显示范围

%  param, 3维行向量, 是直线参数[A B C], Ax+By+C=0
%  char, 'x'或'y', 表Range的含义
%  range, 指定x轴或y轴（依据char）的显示范围,[min max]

%% Function

function [] = lee_DrawLine( param, char, range )
switch char
case 'x'
pt1=cross(param,[1;0;-range(1)]);  pt1=pt1./pt1(3);
pt2=cross(param,[1;0;-range(2)]);  pt2=pt2./pt2(3);
case 'y'
pt1=cross(param,[0;1;-range(1)]);  pt1=pt1./pt1(3);
pt2=cross(param,[0;1;-range(2)]);  pt2=pt2./pt2(3);
end
line( [pt1(1), pt2(1)], [pt1(2), pt2(2)]);
end

lee_DrawLine3

画线段

%% 说明

%  画3维线段

%  cen, 3维行向量, 线段起点
%  vec, 3维行向量, 线段方向
%  length 线段长度

%% Function

function [] = lee_DrawLine3( cen, vec, length )
x1 = cen;
x2 = cen + vec * length;
line( [x1(1), x2(1)], [x1(2), x2(2)], [x1(3), x2(3)]);
end

lee_DrawPlane

画平面

%% 说明

%  画3维平面

%  param, 4维行向量, 是平面参数[A B C D], Ax+By+Cz+D=0
%  xRange,yRange 2维行向量, 指定x,y轴的显示范围,[xmin xmax][ymin ymax]
%  step, 显示密度, 默认1
%  a, 透明度, 默认0.2

%% Function

function [] = lee_DrawPlane( param, xRange, yRange, step, a )

if nargin <= 4
a = 0.2;
end
if nargin <= 3
step = 1;
end

x = xRange(1) : step : xRange(2);
y = yRange(1) : step : yRange(2);

[x,y]=meshgrid(x,y);
z=-(param(1)*x+param(2)*y+param(4))/param(3);

surf(x,y,z);
alpha(a);
end

lee_Fill_Line

连接多段线端点

%% 说明
%  inPts是多段线的有序端点, 将其按一定步长填充
%  inPts如果是闭合的, 末端与开端不会被填充连接, 可输入"[A,A(:,1)]"以保证填充完整

%  inPts,多段线的有序端点,行==维数,列==点数
%  step,填充步长

%  outPts,填充后的多段线

%% Function

function outPts = lee_Fill_Line( inPts, step )
outPts = inPts;
i = 1;
while i < size( inPts, 2)
length = norm( inPts( :, i) - inPts( :, i+1) );
num = floor( length / step );
Fstep = ( inPts( :, i+1) - inPts( :, i) )/num;
tPtM = [];
for j = 1: num
tPtM = [ tPtM, inPts( :, i) + Fstep * j];
end
outPts = [ outPts(:,1:i), tPtM, outPts( :,i+1: size( outPts, 2))];
i = i + 1;
end
end

lee_RemoveHomoFactor

去齐次项

%% 说明

%  将inPts的齐次项归1并去除
%  inPts, dim*n矩阵（维数*点数）, xy制, 最后一维是齐次项
%  outPts, (dim-1)*n矩阵, xy制

%% Function

function outPts = lee_RemoveHomoFactor( inPts )
[m,~] = size(inPts);
outPts = inPts;
for i = 1 : m-1
outPts(i,:) = outPts(i,:) ./ outPts(m,:);
end
outPts(m,:) = [];
end

lee_CalcCrossMatrix

算叉乘算子

%% 说明

%  产生一个三维向量的叉乘算子

%%  Function

function crossM = lee_CalcCrossMatrix( e )
crossM = [0 -e(3) e(2); e(3) 0 -e(1); -e(2), e(1), 0];
end

lee_CalcLineParam

算直线参数

%% 说明

%  2点计算直线方程参数

%  pt1,pt2 2维列向量, 直线经过的点
%  lparam, 直线参数[A B C], Ax+By+C=0

%% Function

function lparam = lee_CalcLineParam( pt1, pt2 )
syms A B;
Eq1 = A * pt1(1) + B * pt1(2) + 1;
Eq2 = A * pt2(1) + B * pt2(2) + 1;
[a, b]=solve(Eq1,Eq2,'A','B');
lparam = [double(a), double(b), 1];
end

lee_pt3_from_pts2_F

双目相关

%% 说明

%  依据基础矩阵、双目标定参数将两组不同目下的点集合并为空间点集

%  pts1,pts2 相机1、相机2中点的像素坐标,分别为2*m、2*n矩阵,xy制
%  CamParam, 相关工具箱给出的CameraParameters
%  F, 基础矩阵, 与几何表示一致, 即l2=F*x1

%  Pts, 空间点集, 一般为相机1坐标系下的坐标, 由CamParam决定, 3*k矩阵
%  vec, 空间点集协方差矩阵的特征向量之一, 指代方差最小的方向, 3维列向量
%  e, 对应于vec的特征值, 最小的特征值

%% Function
function [Pts, vec, e] = lee_pt3_from_pts2_F( pts1, pts2, CamParam, F )
[~, m] = size(pts1);
[~, n] = size(pts2);
Pts = [];
for i = 1 : m
l2 = F * [ pts1(:, i); 1];
div = ( l2(1)^2 + l2(2)^2 )^ 0.5;
e = 0.5 * 2^ 0.5 * div;
for j = 1 : n
if abs( l2' * [ pts2(:, j); 1]) < e
t_P = triangulate( pts1(:, i)', pts2(:, j)', CamParam);
Pts = [ Pts, t_P'];
end
end
end
covM = cov(Pts');
[v, e] = eig(covM);
[~, pos] = find( e == min(min( e(e~=0) )) );
vec = v(:, pos(1));
end

lee_H_from_P

双目相关

%% 说明

%  双目系统, 计算两个相机间的单应性H, 相机与输入平面的单应性

%  CamParam, matlab双目标定工具箱输出参数,含CameraParam1,CameraParam2,ROfCam2等
%  PlaneParam,平面在相机1坐标系下的方程4参数[A B C D], Ax+By+Cz+D=0
%  isPixel, 指示生成单应性矩阵的尺寸, 决定其计算出的是空间坐标还是图像坐标,默认为0

%  H,目间单应性, x2 = H * x1 (x1,x2是各目像素坐标,有齐次项,3*？矩阵)
%  H1, 目到平面的单应性, X = H1 * x1
%       (isPixel==0时, X是平面坐标系下的空间坐标,4*？矩阵)
%       (isPixel==1时, X是平面坐标系下的图像坐标,3*？矩阵)
%       (坐标系原点为相机1坐标系z轴与平面的交点,z轴为平面法向量）
%       (x轴 = 平面坐标系z轴-->相机1坐标系z轴, 满足右手准则叉乘而成)
%  H2, 目到平面的单应性, X = H2 * x2

%% Function

function [H, H1, H2] = lee_H_fromP( CamParam, PlaneParam, isPixel)

if nargin <= 2
isPixel = 0;
end

v = PlaneParam(1:3)./PlaneParam(4);

K1 = CamParam.CameraParameters1.IntrinsicMatrix';
K2 = CamParam.CameraParameters2.IntrinsicMatrix';
R2 = CamParam.RotationOfCamera2';
T2 = CamParam.TranslationOfCamera2';
P1 = K1 * [diag([1,1,1]), zeros(3,1)];
P2 = K2 * [R2, T2];

e  = P2 * [0 0 0 1]';
A  = P2* pinv(P1);

H  =  A - e * v / K1;
H1 = [ inv(K1); -v/ K1];
H2 = H1/H;
% isPixel为1时, H1,H2是目图像坐标到平面图像坐标的单应性
if isPixel == 1
TC1 = [0 0 -1/v(3)]';
AxisZ = v;
AxisX = [0 0 1];
if( AxisZ(1)==0 && AxisZ(2)==0 )
AxisX = [1 0 0];
end
AxisX = cross( AxisZ, AxisX);
AxisY = cross( AxisZ, AxisX);
AxisX = AxisX./norm(AxisX);
AxisY = AxisY./norm(AxisY);
AxisZ = AxisZ./norm(AxisZ);
RC1 = [AxisX', AxisY', AxisZ']';
RTC1= [RC1, -RC1*TC1; 0 0 0 1];
RTC1(3,:) = [];

H1 = RTC1* H1;
H2 = RTC1* H2;
end
end

lee_PtsGen_SymAxis

求点集轴对称

%% 说明

%  生成与输入点集关于输入直线轴对称的点集

%  inPts, 2*m矩阵, xy制
%  lparam, 直线参数[A B C], Ax+By+C=0
%  outPts, 2*m矩阵, xy制

%% Function

function outPts = lee_PtsGen_SymAxis( inPts, lparam )
% 平移
lx = - lparam(3)/lparam(1);
inPts(1,:) = inPts(1,:) - lx;
% 旋转
%k = - lparam(2)/lparam(1);

AxisX = [lparam(2), -lparam(1)];
AxisX = AxisX./norm(AxisX);
R = [AxisX(1), AxisX(2); AxisX(2), -AxisX(1)];
inPts = R * inPts;
% 映射
outPts = inPts;
outPts(2,:) = -outPts(2,:);
% 旋转
outPts = R * outPts;
% 平移
outPts(1,:) = outPts(1,:) + lx;
end

lee_MatModel_Mat

展开一个点云模型

%% 说明

%  绕某轴展开点云数据

%  inModel, 输入点云, 3*m矩阵
%  coor, 展开设置, 3*3矩阵, [中心轴起点 中心轴方向向量 起始方向向量], 右手准则握中心轴, 由起始方向开始展开输入点云
%  scale, outModel的第一行是弧度制, 数值较小, 乘以scale以放大
%        可选参数, 默认为1

%  outModel, 输出点云, 3*m矩阵, 每一列为[角度(弧度制) 半径 高度]'

%% Function

function outModel = lee_MatModel_Mat( inModel, coor, scale )

if nargin <= 2
scale = 1;
end

% set lrθ
Ori   = coor(:,1);
AxisZ = coor(:,2);
AxisX = coor(:,3);
AxisY = cross( AxisZ, AxisX);
AxisX = cross( AxisY, AxisZ);
AxisX = AxisX./norm(AxisX);
AxisY = AxisY./norm(AxisY);
AxisZ = AxisZ./norm(AxisZ);

% xyz --> lrθ
inModel = inModel - repmat( Ori, [ 1, size(inModel,2)]);
inModel = [ AxisX'; AxisY'; AxisZ']* inModel;
[outModel(:,1),outModel(:,2),outModel(:,3)] = cart2pol(inModel(1,:)',inModel(2,:)',inModel(3,:)');
outModel = outModel';
outModel(1,:) = outModel(1,:) .* scale;
end