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

sample for new optimization interface

release/4.3a0
Yong-Dian Jian 2010-10-21 01:51:23 +00:00
parent 07483753e2
commit c4df80df55
2 changed files with 34 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
examples/Pose2SLAMwSPCG.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/linear/SubgraphSolver-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/slam/pose2SLAM.h>
using namespace std;
@ -45,25 +46,28 @@ void generateData() ;
/* ************************************************************************* */
int main(void) {
const bool bNewInterface = true ;
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
// generate measurement and initial configuration
generateData() ;
cout << "Initialize .... " << endl;
SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ;
sharedValue SV(new Values(initial)) ;
SPCGOptimizer optimizer(G, SV, solver) ;
if ( bNewInterface == true ) {
result = optimizeSPCG(*G, initial, NonlinearOptimizationParameters());
}
else {
cout << "Initialize .... " << endl;
SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ;
sharedValue SV(new Values(initial)) ;
SPCGOptimizer optimizer(G, SV, solver) ;
cout << "before optimization, sum of error is " << optimizer.error() << endl;
cout << "Optimize .... " << endl;
NonlinearOptimizationParameters parameter;
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
result = *optimizer2.config() ;
}
cout << "before optimization, sum of error is " << optimizer.error() << endl;
cout << "Optimize .... " << endl;
NonlinearOptimizationParameters parameter;
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
result = *optimizer2.config() ;
result.print("result") ;
return 0 ;

20
nonlinear/NonlinearOptimization-inl.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/linear/Factorization.h>
#include <gtsam/linear/SubgraphSolver-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -60,7 +61,18 @@ namespace gtsam {
*/
template<class G, class T>
T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
throw runtime_error("optimizeSPCG: not implemented");
// initial optimization state is the same in both cases tested
typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer;
typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate));
SPCGOptimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate),
solver);
// Levenberg-Marquardt
SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
return *result.config();
}
/**
@ -75,9 +87,11 @@ namespace gtsam {
case MULTIFRONTAL:
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
case SPCG:
return optimizeSPCG<G,T>(graph, initialEstimate, parameters) ;
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters) ;
break;
}
throw runtime_error("optimizeSPCG: undefined solver");
throw runtime_error("optimize: undefined solver");
}
} //namespace gtsam