orb_slam2点云数据处理matlab实现--平面拟合、求坐标旋转矩阵、坐标旋转、点云截取

%输入文件：cam.txt                  map.txt
%输出文件：trans_camdata.txt trans_mapdata.txt trans_mapdata_cut.txt      



%------平面拟合------------------------------------------------------------
%导入相机轨迹点云数据
camdata=load('cam.txt');

%求相机轨迹点云的中心（camx0,camy0,camz0）
camx=camdata(:,1);
camy=camdata(:,2);
camz=camdata(:,3);
camx0=mean(camx);%求camx均值
camy0=mean(camy);%求camy均值
camz0=mean(camz);%求camz均值

%拟合平面运算
%Ax+By+Cz=1
%p=[A;B;C]
[p,~,rp]=regress(ones(size(camdata,1),1),camdata);



%-----求坐标旋转矩阵-------------------------------------------------------
co_theta_z=p(1,1)/((p(1,1)^2+p(2,1)^2)^0.5);
si_theta_z=p(2,1)/((p(1,1)^2+p(2,1)^2)^0.5);
co_theta_y=p(3,1)/((p(1,1)^2+p(2,1)^2+p(3,1)^2)^0.5);
si_theta_y=((p(1,1)^2+p(2,1)^2)^0.5)/((p(1,1)^2+p(2,1)^2+p(3,1)^2)^0.5);

R_theta_z=[
co_theta_z si_theta_z 0
-si_theta_z co_theta_z 0
0 0 1
];
R_theta_y=[
co_theta_y 0 -si_theta_y
0 1 0
si_theta_y 0 co_theta_y
];
R_trans=R_theta_y*R_theta_z;



%----坐标旋转运算----------------------------------------------------------
%相机轨迹点云坐标旋转
trans_camdata=(R_trans*(camdata'))';
%保存相机轨迹旋转结果
[rcam,ccam]=size(trans_camdata);
fcamid=fopen('trans_camdata.txt','w');
for i=1:rcam
    fprintf(fcamid,'v ');
    for j=1:ccam
        fprintf(fcamid,'%5f ',trans_camdata(i,j));
        if j==ccam  
            fprintf(fcamid,'\r\n');
        end
    end
end
fclose(fcamid);

%特征点云坐标旋转
mapdata=load('map.txt');
trans_mapdata=(R_trans*(mapdata'))';
%保存特征点云旋转结果
[rmap,cmap]=size(trans_mapdata);
fmapid=fopen('trans_mapdata.txt','w');
for i=1:rmap
    fprintf(fmapid,'v ');
    for j=1:cmap
        fprintf(fmapid,'%5f ',trans_mapdata(i,j));
        if j==cmap  
            fprintf(fmapid,'\r\n');
        end
    end
end
fclose(fmapid);



%------------旋转后的特征点云数据处理---------------------------------------
%统计点云坐标值分布范围
zmin=min(trans_mapdata(:,3));
zmax=max(trans_mapdata(:,3));
zequ=mean(trans_mapdata(:,3));
ymin=min(trans_mapdata(:,2));
ymax=max(trans_mapdata(:,2));
yequ=mean(trans_mapdata(:,2));
xmin=min(trans_mapdata(:,1));
xmax=max(trans_mapdata(:,1));
xequ=mean(trans_mapdata(:,1));

%z轴范围截取
[rmap_cut,cmap_cut]=size(trans_mapdata);
fmap_cutid=fopen('trans_mapdata_cut.txt','w');
for i=1:rmap_cut
    %填入z轴截取范围
    if trans_mapdata(i,3)>0
        %trans_mapdata(i,3)=0;
        fprintf(fmap_cutid,'v ');
        for j=1:cmap_cut
            fprintf(fmap_cutid,'%5f ',trans_mapdata(i,j));
            if j==cmap_cut  
            fprintf(fmap_cutid,'\r\n');
            end
        end
    end
end
fclose(fmap_cutid);