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

Change in DoglegOptimizerImpl function arguments to make ISAM2 compatibility easier

release/4.3a0
Richard Roberts 2013-08-09 21:35:45 +00:00
parent 222bd888ef
commit 6843ee8227
2 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -64,14 +64,20 @@ void DoglegOptimizer::iterate(void) {
if ( params_.isMultifrontal() ) {
GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction());
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, state_.error, dlVerbose);
VectorValues dx_u = bt.optimizeGradientSearch();
VectorValues dx_n = bt.optimize();
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
dx_u, dx_n, bt, graph_, state_.values, state_.error, dlVerbose);
}
else if ( params_.isSequential() ) {
GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction());
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bn, graph_, state_.values, state_.error, dlVerbose);
VectorValues dx_u = bn.optimizeGradientSearch();
VectorValues dx_n = bn.optimize();
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose);
}
else if ( params_.isCG() ) {
throw runtime_error("todo: ");
throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver");
}
else {
throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination");

15
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -100,8 +100,8 @@ struct GTSAM_EXPORT DoglegOptimizerImpl {
*/
template<class M, class F, class VALUES>
static IterationResult Iterate(
double Delta, TrustRegionAdaptationMode mode, const M& Rd,
const F& f, const VALUES& x0, const double f_error, const bool verbose=false);
double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false);
/**
* Compute the dogleg point given a trust region radius \f$ \Delta \f$. The
@ -143,16 +143,9 @@ struct GTSAM_EXPORT DoglegOptimizerImpl {
/* ************************************************************************* */
template<class M, class F, class VALUES>
typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
double Delta, TrustRegionAdaptationMode mode, const M& Rd,
const F& f, const VALUES& x0, const double f_error, const bool verbose)
double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose)
{
// Compute steepest descent and Newton's method points
gttic(optimizeGradientSearch);
VectorValues dx_u = GaussianFactorGraph(Rd).optimizeGradientSearch();
gttoc(optimizeGradientSearch);
gttic(optimize);
VectorValues dx_n = Rd.optimize();
gttoc(optimize);
gttic(M_error);
const double M_error = Rd.error(VectorValues::Zero(dx_u));
gttoc(M_error);