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

Added relinSkip option to SolverComparer, trapping/printing exceptions, and changed default batch mode to Cholesky

release/4.3a0
Richard Roberts 2013-08-15 20:16:59 +00:00
parent 2321952f0a
commit a0b9dd2724
1 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
examples/SolverComparer.cpp
View File

@ -87,6 +87,7 @@ string datasetName;
int firstStep; int firstStep;
int lastStep; int lastStep;
int nThreads; int nThreads;
int relinSkip;
bool incremental; bool incremental;
bool batch; bool batch;
bool compare; bool compare;
@ -115,6 +116,7 @@ int main(int argc, char *argv[]) {
("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file") ("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file")
("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset") ("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset")
("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors") ("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors")
("relinSkip", po::value<int>(&relinSkip)->default_value(10), "Fluid relinearization check every arg steps")
("incremental", "Run in incremental mode using ISAM2 (default)") ("incremental", "Run in incremental mode using ISAM2 (default)")
("batch", "Run in batch mode, requires an initialization from --read-solution") ("batch", "Run in batch mode, requires an initialization from --read-solution")
("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files") ("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
@ -206,7 +208,10 @@ int main(int argc, char *argv[]) {
/* ************************************************************************* */ /* ************************************************************************* */
void runIncremental() void runIncremental()
{ {
ISAM2 isam2; ISAM2Params params;
params.relinearizeSkip = relinSkip;
params.enablePartialRelinearizationCheck = true;
ISAM2 isam2(params);
// Look for the first measurement to use // Look for the first measurement to use
cout << "Looking for first measurement from step " << firstStep << endl; cout << "Looking for first measurement from step " << firstStep << endl;
@ -334,16 +339,26 @@ void runIncremental()
gttoc_(Collect_measurements); gttoc_(Collect_measurements);
// Update iSAM2 // Update iSAM2
try {
gttic_(Update_ISAM2); gttic_(Update_ISAM2);
isam2.update(newFactors, newVariables); isam2.update(newFactors, newVariables);
gttoc_(Update_ISAM2); gttoc_(Update_ISAM2);
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
if((step - firstPose) % 100 == 0) { if((step - firstPose) % 1000 == 0) {
try {
gttic_(chi2); gttic_(chi2);
Values estimate(isam2.calculateEstimate()); Values estimate(isam2.calculateEstimate());
double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate); double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
cout << "chi2 = " << chi2 << endl; cout << "chi2 = " << chi2 << endl;
gttoc_(chi2); gttoc_(chi2);
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
} }
tictoc_finishedIteration_(); tictoc_finishedIteration_();
@ -418,7 +433,7 @@ void runBatch()
gttic_(Create_optimizer); gttic_(Create_optimizer);
GaussNewtonParams params; GaussNewtonParams params;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
GaussNewtonOptimizer optimizer(measurements, initial, params); GaussNewtonOptimizer optimizer(measurements, initial, params);
gttoc_(Create_optimizer); gttoc_(Create_optimizer);
double lastError; double lastError;