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

extended example for robust kernels

release/4.3a0
Luca 2014-10-16 14:39:23 -04:00
parent c10993a690
commit e96ceb2b4f
1 changed files with 37 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
examples/Pose2SLAMExample_g2o.cpp
View File

@ -26,30 +26,53 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) { int main(const int argc, const char *argv[]) {
// Read graph from file string kernelType = "none";
string g2oFile; int maxIterations = 100; // default
if (argc < 2) string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph; NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial; Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile); bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
}
if(kernelType.compare("huber") == 0){
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph; NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params; GaussNewtonParams params;
params.setVerbosity("TERMINATION"); params.setVerbosity("TERMINATION");
if (argc == 4) { if (argc > 3) {
params.maxIterations = atoi(argv[3]); params.maxIterations = maxIterations;
std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl; std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
} }
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
@ -65,7 +88,10 @@ int main(const int argc, const char *argv[]) {
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile); NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl; std::cout << "done! " << std::endl;
} }
return 0; return 0;