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

done - PGO works like a charm!

release/4.3a0
lcarlone 2020-11-27 23:12:26 -05:00
parent 7699f04820
commit 786d4bbf9a
1 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
tests/testGncOptimizer.cpp
View File

@ -20,6 +20,8 @@
* From Non-Minimal Solvers to Global Outlier Rejection", RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -579,6 +581,42 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
DOUBLES_EQUAL(1.0, finalWeights[2], tol);
}
/* ************************************************************************* */
TEST(GncOptimizer, optimizeSmallPoseGraph) {
/// load small pose graph
const string filename = findExampleDataFile("w100.graph");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph -> addPrior(0, priorMean, priorNoise);
/// get expected values by optimizing outlier-free graph
Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
// add a few outliers
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01));
graph->push_back( BetweenFactor<Pose2>(90 , 50 , Pose2(), betweenNoise) ); // some arbitrary and incorrect between factor
/// get expected values by optimizing outlier-free graph
Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
// as expected, the following test fails due to the presence of an outlier!
// CHECK(assert_equal(expected, expectedWithOutliers, 1e-3));
// GNC
// Note: in difficult instances, we set the odometry measurements to be inliers,
// but this problem is simple enought to succeed even without that assumption
// std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams> gncParams = GncParams<GaussNewtonParams>();
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial, gncParams);
Values actual = gnc.optimize();
// compare
CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers!
}
/* ************************************************************************* */
int main() {
TestResult tr;