estimator.cpp
Go to the documentation of this file.
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


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Nov 28 2013 11:46:23