57 lines
1.6 KiB
Matlab
57 lines
1.6 KiB
Matlab
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
% GTSAM Copyright 2010-2013, Georgia Tech Research Corporation,
|
||
|
|
% Atlanta, Georgia 30332-0415
|
||
|
|
% All Rights Reserved
|
||
|
|
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||
|
|
%
|
||
|
|
% See LICENSE for the license information
|
||
|
|
%
|
||
|
|
% @brief Monocular VO Example with 5 landmarks and two cameras
|
||
|
|
% @author Frank Dellaert
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
%% import
|
||
|
|
import gtsam.*
|
||
|
|
|
||
|
|
%% Create two cameras and corresponding essential matrix E
|
||
|
|
aRb = Rot3.Yaw(pi/2);
|
||
|
|
aTb = Point3 (0.1, 0, 0);
|
||
|
|
identity=Pose3;
|
||
|
|
aPb = Pose3(aRb, aTb);
|
||
|
|
cameraA = CalibratedCamera(identity);
|
||
|
|
cameraB = CalibratedCamera(aPb);
|
||
|
|
|
||
|
|
%% Create 5 points
|
||
|
|
P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
|
||
|
|
|
||
|
|
%% Project points in both cameras
|
||
|
|
for i=1:5
|
||
|
|
pA{i}= cameraA.project(P{i});
|
||
|
|
pB{i}= cameraB.project(P{i});
|
||
|
|
end
|
||
|
|
|
||
|
|
%% We start with a factor graph and add epipolar constraints to it
|
||
|
|
% Noise sigma is 1cm, assuming metric measurements
|
||
|
|
graph = NonlinearFactorGraph;
|
||
|
|
model = noiseModel.Isotropic.Sigma(1,0.01);
|
||
|
|
for i=1:5
|
||
|
|
graph.add(EssentialMatrixFactor(1, pA{i}, pB{i}, model));
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Create initial estimate
|
||
|
|
initial = Values;
|
||
|
|
trueE = EssentialMatrix(aRb,Sphere2(aTb));
|
||
|
|
initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
|
||
|
|
initial.insert(1, initialE);
|
||
|
|
|
||
|
|
%% Optimize
|
||
|
|
parameters = LevenbergMarquardtParams;
|
||
|
|
% parameters.setVerbosity('ERROR');
|
||
|
|
optimizer = LevenbergMarquardtOptimizer(graph, initial, parameters);
|
||
|
|
result = optimizer.optimize();
|
||
|
|
|
||
|
|
%% Print result (as essentialMatrix) and error
|
||
|
|
error = graph.error(result)
|
||
|
|
E = result.at(1)
|
||
|
|
|