%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 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 = [.1, 0, 0]'; identity=Pose3; aPb = Pose3(aRb, aTb); cameraA = CalibratedCamera(identity); cameraB = CalibratedCamera(aPb); %% Create 5 points P = { [0, 0, 1]', [-0.1, 0, 1]', [0.1, 0, 1]', [0, 0.5, 0.5]', [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,Unit3(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.atEssentialMatrix(1)