GncPoseAveragingExample.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */
 
#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;
}