Go to the documentation of this file.
32 using namespace gtsam;
34 int main(
int argc,
char** argv){
35 cout <<
"== Robust Pose Averaging Example === " << endl;
38 size_t nrInliers = 10;
39 size_t nrOutliers = 10;
43 nrInliers = atoi(argv[1]);
45 nrOutliers = atoi(argv[2]);
46 cout <<
"nrInliers " << nrInliers <<
" nrOutliers "<< nrOutliers << endl;
51 uniform_real_distribution<double>
uniform(-10, 10);
52 normal_distribution<double> normalInliers(0.0, 0.05);
59 for(
size_t i = 0;
i < 6; ++
i){
67 for(
size_t i=0;
i<nrInliers;
i++){
69 for(
size_t i = 0;
i < 6; ++
i){
70 poseNoise(
i) = normalInliers(
rng);
77 for(
size_t i=0;
i<nrOutliers;
i++){
79 for(
size_t i = 0;
i < 6; ++
i){
91 Values estimate = gnc.optimize();
93 cout <<
"norm of translation error: " << poseError.
translation().norm() <<
94 " norm of rotation error: " << poseError.
rotation().
rpy().norm() << endl;
Class between(const Class &g) const
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
static std::uniform_real_distribution uniform(0.0, 1.0)
Pose2_ Expmap(const Vector3_ &xi)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
int main(int argc, char **argv)
noiseModel::Diagonal::shared_ptr model
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
std::shared_ptr< Isotropic > shared_ptr
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
NonlinearFactorGraph graph
A non-templated config holding any types of Manifold-group elements.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:39