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/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <boost/make_shared.hpp>
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
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/Marginals.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>
namespace visualSLAM {
@ -213,9 +215,7 @@ namespace visualSLAM {
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
Values optimize(const Values& initialEstimate) {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
Values optimize(const Values& initialEstimate, size_t verbosity) const;
/// Return a Marginals object
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
if options.batchInitialization % Do a full optimize for first two poses
fullyOptimized = newFactors.optimize(initialEstimates);
fullyOptimized = newFactors.optimize(initialEstimates,0);
isam.update(newFactors, fullyOptimized);
else
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
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;
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
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
initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% 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 '));
%% Plot results with covariance ellipses