1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file GncPoseAveragingExample.cpp
14 * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers
15 * You can run this example using: ./GncPoseAveragingExample nrInliers nrOutliers
16 * e.g.,: ./GncPoseAveragingExample 10 5 (if the numbers are not specified, default
17 * values nrInliers = 10 and nrOutliers = 10 are used)
18 * @date May 8, 2021
19 * @author Luca Carlone
20 */
21
22#include <gtsam/geometry/Pose3.h>
23#include <gtsam/nonlinear/Values.h>
24#include <gtsam/nonlinear/GncOptimizer.h>
25
26#include <string>
27#include <fstream>
28#include <iostream>
29#include <random>
30
31using namespace std;
32using namespace gtsam;
33
34int main(int argc, char** argv){
35 cout << "== Robust Pose Averaging Example === " << endl;
36
37 // default number of inliers and outliers
38 size_t nrInliers = 10;
39 size_t nrOutliers = 10;
40
41 // User can pass arbitrary number of inliers and outliers for testing
42 if (argc > 1)
43 nrInliers = atoi(nptr: argv[1]);
44 if (argc > 2)
45 nrOutliers = atoi(nptr: argv[2]);
46 cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl;
47
48 // Seed random number generator
49 random_device rd;
50 mt19937 rng(rd());
51 uniform_real_distribution<double> uniform(-10, 10);
52 normal_distribution<double> normalInliers(0.0, 0.05);
53
54 Values initial;
55 initial.insert(j: 0, val: Pose3()); // identity pose as initialization
56
57 // create ground truth pose
58 Vector6 poseGtVector;
59 for(size_t i = 0; i < 6; ++i){
60 poseGtVector(i) = uniform(rng);
61 }
62 Pose3 gtPose = Pose3::Expmap(xi: poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );
63
64 NonlinearFactorGraph graph;
65 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(dim: 6,sigma: 0.05);
66 // create inliers
67 for(size_t i=0; i<nrInliers; i++){
68 Vector6 poseNoise;
69 for(size_t i = 0; i < 6; ++i){
70 poseNoise(i) = normalInliers(rng);
71 }
72 Pose3 poseMeasurement = gtPose.retract(v: poseNoise);
73 graph.add(factorOrContainer: gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
74 }
75
76 // create outliers
77 for(size_t i=0; i<nrOutliers; i++){
78 Vector6 poseNoise;
79 for(size_t i = 0; i < 6; ++i){
80 poseNoise(i) = uniform(rng);
81 }
82 Pose3 poseMeasurement = gtPose.retract(v: poseNoise);
83 graph.add(factorOrContainer: gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
84 }
85
86 GncParams<LevenbergMarquardtParams> gncParams;
87 auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
88 initial,
89 gncParams);
90
91 Values estimate = gnc.optimize();
92 Pose3 poseError = gtPose.between( g: estimate.at<Pose3>(j: 0) );
93 cout << "norm of translation error: " << poseError.translation().norm() <<
94 " norm of rotation error: " << poseError.rotation().rpy().norm() << endl;
95 // poseError.print("pose error: \n ");
96 return 0;
97}
98