39 static cv::Mat
getRTMatrix(
const cv::Mat &_rvec,
const cv::Mat &_tvec) {
42 cv::Mat m = cv::Mat::eye(4, 4, CV_32FC1);
43 cv::Mat R33 = cv::Mat(m, cv::Rect(0, 0, 3, 3));
44 cv::Rodrigues(_rvec, R33);
45 for (
int i = 0; i < 3; i++)
46 m.at<
float>(i, 3) = _tvec.ptr<
float>(0)[i];
54 std::vector <MarkerFiducials> &ellipseFiducials,
55 std::vector <MarkerPose> &markerPoses) {
57 for (
auto &fiducial:idFiducials) {
58 std::cout <<
"FiducialId - i: " << std::endl << fiducial.image_points << std::endl <<
" o: " << std::endl
59 << fiducial.object_points << std::endl;
62 std::vector<MarkerPose> idMarkerPoses;
64 idPoseEstimation.
estimatePose(idFiducials, camera_k, camera_d, idMarkerPoses);
67 std::vector<cv::Point3f> objectPoints;
68 objectPoints.push_back(cv::Point3f(0.055
f, 0.0
f, 0.0
f));
69 objectPoints.push_back(cv::Point3f(0.00
f, 0.055
f, 0.0
f));
70 objectPoints.push_back(cv::Point3f(-0.055
f, 0.0
f, 0.0
f));
71 objectPoints.push_back(cv::Point3f(0.00
f, -0.055
f, 0.0
f));
80 std::vector<MarkerFiducials> combinedFiucials;
82 for (
auto &markerPose:idMarkerPoses) {
85 std::vector<cv::Point2f> imagePoints;
86 cv::projectPoints(objectPoints, markerPose.getRVec(), markerPose.getTVec(), camera_k, camera_d, imagePoints);
88 for (
int i = 0; i < objectPoints.size(); i++) {
89 cv::Point2f ellipseCenter = cv::Point2f(-1.0
f, -1.0
f);
92 cv::Point2f searchCenter = imagePoints[i];
93 for (
auto &ellipseFiducial:ellipseFiducials) {
94 cv::Point2f c = ellipseFiducial.image_points[0];
95 double dist = cv::norm(searchCenter - c);
97 std::cout << dist << std::endl;
103 if (ellipseCenter.x >= 0 && ellipseCenter.y >= 0) {
111 combinedFiucials.push_back(marker);
115 for (
auto &fiducial:combinedFiucials) {
116 std::cout <<
"FiducialCombined - i: " << std::endl << fiducial.image_points << std::endl <<
" o: " << std::endl
117 << fiducial.object_points << std::endl;
121 combinedPoseEstimation.
estimatePose(combinedFiucials, camera_k, camera_d, markerPoses);
void estimatePose(std::vector< MarkerFiducials > &idFiducials, cv::Mat &camera_k, cv::Mat &camera_d, std::vector< MarkerFiducials > &ellipseFiducials, std::vector< MarkerPose > &markerPoses)
static double max_search_pixel_radius
void estimatePose(std::vector< MarkerFiducials > &markers, cv::Mat &camera_k, cv::Mat &camera_d, std::vector< MarkerPose > &markerPoses)
static cv::Mat getRTMatrix(const cv::Mat &_rvec, const cv::Mat &_tvec)
std::vector< cv::Point3f > object_points
std::vector< cv::Point2f > image_points