Program Listing for File rotation-only-detector.h

Return to documentation for file (algorithms/geometric-vision-algorithms/include/geometric-vision/rotation-only-detector.h)

#ifndef GEOMETRIC_VISION_ROTATION_ONLY_DETECTOR_H_
#define GEOMETRIC_VISION_ROTATION_ONLY_DETECTOR_H_

#include <vector>

#include <Eigen/Core>
#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 "geometric-vision/relative-non-central-pnp.h"

namespace geometric_vision {

// Inspired by:
// "Detecting and dealing with hovering maneuvers in vision-aided inertial
// navigation systems"
// by Kottas, Dimitrios G. and Wu, Kejian J. and Roumeliotis, Stergios I., IROS
// 2013.
class RotationOnlyDetector {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  RotationOnlyDetector() : motion_classification_threshold_(1.0e-5) {
    is_only_rotated_k_ = true;
  }

  explicit RotationOnlyDetector(double motion_classification_threshold)
      : motion_classification_threshold_(motion_classification_threshold) {
    CHECK_GT(motion_classification_threshold_, 0.0);
    is_only_rotated_k_ = true;
  }

  bool isRotationOnlyMotion(
      const aslam::VisualNFrame::Ptr& nframe_kp1,
      const aslam::VisualNFrame::Ptr& nframe_k,
      const aslam::FrameToFrameMatchesList& matches_kp1_k,
      const aslam::Quaternion& q_Bkp1_Bk, const bool remove_outliers);

  inline double getDeviationFromParallelBearingVectorLastUpdate() const {
    return deviation_from_parallel_bearing_vector_;
  }

  inline double getMotionClassificationThreshold() const {
    return motion_classification_threshold_;
  }

 private:
  void getInlierMatches(
      const aslam::VisualNFrame::Ptr& nframe_kp1,
      const aslam::VisualNFrame::Ptr& nframe_k,
      const aslam::FrameToFrameMatchesList& matches_kp1_k,
      const aslam::Quaternion& q_Ckp1_Ck, const size_t camera_index,
      aslam::FrameToFrameMatches* inlier_matches_kp1_k) const;

  double motion_classification_threshold_;
  double deviation_from_parallel_bearing_vector_;
  bool is_only_rotated_k_;

  static constexpr double kTwoPointRansacThreshold = 0.000005181;
  static constexpr size_t kTwoPointRansacMaxIterations = 100;
};

}  // namespace geometric_vision
#endif  // GEOMETRIC_VISION_ROTATION_ONLY_DETECTOR_H_