MATLAB模拟退火算法优化包围盒（OBB）问题

MATLAB模拟退火算法优化包围盒（OBB）问题

一、实现效果

注意：本文只涉及OBB坐标计算部分，图形显示由OpenGL处理，本文不作介绍

二、算法设计

众所周知，模拟退火算法的核心在于如何选择与更新解空间以及设计成本函数。确定了这几个部分，接下来只需要套用算法框架即可。

1、待优化参数（解空间）的确定

对于OBB问题，有两种参数选择方案：
一是，直接以包围盒三条邻边顶点及原点坐标为参数：

二是，在确定单位立方体的基础上将原点坐标、xyz三轴缩放、xyz三轴旋转作为参数
第一种 优点在于能直接得出最终结果（绝对坐标）不需要进行换算，但是问题在于每次随机更新坐标，都会使得ox、oy、oz向量不再相互垂直，需要添加额外的约束条件，浪费计算力。
第二种 的优点很明显，默认三轴垂直，不再需要考虑该约束条件，但是要求出绝对坐标还需要额外换算。
本文选择第二种方案作为解空间。
下面给出各参数的初始化代码：
init.m

clear all;
%% 模拟退火基本参数初始化
T=80;%初温
Tend=0.0001;%末温
searchCount=100;%单温内搜索次数
q=0.98;

%% 解空间初始化
O=[rand*2-1,rand*2-1,rand*2-1];%原点
    % 三轴缩放
Xzoom=abs(randn(1,1));
Yzoom=abs(randn(1,1));
Zzoom=abs(randn(1,1));
    % 三轴旋转
Xroll=randi(360);
Yroll=randi(360);
Zroll=randi(360);

%% 记录器初始化
log_count=1;
costLog=[];
OLog=[0,0,0];
xLog=[0,0,0];
yLog=[0,0,0];
zLog=[0,0,0];

2、更新解空间

模拟退火要获得较精确的解需要在高温时更新解的范围大，来跳出局部最优解；在低温时更新解的范围小，以找到精确的解。
由此可知，更新解的函数需要与温度有关，代码如下：
getNew.m

% z=[Xzoom,Yzoom,Zzoom]
% r=[Xroll,Yroll,Zroll]
function [o,zoom,roll] = getNew(O,z,r,T)
    % 获取新解
    o=[O(1)+(rand*2-1)*T,O(2)+(rand*2-1)*T,O(3)+(rand*2-1)*T];
    zoom=[abs(z(1)+randn(1,1)*T),abs(z(2)+randn(1,1)*T),abs(z(3)+randn(1,1)*T)];
    roll=[r(1)+(rand*2-1)*T,r(2)+(rand*2-1)*T,r(3)+(rand*2-1)*T];
end

3、成本函数

OBB问题要求包围盒尽可能小，尽可能贴近模型边界，于是选择用体积来表示成本。此外，还有一个重要的约束条件：模型的每个点都不能在包围盒的范围外。

其中f(x)为约束条件函数，当x满足约束条件，则无附加成本；当x不满足条件，则成本无穷大。
设模型中的任意点为P，x为OP在OX轴上的投影与|OX|的比值。若x在（0,1）之间，则表示P点在包围盒内。

其中O、X为包围盒原点和X轴顶点的绝对坐标。
但是我们计算的参数均为变换参数，需要将其转化为绝对坐标才能进行运算，下面是转换代码：
getVertex.m

function [O,vx,vy,vz] = getVertex(o,z,r)
    %变换->点
    vx0=[o(1)+z(1),o(2),o(3)];
    vy0=[o(1),o(2)+z(2),o(3)];
    vz0=[o(1),o(2),o(3)+z(3)];
    rx=[1,0,0 ; 0,cosd(r(1)),-sind(r(1)) ; 0,sind(r(1)),cosd(r(1))];%绕x轴旋转矩阵
    ry=[cosd(r(2)),0,sind(r(2)) ; 0,1,0 ; -sind(r(2)),0,cosd(r(2))];%绕y轴旋转矩阵
    rz=[cosd(r(3)),-sind(r(3)),0 ; sind(r(3)),cosd(r(3)),0 ; 0,0,1];%绕z轴旋转矩阵
    O=o*rx*ry*rz;
    vx=vx0*rx*ry*rz;
    vy=vy0*rx*ry*rz;
    vz=vz0*rx*ry*rz;
end

在获取了顶点坐标以后就可以计算通过上面的公式计算成本函数了，代码如下：（注意：模型点坐标保存在points矩阵中，这里不做展示）
goal.m

function [cost] = goal(o,x,y,z,points)
[row,~]=size(points);
cons=0;
for i= 1:row
    p=points(i,:);%提取第i行数据
    c1=isInLine(o,x,p);
    c2=isInLine(o,y,p);
    c3=isInLine(o,z,p);
    if c1 ~= 0 || c2 ~= 0 || c3 ~= 0 % 判断是否在OBB内部
        cons=1e10;
        break
    end
end
ox=norm(x-o); %取模
oy=norm(y-o);
oz=norm(z-o);
cost=ox*oy*oz+cons;

isInLine.m

%% 判断点是否在OBB内，在则返回0，不在则返回1
function [value] = isInLine(o,n,p)
o_p=p-o;
o_n=n-o;
o_n1=o_n/norm(n-o);
x=(dot(o_p,o_n1))/norm(n-o);
if x > 0 && x <1
    value=0;
else
    value=1;
end
end

三、模拟退火代码

main.m

run init;
[o0,zoom0,roll0]=getNew(O,[Xzoom,Yzoom,Zzoom],[Xroll,Yroll,Zroll],T);
[O0,x0,y0,z0]=getVertex(o0,zoom0,roll0);
cost0=goal(O0,x0,y0,z0,points);
cost_final=cost0;
while T>Tend
    for i=1:searchCount
        [oNew,zoomNew,rollNew] = getNew(o0,zoom0,roll0,T);
        [ONew,xNew,yNew,zNew]=getVertex(oNew,zoomNew,rollNew);
        cost=goal(ONew,xNew,yNew,zNew,points);
        if cost < cost0    % 如果新解成本值小于当前解的成本
            o0 = oNew; % 更新当前解为新解
            zoom0=zoomNew;
            roll0=rollNew;
            cost0 = cost;
        else
            p = exp(-(cost - cost0)/T); % 根据Metropolis准则计算一个概率
            if rand(1) < p 
                o0 = oNew; % 更新当前解为新解
                zoom0=zoomNew;
                roll0=rollNew;
                cost0 = cost;
            end
        end
        if cost<cost_final
            O_final=ONew;
            x_final=xNew;
            y_final=yNew;
            z_final=zNew;
            cost_final=cost;
            % 记录解
            costLog(log_count)=cost;
            OLog=cat(1,OLog,O_final);
            xLog=cat(1,xLog,x_final);
            yLog=cat(1,yLog,y_final);
            zLog=cat(1,zLog,z_final);
            log_count=log_count+1;
        end
    end
    T=T*q;
end