00001 #ifndef VISUAL_POSE_ESTIMATION_POSE_ESTIMATOR_H 00002 #define VISUAL_POSE_ESTIMATION_POSE_ESTIMATOR_H 00003 00004 #include <opencv/cv.h> 00005 #include <image_geometry/pinhole_camera_model.h> 00006 #include <tf/tf.h> 00007 00008 namespace visual_pose_estimation { 00009 00010 class PoseEstimator 00011 { 00012 public: 00013 PoseEstimator(); 00014 00015 PoseEstimator(const cv::Mat& object_points); 00016 00017 const cv::Mat_<cv::Vec3f>& objectModel() const; 00018 00019 void setObjectModel(const cv::Mat& object_points); 00020 00021 bool getPlanar() const; 00022 void setPlanar(bool is_planar); 00023 00024 // Solve without any prior information 00025 tf::Pose solve(const std::vector<cv::Point2f>& image_points, 00026 const image_geometry::PinholeCameraModel& model) const; 00027 00028 // Solve with prior in the camera frame 00029 tf::Pose solveWithPrior(const std::vector<cv::Point2f>& image_points, 00030 const image_geometry::PinholeCameraModel& model, 00031 const tf::Pose& prior) const; 00032 00033 protected: 00034 cv::Mat_<cv::Vec3f> object_points_; 00035 bool use_planar_solve_; 00036 00037 void solveImpl(const std::vector<cv::Point2f>& image_points, 00038 const image_geometry::PinholeCameraModel& model, 00039 tf::Pose& pose, bool have_prior) const; 00040 }; 00041 00042 00043 inline const cv::Mat_<cv::Vec3f>& PoseEstimator::objectModel() const 00044 { 00045 return object_points_; 00046 } 00047 00048 inline bool PoseEstimator::getPlanar() const { return use_planar_solve_; } 00049 inline void PoseEstimator::setPlanar(bool is_planar) { use_planar_solve_ = is_planar; } 00050 00051 } //namespace visual_pose_estimation 00052 00053 #endif