基于Matlab的三维重建代码

基于Matlab的三维重建代码,可以作为学习！

function graph = bundleAdjustment（graph adjustFocalLength）

% convert from Rt matrix to AngleAxis
nCam=length（graph.frames）;
Mot = zeros（32nCam）;
for camera=1:nCam
    Mot（:1camera） = RotationMatrix2AngleAxis（graph.Mot（:1:3camera））;
    Mot（:2camera） = graph.Mot（:4camera）;
end



Str = graph.Str;
f  = graph.f;


% assume px py=0
px = 0;
py = 0;



residuals = reprojectionResidual（graph.ObsIdxgraph.ObsValpxpyfMotStr）;
fprintf（‘initial error = %f\n‘ 2*sqrt（sum（residuals.^2）/length（residuals）））;

% bundle adjustment using lsqnonlin in Matlab （Levenberg-Marquardt）
options = optimoptions（@lsqnonlin‘Algorithm‘‘levenberg-marquardt‘‘Display‘‘off‘）;

% adjust structure [for homework]
% !!! fill in your code here
%fprintf（‘error = %f\n‘ 2*sqrt（resnorm/length（residuals）））;

% adjust motion [for homework]
% !!! fill in your code here
%fprintf（‘error = %f\n‘ 2*sqrt（resnorm/length（residuals）））;

% adjust motion and structure
[vecresnormresidualsexitflag] = lsqnonlin（@（x） reprojectionResidual（graph.ObsIdxgraph.ObsValpxpyfx） [Mot（:）; Str（:）][][]options）;
[MotStr] = unpackMotStrf（nCamvec）;
fprintf（‘error = %f\n‘ 2*sqrt（resnorm/length（residuals）））;


if exist（‘adjustFocalLength‘‘var‘） && adjustFocalLength
    % adjust focal length motion and structure
    [vecresnormresidualsexitflag] = lsqnonlin（@（x） reprojectionResidual（graph.ObsIdxgraph.ObsValpxpyx） [f; Mot（:）; Str（:）][][]options）;
    [MotStrf] = unpackMotStrf（nCamvec）;
    fprintf（‘error = %f\n‘ resnorm/length（residuals））;
    graph.f = f;
end


%residuals = reprojectionResidual（graph.ObsIdxgraph.ObsValpxpyfMotStr）;
%fprintf（‘final error = %f\n‘ 2*sqrt（sum（residuals.^2）/length（residuals）））;


for camera=1:nCam
    graph.Mot（::camera） = [AngleAxis2RotationMatrix（Mot（:1camera））  Mot（:2camera）];    
end
graph.Str = Str;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-11-15 15:22  SFMedu2\
     文件       24580  2014-11-15 15:22  SFMedu2\.DS_Store
     目录           0  2014-11-15 15:23  __MACOSX\
     目录           0  2014-11-15 15:23  __MACOSX\SFMedu2\
     文件         222  2014-11-15 15:22  __MACOSX\SFMedu2\._.DS_Store
     文件        1856  2014-09-03 18:08  SFMedu2\bundleAdjustment.m
     文件         222  2014-09-03 18:08  __MACOSX\SFMedu2\._bundleAdjustment.m
     目录           0  2014-11-15 15:22  SFMedu2\denseMatch\
     文件       15364  2014-11-15 15:23  SFMedu2\denseMatch\.DS_Store
     目录           0  2014-11-15 15:23  __MACOSX\SFMedu2\denseMatch\
     文件         222  2014-11-15 15:23  __MACOSX\SFMedu2\denseMatch\._.DS_Store
     文件        1899  2014-09-02 22:42  SFMedu2\denseMatch\denseMatch.m
     文件         222  2014-09-02 22:42  __MACOSX\SFMedu2\denseMatch\._denseMatch.m
     目录           0  2014-11-15 15:22  SFMedu2\denseMatch\priority_queue_1.0\
     文件       15364  2014-11-15 15:22  SFMedu2\denseMatch\priority_queue_1.0\.DS_Store
     目录           0  2014-11-15 15:23  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\
     文件         222  2014-11-15 15:22  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._.DS_Store
     文件         104  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\CHANGES
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._CHANGES
     文件        1340  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\license.txt
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._license.txt
     文件       13658  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\MyHeap.h
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._MyHeap.h
     文件       13341  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\MyHeap_old.h
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._MyHeap_old.h
     文件        1760  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\pq_create.cpp
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._pq_create.cpp
     文件        1270  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\pq_create.m
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._pq_create.m
     文件       10444  2012-08-25 15:06  SFMedu2\denseMatch\priority_queue_1.0\pq_create.mexa64
     文件         222  2012-08-25 15:06  __MACOSX\SFMedu2\denseMatch\priority_queue_1.0\._pq_create.mexa64
............此处省略3932个文件信息