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