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

Added verbosity flag in optimize. Really, we should have more sophisticated optimize.

release/4.3a0
Frank Dellaert 2012-06-19 15:00:41 +00:00
parent 584c5c11c4
commit 615cfee44c
4 changed files with 23 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/slam/visualSLAM.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/slam/visualSLAM.h> #include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
using boost::make_shared; using boost::make_shared;
@ -120,4 +121,11 @@ namespace visualSLAM {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
LevenbergMarquardtParams params;
params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
return optimizer.optimize();
}
/* ************************************************************************* */
} }

16
gtsam/slam/visualSLAM.h
View File

@ -20,14 +20,16 @@
#pragma once #pragma once
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/RangeFactor.h> #include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearISAM.h> #include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
namespace visualSLAM { namespace visualSLAM {
@ -213,9 +215,7 @@ namespace visualSLAM {
* @param range approximate range to landmark * @param range approximate range to landmark
* @param model uncertainty model of this prior * @param model uncertainty model of this prior
*/ */
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate, size_t verbosity) const;
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
/// Return a Marginals object /// Return a Marginals object
Marginals marginals(const Values& solution) const { Marginals marginals(const Values& solution) const {

2
matlab/VisualISAMInitialize.m
View File

@ -49,7 +49,7 @@ newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseMode
%% Update ISAM %% Update ISAM
if options.batchInitialization % Do a full optimize for first two poses if options.batchInitialization % Do a full optimize for first two poses
fullyOptimized = newFactors.optimize(initialEstimates); fullyOptimized = newFactors.optimize(initialEstimates,0);
isam.update(newFactors, fullyOptimized); isam.update(newFactors, fullyOptimized);
else else
isam.update(newFactors, initialEstimates); isam.update(newFactors, initialEstimates);

10
matlab/examples/SFMExample.m
View File

@ -49,18 +49,20 @@ graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% Print the graph %% Print the graph
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
%% Initialize cameras and points to ground truth in this example %% Initialize cameras and points close to ground truth in this example
initialEstimate = visualSLAMValues; initialEstimate = visualSLAMValues;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
initialEstimate.insertPose(symbol('x',i), truth.cameras{i}.pose); pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
initialEstimate.insertPose(symbol('x',i), pose_i);
end end
for j=1:size(truth.points,2) for j=1:size(truth.points,2)
initialEstimate.insertPoint(symbol('p',j), truth.points{j}); point_j = truth.points{j}.retract(0.1*randn(3,1));
initialEstimate.insertPoint(symbol('p',j), point_j);
end end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize(initialEstimate,1);
result.print(sprintf('\nFinal result:\n ')); result.print(sprintf('\nFinal result:\n '));
%% Plot results with covariance ellipses %% Plot results with covariance ellipses