12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix TransformCalProjectionFactorExampleISAM.m

release/4.3a0
lvzhaoyang 2014-12-07 00:47:03 -05:00
parent 240a9592b9
commit 9dfd6a10e6
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
View File

@ -96,17 +96,17 @@ for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
initial.insert(i,result.atPose3(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
initial.insert(i,result.atPose3(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
@ -137,10 +137,10 @@ for i=1:20
hold on;
%% plot results
result_camera_transform = result.at(transformKey);
result_camera_transform = result.atPose3(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j),[],0.5);
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -153,12 +153,12 @@ for i=1:20
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
ty = result.atPose3(transformKey).translation().y();
fx = result.atCal3_S2(calibrationKey).fx();
fy = result.atCal3_S2(calibrationKey).fy();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
@ -180,10 +180,10 @@ end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
result.atPose3(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)
result.atCal3_S2(calibrationKey)