Program Listing for File relative-non-central-pnp.h

Return to documentation for file (algorithms/geometric-vision-algorithms/include/geometric-vision/relative-non-central-pnp.h)

#ifndef GEOMETRIC_VISION_RELATIVE_NON_CENTRAL_PNP_H_
#define GEOMETRIC_VISION_RELATIVE_NON_CENTRAL_PNP_H_

#include <utility>
#include <vector>

#include <Eigen/Core>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/cameras/ncamera.h>
#include <aslam/frames/visual-frame.h>
#include <aslam/frames/visual-nframe.h>
#include <aslam/matcher/match.h>
#include <glog/logging.h>
#include <maplab-common/pose_types.h>
#include <opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.hpp>

namespace geometric_vision {
typedef opengv::sac_problems::relative_pose::
    MultiNoncentralRelativePoseSacProblem MultiNonCentralRelativeSacPnp;

class RelativeNonCentralPnp {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  explicit RelativeNonCentralPnp(bool run_nonlinear_refinement)
      : random_seed_(true),
        solver_(MultiNonCentralRelativeSacPnp::Algorithm::SEVENTEENPT),
        minimum_required_matches_(setup(solver_)),
        run_nonlinear_refinement_(run_nonlinear_refinement) {}

  RelativeNonCentralPnp(
      bool run_nonlinear_refinement,
      MultiNonCentralRelativeSacPnp::Algorithm solver, bool random_seed)
      : random_seed_(random_seed),
        solver_(solver),
        minimum_required_matches_(setup(solver_)),
        run_nonlinear_refinement_(run_nonlinear_refinement) {}

  int getMinRequiredMatches() const {
    return minimum_required_matches_;
  }

  bool computePinhole(
      const aslam::VisualNFrame::ConstPtr& nframe_0,
      const aslam::VisualNFrame::ConstPtr& nframe_1,
      const aslam::FrameToFrameMatchesList& matches_0_1, double pixel_sigma,
      size_t ransac_max_iters, pose::Transformation* T_I0_I1,
      size_t* num_inliers) const;

  bool compute(
      const aslam::VisualNFrame::ConstPtr& nframe_0,
      const aslam::VisualNFrame::ConstPtr& nframe_1,
      const aslam::FrameToFrameMatchesList& matches_0_1,
      double ransac_threshold, size_t ransac_max_iters,
      pose::Transformation* T_I0_I1, size_t* num_inliers) const;

 private:
  // TODO(hitimo): Check if these values are actually correct.
  static size_t setup(MultiNonCentralRelativeSacPnp::Algorithm solver) {
    switch (solver) {
      case MultiNonCentralRelativeSacPnp::Algorithm::SIXPT:
        return 6;
        break;
      case MultiNonCentralRelativeSacPnp::Algorithm::GE:
        return 2;
        break;
      case MultiNonCentralRelativeSacPnp::Algorithm::SEVENTEENPT:
        return 17;
        break;
      default:
        LOG(FATAL) << "Unsupported PnP-solver.";
    }
    return 0;
  }
  // Whether to let RANSAC pick a timestamp-based random seed or not. If false,
  // a seed can be set with srand().
  const bool random_seed_;

  // Solve problem with only one camera?
  static constexpr bool kAsCentral = false;

  const MultiNonCentralRelativeSacPnp::Algorithm solver_;

  const int minimum_required_matches_;

  static constexpr double kRansacInlierRatioThreshold = 0.7;

  const bool run_nonlinear_refinement_;
};
}  // namespace geometric_vision

#endif  // GEOMETRIC_VISION_RELATIVE_NON_CENTRAL_PNP_H_