GncPoseAveragingExample.cpp
Go to the documentation of this file.
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 
22 #include <gtsam/geometry/Pose3.h>
23 #include <gtsam/nonlinear/Values.h>
25 
26 #include <string>
27 #include <fstream>
28 #include <iostream>
29 #include <random>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 int 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(argv[1]);
44  if (argc > 2)
45  nrOutliers = atoi(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 
55  initial.insert(0, 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(poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );
63 
65  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,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(poseNoise);
73  graph.add(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(poseNoise);
83  graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
84  }
85 
88  initial,
89  gncParams);
90 
91  Values estimate = gnc.optimize();
92  Pose3 poseError = gtPose.between( estimate.at<Pose3>(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 }
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:191
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
gtsam::GncOptimizer
Definition: GncOptimizer.h:44
uniform
static std::uniform_real_distribution uniform(0.0, 1.0)
gtsam::GncParams
Definition: GncParams.h:42
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
main
int main(int argc, char **argv)
Definition: GncPoseAveragingExample.cpp:34
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
GncOptimizer.h
The GncOptimizer class.
initial
Definition: testScenarioRunner.cpp:148
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:23