40 static cv::Mat
getRTMatrix(
const cv::Mat &_rvec,
const cv::Mat &_tvec) {
43 cv::Mat m = cv::Mat::eye(4, 4, CV_32FC1);
44 cv::Mat R33 = cv::Mat(m, cv::Rect(0, 0, 3, 3));
45 cv::Rodrigues(_rvec, R33);
46 for (
int i = 0; i < 3; i++)
47 m.at<
float>(i, 3) = _tvec.ptr<
float>(0)[i];
53 std::vector<MarkerPose> &markerPosesOutput) {
54 std::vector<cv::Point3f> object_points;
55 std::vector<cv::Point2f> image_points;
57 std::vector<MarkerFiducials>::iterator it = markerFiducials.begin();
58 while (it != markerFiducials.end()) {
62 if (fiducial.
ids.size() > 0) {
63 int id = fiducial.
ids[0];
68 if (
id == markerDetails.id) {
72 object_point = object_point + cv::Point3f(markerDetails.position);
75 object_points.push_back(object_point);
79 image_points.push_back(image_point);
86 it = markerFiducials.erase(it);
93 if (image_points.size() > 2 && object_points.size() > 2) {
97 cv::solvePnP(object_points, image_points, camera_k, camera_d, rv, tv);
100 rv.convertTo(rvec, CV_32F);
101 tv.convertTo(tvec, CV_32F);
105 if (!pose.rt_matrix.empty())
106 markerPosesOutput.push_back(pose);
std::vector< MarkerDetails > markers
MarkerMapEstimator(MarkerMapDetails details)
static cv::Mat getRTMatrix(const cv::Mat &_rvec, const cv::Mat &_tvec)
void estimatePose(std::vector< MarkerFiducials > &markers, cv::Mat &camera_k, cv::Mat &camera_d, std::vector< MarkerPose > &markerPoses)
std::vector< cv::Point3f > object_points
MarkerMapDetails details_
std::vector< cv::Point2f > image_points