%估计姿势负载(“estimatedpose.mat”）;% Loopclosure边缘负载(“loopedge.mat”）;% Groundtruth相机位置负载(“groundtruthlocations.mat”）;

pg3D = poseGraph3D;

len =大小(estimatedPose, 2);信息矩阵= [1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1];在所有连续帧之间插入相对姿态为k = 2:兰当前帧和前一帧之间的相对姿态relativePose = estimatedPose {k - 1} / estimatedPose {k};%用四元数表示的相对方向relativeQuat = tform2quat (relativePose);%相对位姿为[x y qw qx qy qz]relativePose = [tform2trvec (relativePose) relativeQuat);添加姿势到姿势图addRelativePose (pg3D relativePose informationmatrix);结束

%转换姿势从变换到姿势向量。relativeQuat = tform2quat (loopedge);relativePose = [tform2trvec (loopedge) relativeQuat);%循环候选人loopcandidateframeid = 1;%当前帧currentframeid = 100;addRelativePose (pg3D relativePose informationmatrix,．..loopcandidateframeid currentframeid);图显示(pg3D);

位姿图优化optimizedPosegraph = optimizePoseGraph (pg3D);optimizedposes =节点(optimizedPosegraph);%摄像机轨迹图figure estimatedpose = nodes(pg3D);plot3 (estimatedposes (: 1) estimatedposes (:, 2), estimatedposes (:, 3),“r”）;持有在plot3 (groundtruthlocations (: 1) groundtruthlocations (:, 2), groundtruthlocations (:, 3),‘g’）;plot3 (optimizedposes (: 1) optimizedposes (:, 2), optimizedposes (:, 3),“b”）;持有从传奇(“估计姿势图”，“地面真实姿势图”，“优化构成图”）;视图(-20.8,-56.4);