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