98 lines
3.1 KiB
C++
98 lines
3.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file GncPoseAveragingExample.cpp
|
|
* @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers
|
|
* You can run this example using: ./GncPoseAveragingExample nrInliers nrOutliers
|
|
* e.g.,: ./GncPoseAveragingExample 10 5 (if the numbers are not specified, default
|
|
* values nrInliers = 10 and nrOutliers = 10 are used)
|
|
* @date May 8, 2021
|
|
* @author Luca Carlone
|
|
*/
|
|
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <gtsam/nonlinear/GncOptimizer.h>
|
|
|
|
#include <string>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <random>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
int main(int argc, char** argv){
|
|
cout << "== Robust Pose Averaging Example === " << endl;
|
|
|
|
// default number of inliers and outliers
|
|
size_t nrInliers = 10;
|
|
size_t nrOutliers = 10;
|
|
|
|
// User can pass arbitrary number of inliers and outliers for testing
|
|
if (argc > 1)
|
|
nrInliers = atoi(argv[1]);
|
|
if (argc > 2)
|
|
nrOutliers = atoi(argv[2]);
|
|
cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl;
|
|
|
|
// Seed random number generator
|
|
random_device rd;
|
|
mt19937 rng(rd());
|
|
uniform_real_distribution<double> uniform(-10, 10);
|
|
normal_distribution<double> normalInliers(0.0, 0.05);
|
|
|
|
Values initial;
|
|
initial.insert(0, Pose3()); // identity pose as initialization
|
|
|
|
// create ground truth pose
|
|
Vector6 poseGtVector;
|
|
for(size_t i = 0; i < 6; ++i){
|
|
poseGtVector(i) = uniform(rng);
|
|
}
|
|
Pose3 gtPose = Pose3::Expmap(poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );
|
|
|
|
NonlinearFactorGraph graph;
|
|
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05);
|
|
// create inliers
|
|
for(size_t i=0; i<nrInliers; i++){
|
|
Vector6 poseNoise;
|
|
for(size_t i = 0; i < 6; ++i){
|
|
poseNoise(i) = normalInliers(rng);
|
|
}
|
|
Pose3 poseMeasurement = gtPose.retract(poseNoise);
|
|
graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
|
|
}
|
|
|
|
// create outliers
|
|
for(size_t i=0; i<nrOutliers; i++){
|
|
Vector6 poseNoise;
|
|
for(size_t i = 0; i < 6; ++i){
|
|
poseNoise(i) = uniform(rng);
|
|
}
|
|
Pose3 poseMeasurement = gtPose.retract(poseNoise);
|
|
graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
|
|
}
|
|
|
|
GncParams<LevenbergMarquardtParams> gncParams;
|
|
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
|
|
initial,
|
|
gncParams);
|
|
|
|
Values estimate = gnc.optimize();
|
|
Pose3 poseError = gtPose.between( estimate.at<Pose3>(0) );
|
|
cout << "norm of translation error: " << poseError.translation().norm() <<
|
|
" norm of rotation error: " << poseError.rotation().rpy().norm() << endl;
|
|
// poseError.print("pose error: \n ");
|
|
return 0;
|
|
}
|