00001 #include "outlet_pose_estimation/estimator.h" 00002 #include <cassert> 00003 00004 namespace outlet_pose_estimation { 00005 00006 visual_pose_estimation::PoseEstimator createOutletEstimator(const outlet_template_t& _template) 00007 { 00008 std::vector<cv::Point3f> pts; 00009 _template.get_holes_3d(pts); 00010 assert(pts.size() == 6); 00011 00012 cv::Mat_<cv::Vec3f> hole_points(pts.size(), 1); 00013 cv::Point3f origin = pts[5]; // top ground hole 00014 for (size_t i = 0; i < pts.size(); ++i) { 00015 // Translate each point so the top ground hole is the origin 00016 cv::Point3f pt = pts[i] - origin; 00017 // Rotate into outlet frame and convert meters->mm 00018 hole_points(i, 0) = cv::Vec3f(pt.z * 1e-3f, pt.x * 1e-3f, pt.y * 1e-3f); 00019 } 00020 00021 visual_pose_estimation::PoseEstimator estimator(hole_points); 00022 estimator.setPlanar(true); 00023 00024 return estimator; 00025 } 00026 00027 } //namespace outlet_pose_estimation