TrajectoryAlignerSim3.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2020, 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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/UnaryMeasurement.h>
 
#include <vector>
 
namespace gtsam {

class GTSAM_EXPORT TrajectoryAlignerSim3 {
 private:
  // Data members.
  ExpressionFactorGraph graph_;
  Values initial_;
  bool use_gnc_optimizer_;
 
 public:
  TrajectoryAlignerSim3(
      const std::vector<UnaryMeasurement<Pose3>> &aTi,
      const std::vector<std::vector<UnaryMeasurement<Pose3>>> &bTi_all,
      const std::vector<Similarity3> &bSa_all = {},
      const bool use_gnc_optimizer = false,
      const std::vector<std::vector<std::pair<Point3, Point3>>>
          &overlapping_points = {},
      const double point3_factor_sigma = 1e-2);

  Values solve() const;

  Marginals marginalize(
      const Values &solution,
      const Ordering::OrderingType ordering_type = Ordering::COLAMD) const;
};
}  // namespace gtsam